From 63f016e00f5037b5ab162d5e123af265387076dc Mon Sep 17 00:00:00 2001 From: Tim Korjakow Date: Mon, 10 Jul 2023 22:31:15 +0800 Subject: [PATCH] Add libs --- src/dcaitirobot/.gitignore | 1 - .../embedded/lib/MotorWheel/MotorWheel.cpp | 385 ++++++++++++++++++ .../embedded/lib/MotorWheel/MotorWheel.h | 349 ++++++++++++++++ .../Examples/PIDSample/PIDSample.pde | 31 ++ .../Examples/PIDSample2/PIDSample2.pde | 55 +++ .../Examples/PIDSample3/PIDSample3.pde | 54 +++ .../embedded/lib/PID_Beta6/PID_Beta6.cpp | 309 ++++++++++++++ .../embedded/lib/PID_Beta6/PID_Beta6.h | 113 +++++ .../embedded/lib/PID_Beta6/PID_Beta6.o | Bin 0 -> 17524 bytes .../embedded/lib/PID_Beta6/fuzzy_table.h | 66 +++ .../embedded/lib/PID_Beta6/keywords.txt | 38 ++ .../lib/PinChangeInt/PinChangeInt.cpp | 191 +++++++++ .../embedded/lib/PinChangeInt/PinChangeInt.h | 136 +++++++ .../lib/PinChangeInt/PinChangeIntConfig.h | 20 + src/dcaitirobot/embedded/lib/README | 46 +++ 15 files changed, 1793 insertions(+), 1 deletion(-) create mode 100644 src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp create mode 100644 src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h create mode 100644 src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt create mode 100644 src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp create mode 100644 src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h create mode 100644 src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h create mode 100644 src/dcaitirobot/embedded/lib/README diff --git a/src/dcaitirobot/.gitignore b/src/dcaitirobot/.gitignore index 16ec450..082450c 100644 --- a/src/dcaitirobot/.gitignore +++ b/src/dcaitirobot/.gitignore @@ -3,7 +3,6 @@ logs/ install/ build/ bin/ -lib/ log/ msg_gen/ srv_gen/ diff --git a/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp new file mode 100644 index 0000000..64ec31b --- /dev/null +++ b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp @@ -0,0 +1,385 @@ +#include + +Motor::Motor(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr) + :PID(&speedRPMInput,&speedRPMOutput,&speedRPMDesired,KC,TAUI,TAUD), + pinPWM(_pinPWM),pinDir(_pinDir),isr(_isr) { + debug(); + + isr->pinIRQ=_pinIRQ; + isr->pinIRQB=_pinIRQB; + +/*for maple*/ +#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini) + pinMode(pinPWM,PWM); + pinMode(pinDir,OUTPUT); + pinMode(isr->pinIRQ,INPUT_FLOATING); + +/*for arduino*/ +#else + pinMode(pinPWM,OUTPUT); + pinMode(pinDir,OUTPUT); + pinMode(isr->pinIRQ,INPUT); + +#endif + if(isr->pinIRQB!=PIN_UNDEFINED) { + pinMode(isr->pinIRQB,INPUT); + } + + PIDDisable(); +} + +void Motor::setupInterrupt() { +/*for maple*/ +#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini) + attachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER); // RISING --> CHANGE 201207 + +/*for arduino*/ +#else + if(isr->pinIRQ==2 || isr->pinIRQ==3) attachInterrupt(isr->pinIRQ-2,isr->ISRfunc,TRIGGER); + else { + PCattachInterrupt(isr->pinIRQ,isr->ISRfunc,TRIGGER); // RISING --> CHANGE 201207 + } +//#ifndef _NAMIKI_MOTOR +// // Namiki motor can use 2 times only, because of the distance of the two optical sensors +// // 4 times of pulses, 201208 +// if(isr->pinIRQB==2 || isr->pinIRQB==3) attachInterrupt(isr->pinIRQB-2,isr->ISRfunc,CHANGE); +// else { +// PCattachInterrupt(isr->pinIRQB,isr->ISRfunc,CHANGE); // RISING --> CHANGE 201207 +// } +//#endif +#endif +} + +unsigned char Motor::getPinPWM() const { + debug(); + return pinPWM; +} +unsigned char Motor::getPinDir() const { + debug(); + return pinDir; +} +unsigned char Motor::getPinIRQ() const { + debug(); + return isr->pinIRQ; +} +unsigned char Motor::getPinIRQB() const { + debug(); + return isr->pinIRQB; +} + +unsigned int Motor::runPWM(unsigned int PWM,bool dir,bool saveDir) { + debug(); + speedPWM=PWM; + if(saveDir) desiredDirection=dir; + analogWrite(pinPWM,PWM); + digitalWrite(pinDir,dir); + return PWM; +} +unsigned int Motor::advancePWM(unsigned int PWM) { + debug(); + return runPWM(PWM,DIR_ADVANCE); +} +unsigned int Motor::backoffPWM(unsigned int PWM) { + debug(); + return runPWM(PWM,DIR_BACKOFF); +} +unsigned int Motor::getPWM() const { + debug(); + return speedPWM; +} +bool Motor::setDesiredDir(bool dir) { + debug(); + //runPWM(getPWM(),dir); // error + desiredDirection=dir; + return getDesiredDir(); +} +bool Motor::getDesiredDir() const { + debug(); + return desiredDirection; +} +bool Motor::reverseDesiredDir() { + debug(); + runPWM(getPWM(),!getDesiredDir()); + return getDesiredDir(); +} + +bool Motor::getCurrDir() const { + return isr->currDirection; +} +bool Motor::setCurrDir() { + if(getPinIRQB()!=PIN_UNDEFINED) + return isr->currDirection=digitalRead(isr->pinIRQB); + return false; +} + +/* +int Motor::getAccRPMM() const { + debug(); + return SPEEDPPS2SPEEDRPM(isr->accPPSS); +} + */ +/* +unsigned int Motor::getSpeedRPM() const { + debug(); + return SPEEDPPS2SPEEDRPM(isr->speedPPS); +} + */ +unsigned int Motor::setSpeedRPM(int speedRPM,bool dir) { + debug(); + PIDSetSpeedRPMDesired(speedRPM); + setDesiredDir(dir); + return abs(getSpeedRPM()); +} +// direction sensitive 201208 +int Motor::getSpeedRPM() const { + debug(); + if(getCurrDir()==DIR_ADVANCE) + return SPEEDPPS2SPEEDRPM(isr->speedPPS); + return -SPEEDPPS2SPEEDRPM(isr->speedPPS); +} +int Motor::setSpeedRPM(int speedRPM) { + debug(); + if(speedRPM>=0) return setSpeedRPM(speedRPM,DIR_ADVANCE); + else return setSpeedRPM(abs(speedRPM),DIR_BACKOFF); +} + + +bool Motor::PIDGetStatus() const { + debug(); + return pidCtrl; +} +bool Motor::PIDEnable(float kc,float taui,float taud,unsigned int sampleTime) { + debug(); + setupInterrupt(); + PIDSetup(kc,taui,taud,sampleTime); + return pidCtrl=true; +} +bool Motor::PIDDisable() { + debug(); + + return pidCtrl=false; +} +bool Motor::PIDReset() { + debug(); + if(PIDGetStatus()==false) return false; + PID::Reset(); + return true; +} + +bool Motor::PIDSetup(float kc,float taui,float taud,unsigned int sampleTime) { + debug(); + PID::SetTunings(kc,taui,taud); + PID::SetInputLimits(0,MAX_SPEEDRPM); + PID::SetOutputLimits(0,MAX_SPEEDRPM); + PID::SetSampleTime(sampleTime); + PID::SetMode(AUTO); + return true; +} +unsigned int Motor::PIDSetSpeedRPMDesired(unsigned int speedRPM) { + debug(); + if(speedRPM>MAX_SPEEDRPM) speedRPMDesired=MAX_SPEEDRPM; + else speedRPMDesired=speedRPM; + return PIDGetSpeedRPMDesired(); +} +unsigned int Motor::PIDGetSpeedRPMDesired() const { + debug(); + return speedRPMDesired; +} + +bool Motor::PIDRegulate(bool doRegulate) { + debug(); + if(PIDGetStatus()==false) return false; + if(getPinIRQB()!=PIN_UNDEFINED && getDesiredDir()!=getCurrDir()) { + speedRPMInput=-SPEEDPPS2SPEEDRPM(isr->speedPPS); + } else { + speedRPMInput=SPEEDPPS2SPEEDRPM(isr->speedPPS); + } + + PID::Compute(); + if(doRegulate && PID::JustCalculated()) { + speed2DutyCycle+=speedRPMOutput; + + if(speed2DutyCycle>=MAX_SPEEDRPM) speed2DutyCycle=MAX_SPEEDRPM; + else if(speed2DutyCycle<=-MAX_SPEEDRPM) speed2DutyCycle=-MAX_SPEEDRPM; + if(speed2DutyCycle>=0) { + runPWM(map(speed2DutyCycle,0,MAX_SPEEDRPM,0,MAX_PWM),getDesiredDir(),false); + } else { + runPWM(map(abs(speed2DutyCycle),0,MAX_SPEEDRPM,0,MAX_PWM),!getDesiredDir(),false); + } + return true; + } + return false; +} + +int Motor::getSpeedPPS() const { + return isr->speedPPS; +} +long Motor::getCurrPulse() const { + return isr->pulses; +} +long Motor::setCurrPulse(long _pulse) { + isr->pulses=_pulse; + return getCurrPulse(); +} +long Motor::resetCurrPulse() { + return setCurrPulse(0); +} + +void Motor::delayMS(unsigned int ms,bool debug) { + for(unsigned long endTime=millis()+ms;millis()=SAMPLETIME) delay(SAMPLETIME); + else delay(endTime-millis()); + } +} + +// to reduce the binary for Atmega168 (16KB), Serial output is disabled +void Motor::debugger() const { + debug() + +#ifdef DEBUG + if(!Serial.available()) Serial.begin(Baudrate); +/* + Serial.print("pinPWM -> "); + Serial.println(pinPWM,DEC); + Serial.print("pinDir -> "); + Serial.println(pinDir,DEC); + Serial.print("pinIRQ -> "); + Serial.println(pinIRQ,DEC); + Serial.print("pinIRQB-> "); + Serial.println(pinIRQB,DEC); + */ + + Serial.print("DesiredDir -> "); + Serial.println(desiredDirection); + Serial.print("currDir ->"); + Serial.println(isr->currDirection); + + Serial.print("PWM -> "); + Serial.println(speedPWM,DEC); + Serial.print("Input -> "); + Serial.println(speedRPMInput,DEC); + Serial.print("Output -> "); + Serial.println(speedRPMOutput,DEC); + Serial.print("Desired-> "); + Serial.println(speedRPMDesired,DEC); + +/* + Serial.print("speed2DutyCycle-> "); + Serial.println(speed2DutyCycle); + Serial.print("speedPPS> "); + Serial.println(isr->speedPPS,DEC); + Serial.print("pulses -> "); + Serial.println(isr->pulses,DEC); + */ + +#endif + +} + + +GearedMotor::GearedMotor(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr,unsigned int ratio): + Motor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr),_ratio(ratio) { + debug(); +} +unsigned int GearedMotor::getRatio() const { + return _ratio; +} +unsigned int GearedMotor::setRatio(unsigned int ratio) { + _ratio=ratio; + return getRatio(); +} +/* +float GearedMotor::getGearedAccRPMM() const { + debug(); + return (float)Motor::getAccRPMM()/getRatio(); +} + */ +float GearedMotor::getGearedSpeedRPM() const { + debug(); + //return (float)Motor::getSpeedRPM()/REDUCTION_RATIO; + //return (float)Motor::getSpeedRPM()/_ratio; + return (float)Motor::getSpeedRPM()/getRatio(); +} +float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM,bool dir) { + debug(); + //Motor::setSpeedRPM(abs(gearedSpeedRPM*REDUCTION_RATIO),dir); + Motor::setSpeedRPM(abs(round(gearedSpeedRPM*_ratio)),dir); + return getGearedSpeedRPM(); +} +// direction sensitive, 201208 +float GearedMotor::setGearedSpeedRPM(float gearedSpeedRPM) { + debug(); + //Motor::setSpeedRPM(gearedSpeedRPM*_ratio); + Motor::setSpeedRPM(round(gearedSpeedRPM*_ratio)); + return getGearedSpeedRPM(); +} + +float GearedMotor::getPosition() { + debug(); + int ticks_per_rev = getRatio() * CPR; + return (float)(Motor::getCurrPulse() % ticks_per_rev)/ticks_per_rev; +} + +MotorWheel::MotorWheel(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr, + unsigned int ratio,unsigned int cirMM): + GearedMotor(_pinPWM,_pinDir,_pinIRQ,_pinIRQB,_isr,ratio),_cirMM(cirMM) { + debug(); + +} +unsigned int MotorWheel::getCirMM() const { + return _cirMM; +} +unsigned int MotorWheel::setCirMM(unsigned int cirMM) { + if(cirMM>0) _cirMM=cirMM; + return getCirMM(); +} +// direction sensitive, 201208 +int MotorWheel::getSpeedCMPM() const { + debug(); + //return int(GearedMotor::getGearedSpeedRPM()*CIR); + return int(GearedMotor::getGearedSpeedRPM()*getCirMM()/10); +} +int MotorWheel::setSpeedCMPM(unsigned int cm,bool dir) { + debug(); + //GearedMotor::setGearedSpeedRPM(cm/CIR,dir); + GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM(),dir); + return getSpeedCMPM(); +} +// direction sensitive, 201208 +int MotorWheel::setSpeedCMPM(int cm) { + debug(); + //GearedMotor::setGearedSpeedRPM(cm/CIR,dir); + GearedMotor::setGearedSpeedRPM(cm*10.0/getCirMM()); + return getSpeedCMPM(); +} + +void MotorWheel::setGearedRPM(float rpm, bool dir) { + debug(); + GearedMotor::setGearedSpeedRPM(rpm, dir); + return ; +} +// direction sensitive, 201208 +int MotorWheel::getSpeedMMPS() const { + debug(); + return int(getSpeedCMPM()/6);//(mm/sec)/(cm/min) = 6 +} + +int MotorWheel::setSpeedMMPS(unsigned int mm,bool dir) { + debug(); + setSpeedCMPM(mm*6,dir); + return getSpeedMMPS(); +} +// direction sensitive, 201208 +int MotorWheel::setSpeedMMPS(int mm) { + debug(); + setSpeedCMPM(mm*6); + return getSpeedMMPS(); +} diff --git a/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h new file mode 100644 index 0000000..8a442c5 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h @@ -0,0 +1,349 @@ + +/* +V1.1 201104 +1. motorwheel library version 1.1,compatible with maple. + +V1.1.1 201110 +1. Add Acceleration + +V1.2 201207 +1. Double CPR from 12 to 24, Interrupt Type from RISING to CHANGE. +2. Reduce default sample time from 10ms to 5ms. +3. Compatible with Namiki 22CL-3501PG80:1, by "#define _NAMIKI_MOTOR" before "#include ...". + +V1.3 201209 +1. R2WD::delayMS(), Omni3WD::delayMS(), Omni4WD::delayMS() are re-implemented, more exactly. +2. getSpeedRPM(), setSpeedRPM(), getSpeedMMPS(), setSpeedMMPS() are plug-minus/direction sensitive now. +3. Acceleration is disabled. + +V1.4 201209 Not Released +1. Increase CPR from 24 to 48 for Faulhaber 2342. + +V1.5 201209 Omni4WD is re-implemented, and now return value of Omni4WD::getSpeedMMPS() is correct. + + */ + +/*for maple*/ +#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini) + #include "wirish.h" + #include "./../PID_Beta6/PID_Beta6.h" + #include "ext_interrupts.h" + +/*for arduino*/ +#else + //#include + //#include + #include + #include + +#endif + +/* + + class PID + class Motor + class GearedMotor + class MotorWheel + + */ + +#ifndef MotorWheel_H +#define MotorWheel_H + +#define DIR_ADVANCE HIGH +#define DIR_BACKOFF LOW + +#define PIN_UNDEFINED 255 +#define REF_VOLT 12 +/*for maple*/ +#if defined(BOARD_maple) || defined(BOARD_maple_native) || defined(BOARD_maple_mini) + #define MAX_PWM 3599 + +/*for arduino*/ +#else + #define MAX_PWM 255 + +#endif + +#ifdef _NAMIKI_MOTOR + #define TRIGGER CHANGE + #define CPR 4 // Namiki motor + #define DIR_INVERSE + #define REDUCTION_RATIO 80 +#else + #define TRIGGER CHANGE + #define CPR 24 // Faulhaber motor + #define DIR_INVERSE ! + #define REDUCTION_RATIO 64 +#endif + +#define MAX_SPEEDRPM 8000 +//#define DESIRED_SPEEDRPM 3000 +//#define SPEED_CONSTANT_RPM 713 +//#define PWM2SPEED_RESOLUTION 143 //713 * 0.2 + +#define SEC_PER_MIN 60 +#define MICROS_PER_SEC 1000000 +//#define SPEEDPPS2SPEEDRPM(freq) ((freq)*((SEC_PER_MIN)/(CPR))) //(freq*SEC_PER_MIN/CPR) +#define SPEEDPPS2SPEEDRPM(freq) ((unsigned long)(freq)*(SEC_PER_MIN)/(CPR)) //(freq*SEC_PER_MIN/CPR) +//#define SPEEDPPS2SPEED2VOLT(spd) ((spd)/SPEED_CONSTANT_RPM) +//#define MAX_SPEEDPPS ((MAX_SPEEDRPM*CPR)/SEC_PER_MIN) +//#define MIN_SPEEDPPS 0 + +#define KC 0.31 +#define TAUI 0.02 +#define TAUD 0.00 +#define SAMPLETIME 5 + +#define Baudrate 19200 +/* +#ifndef DEBUG +#define DEBUG +#endif + */ +#ifdef DEBUG +#define debug() { \ + if(!Serial.available()) Serial.begin(Baudrate); \ + Serial.println(__func__);} +#else +#define debug() {} +#endif + + +// Interrupt Type: RISING --> CHANGE 201207 +#define irqISR(y,x) \ + void x(); \ + struct ISRVars y={x}; \ + void x() { \ + static bool first_pulse=true; \ + y.pulseEndMicros=micros(); \ + if(first_pulse==false && y.pulseEndMicros>y.pulseStartMicros) { \ + y.speedPPS=MICROS_PER_SEC/(y.pulseEndMicros-y.pulseStartMicros); \ + /* y.accPPSS=(y.speedPPS-y.lastSpeedPPS)*y.speedPPS; */ \ + } else first_pulse=false; \ + y.pulseStartMicros=y.pulseEndMicros; \ + /* y.lastSpeedPPS=y.speedPPS; */ \ + if(y.pinIRQB!=PIN_UNDEFINED) \ + y.currDirection=DIR_INVERSE(digitalRead(y.pinIRQ)^digitalRead(y.pinIRQB)); \ + y.currDirection==DIR_ADVANCE?++y.pulses:--y.pulses; \ + } + +struct ISRVars { + void (*ISRfunc)(); + //volatile unsigned long pulses; + volatile long pulses; // 201104, direction sensitive + volatile unsigned long pulseStartMicros; + volatile unsigned long pulseEndMicros; + volatile unsigned int speedPPS; + //volatile unsigned int lastSpeedPPS; + //volatile int accPPSS; // acceleration, Pulse Per Sec^2 + volatile bool currDirection; + unsigned char pinIRQB; + unsigned char pinIRQ; // pinIRQA 201207 +}; + +class Motor: public PID { +public: + Motor(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr); + + void setupInterrupt(); + + unsigned char getPinPWM() const; + unsigned char getPinDir() const; + unsigned char getPinIRQ() const; + unsigned char getPinIRQB() const; + + unsigned int runPWM(unsigned int PWM,bool dir,bool saveDir=true); + unsigned int getPWM() const; + unsigned int advancePWM(unsigned int PWM); + unsigned int backoffPWM(unsigned int PWM); + + bool setDesiredDir(bool dir); + bool getDesiredDir() const; + bool reverseDesiredDir(); + + bool setCurrDir(); + bool getCurrDir() const; + + //int getAccRPMM() const; // Acceleration, Round Per Min^2 + //unsigned int getSpeedRPM() const; + //unsigned int setSpeedRPM(int speedRPM,bool dir); + // direction sensitive 201208 + int getSpeedRPM() const; + unsigned int setSpeedRPM(int speedRPM,bool dir); // preserve + int setSpeedRPM(int speedRPM); + + //void simpleRegulate(); + + bool PIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000); + bool PIDGetStatus() const; + bool PIDEnable(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=1000); + bool PIDDisable(); + bool PIDReset(); + bool PIDRegulate(bool doRegulate=true); + unsigned int PIDSetSpeedRPMDesired(unsigned int speedRPM); + unsigned int PIDGetSpeedRPMDesired() const; + + void delayMS(unsigned int ms,bool debug=false); + void debugger() const; + + //int getAccPPSS() const; + int getSpeedPPS() const; + long getCurrPulse() const; + long setCurrPulse(long _pulse); + long resetCurrPulse(); + + struct ISRVars* isr; + +private: + unsigned char pinPWM; + unsigned char pinDir; + //unsigned char pinIRQ; // moved to isr + //unsigned char pinIRQB; + + //bool currDirection; // current direction + bool desiredDirection; // desired direction + unsigned int speedPWM; // current PWM + + int speedRPMInput; // RPM: Round Per Minute + int speedRPMOutput; // RPM + int speedRPMDesired; // RPM + //float PWMEC; + float speed2DutyCycle; +/* + // the followings are defined in struct ISRvars, + // because ISR must be a global function, without parameters and no return value + + volatile unsigned int speedPPS; // PPS: Pulses Per Second + volatile unsigned long pulseStartMicros; + volatile unsigned long pulseEndMicros; + */ + bool pidCtrl; + + Motor(); + +}; + + +#ifndef REDUCTION_RATIO +#define REDUCTION_RATIO 64 +#endif +class GearedMotor: public Motor { // RPM +public: + GearedMotor(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr, + unsigned int _ratio=REDUCTION_RATIO); + //float getGearedAccRPMM() const; // Acceleration, Round Per Min^2 + float getGearedSpeedRPM() const; + float setGearedSpeedRPM(float gearedSpeedRPM,bool dir); + // direction sensitive 201208 + float setGearedSpeedRPM(float gearedSpeedRPM); + unsigned int getRatio() const; + unsigned int setRatio(unsigned int ratio=REDUCTION_RATIO); + float getPosition(); +private: + unsigned int _ratio; +}; + + +#ifndef PI +#define PI 3.1416 +#endif +//#define CIR 31.4 // cm +//#define CIRMM 150 // mm +//#define CIRMM 188 +#define CIRMM 314 +class MotorWheel: public GearedMotor { // +public: + MotorWheel(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr, + unsigned int ratio=REDUCTION_RATIO,unsigned int cirMM=CIRMM); + + unsigned int getCirMM() const; + unsigned int setCirMM(unsigned int cirMM=CIRMM); +/* + int getAccCMPMM() const; // Acceleration, CM Per Min^2 + unsigned int getSpeedCMPM() const; // cm/min + unsigned int setSpeedCMPM(unsigned int cm,bool dir); + int getAccMMPSS() const; // Acceleration, MM Per Sec^2 + unsigned int getSpeedMMPS() const; // mm/s + unsigned int setSpeedMMPS(unsigned int mm,bool dir); + */ + // direction sensitive 201208 + //int getAccCMPMM() const; // Acceleration, CM Per Min^2 + int getSpeedCMPM() const; // cm/min + int setSpeedCMPM(unsigned int cm,bool dir); // preserve + int setSpeedCMPM(int cm); + //int getAccMMPSS() const; // Acceleration, MM Per Sec^2 + int getSpeedMMPS() const; // mm/s + int setSpeedMMPS(unsigned int mm,bool dir); // preserve + int setSpeedMMPS(int mm); + void setGearedRPM(float rpm, bool dir); + + //unsigned int runTime(unsigned int speedMMPS,bool dir,unsigned int TimeMS); + //unsigned int runDistance(unsigned int speedMMPS,bool dir,unsigned int distanceMM); + +private: + unsigned int _cirMM; +}; + +/* +class samePID: public PID { +public: + samePID(int* input,int* output,int* setPoint,float kc,float taui,float taud) + :PID(input,output,setPoint,kc,taui,taud) { } +}; + +class servoMotor: public Motor,public samePID { +public: + servoMotor(unsigned char _pinPWM,unsigned char _pinDir, + unsigned char _pinIRQ,unsigned char _pinIRQB, + struct ISRVars* _isr); + + int SPIDSetPulseBoundary(int min,int max); + bool SPIDSetup(float kc=KC,float taui=TAUI,float taud=TAUD,unsigned int sampleTime=10); + bool PIDRegulate(bool doRegulate=true); + + enum { + SPIDMODE_UNKNOWN, + SPIDMODE_SPEED, + SPIDMODE_STOP, + SPIDMODE_PULSE, + }; + unsigned char getSPIDMode() const; + unsigned char setSPIDMode(unsigned char _mode); + + int getPulseDesired() const; + int setPulseDesired(int _pulse); + int incPulseDesired(); + int decPulseDesired(); + + bool backToZero(); + bool scanZero(); + + void debugger() const; + +private: + int pulseInput; // + int pulseOutput; // + int pulseDesired; // + + int pulse2SpeedRPM; + + unsigned char SPIDMode; + + bool SPIDRegulateSpeed(bool doRegulate=true); // 201104 + bool SPIDRegulateStop(bool doRegulate=true); + bool SPIDRegulatePulse(bool doRegulate=true); + +}; + */ + + + +#endif + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde new file mode 100644 index 0000000..7b2519b --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde @@ -0,0 +1,31 @@ +/******************************************************** + * PID Simple Example + * Reading analog input 0 to control analog PWM output 3 + ********************************************************/ + +#include + +//Define Variables we'll be connecting to +double Setpoint, Input, Output; + +//Specify the links and initial tuning parameters +PID myPID(&Input, &Output, &Setpoint,2,5,1); + +void setup() +{ + //initialize the variables we're linked to + Input = analogRead(0); + Setpoint = 100; + + //turn the PID on + myPID.SetMode(AUTO); +} + +void loop() +{ + Input = analogRead(0); + myPID.Compute(); + analogWrite(3,Output); +} + + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde new file mode 100644 index 0000000..5674dd0 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde @@ -0,0 +1,55 @@ +/*************************************************** + * Advanced PID Example + * PID input: Analog input 0 + * output = time proportion on/off of output 0 (50% = 2 sec on, 2 sec off) + * and just to make it more complicated: + * - start with a setpoint ramp (0 to 500 over 5 minutes (300 seconds)) + * - use gap control (moderate tunings when within 100 of setpoint, aggressive when outside 100) + ***************************************************/ + + #include + + unsigned long windowStartTime; + unsigned long RampStartTime; + double Input, Output, Setpoint; + PID pid(&Input, &Output, &Setpoint, 3,4,1); + +unsigned long printTime= millis(); + + void setup() + { + + pid.SetOutputLimits(0,4000); //tell the PID to range the output from 0 to 4000 + Output = 4000; //start the output at its max and let the PID adjust it from there + + pid.SetMode(AUTO); //turn on the PID + windowStartTime = millis(); + RampStartTime = millis(); + } + + + void loop() + { + + //Set Point Ramp + if(millis()-RampStartTime<300000) + { + Setpoint = ((double)(millis()-RampStartTime))/(300000.0)*500.0; + } + else Setpoint = 500; + + //Set Tuning Parameters based on how close we are to setpoint + if(abs(Setpoint-Input)>100)pid.SetTunings(6,4,1); //aggressive + else pid.SetTunings(3,4,1); //comparatively moderate + + //give the PID the opportunity to compute if needed + Input = analogRead(0); + pid.Compute(); + + //turn the output pin on/off based on pid output + if(millis()-windowStartTime>4000) windowStartTime+=4000; + if(Output > millis()-windowStartTime) digitalWrite(0,HIGH); + else digitalWrite(0,LOW); + + } + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde new file mode 100644 index 0000000..5abdb06 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde @@ -0,0 +1,54 @@ +#include +#include + +/*************************************************** + * Feed Forward Example : RepRap Extruder Nozzle Temperature + * PID input: Nozzle Temperature + * PID output: PWM signal (0-255) to the heater control + * Other Input: Plastic Feed rate is being read on input 1 + * For the feed forward piece... + * The amount of heat we want to add to the nozzle is largely dependent + * on how fast plastic is being fed. if the feed is stopped we're going + * to want a lot less heat than when the system is running. A pid + * control by itself will eventually adjust the heat, but by then the temp + * will be really high. feed forward can help with this. + * so to make Feed Forward work, at every cycle we look at the current feed speed + * and compute a "bias" which we feed to the pid controller. sort of: "Based on my + * knowledge, your output should be about X. adjust off of that as you see fit" + * What does it get you? well, the instant the feed stops, the baseline drops. That + * means that the controller output immediately drops. if you had been using just a pid, + * you'd have to wait for the temperature to rise quite a bit before seeing the same + * drop in output. + * + * (for this example, we're assuming that for a feed rate of 1023 the output should be + * about 130, and at a rate of 0 the output should be around 10) + ***************************************************/ + + #include + int FeedRate; + + double Input, Output, Setpoint, Bias; + PID pid(&Input, &Output, &Setpoint, &Bias, 3, 4, 1); + + void setup() + { + Setpoint = 400; + pid.SetMode(AUTO); //turn on the PID + + } + + + void loop() + { + + //Read in the Feed rate and compute a baseline output to the heater + FeedRate = analogRead(1); //read the feed rate + Bias = (FeedRate/1023)*(120)+10; + + //give the PID the opportunity to compute if needed + Input = analogRead(0); + pid.Compute(); + + analogWrite(0, Output); + + } diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp b/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp new file mode 100644 index 0000000..59fb921 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp @@ -0,0 +1,309 @@ + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +#include "wiring.h" +#endif + +#include +#include "fuzzy_table.h" +#include +#include + +/* Standard Constructor (...)*********************************************** + * constructor used by most users. the parameters specified are those for + * for which we can't set up reliable defaults, so we need to have the user + * set them. + ***************************************************************************/ +PID::PID(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD) +{ + + PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD); + UsingFeedForward = false; + PID::Reset(); + + +} + +/* Overloaded Constructor(...)********************************************** + * This one is for more advanced users. it's essentially the same as the + * standard constructor, with one addition. you can link to a Feed Forward bias, + * which lets you implement... um.. Feed Forward Control. good stuff. + ***************************************************************************/ +PID::PID(int *Input, int *Output, int *Setpoint, int *FFBias, float Kc, float TauI, float TauD) +{ + + PID::ConstructorCommon(Input, Output, Setpoint, Kc, TauI, TauD); + UsingFeedForward = true; //tell the controller that we'll be using an external + myBias = FFBias; //bias, and where to find it + PID::Reset(); + +} + +/* ConstructorCommon(...)**************************************************** + * Most of what is done in the two constructors is the same. that code + * was put here for ease of maintenance and (minor) reduction of library size + ****************************************************************************/ +void PID::ConstructorCommon(int *Input, int *Output, int *Setpoint, float Kc, float TauI, float TauD) +{ + PID::SetInputLimits(0, 1023); //default the limits to the + PID::SetOutputLimits(0, 255); //full ranges of the I/O + + tSample = 1000; //default Controller Sample Time is 1 second + + PID::SetTunings( Kc, TauI, TauD); + + nextCompTime = millis(); + inAuto = false; + myOutput = Output; + myInput = Input; + mySetpoint = Setpoint; + + Err = lastErr = prevErr = 0; +} + + +/* SetInputLimits(...)***************************************************** + * I don't see this function being called all that much (other than from the + * constructor.) it needs to be here so we can tell the controller what it's + * input limits are, and in most cases the 0-1023 default should be fine. if + * there's an application where the signal being fed to the controller is + * outside that range, well, then this function's here for you. + **************************************************************************/ +void PID::SetInputLimits(int INMin, int INMax) +{ + //after verifying that mins are smaller than maxes, set the values + if(INMin >= INMax) return; + + + inMin = INMin; + inSpan = INMax - INMin; +} + +/* SetOutputLimits(...)**************************************************** + * This function will be used far more often than SetInputLimits. while + * the input to the controller will generally be in the 0-1023 range (which is + * the default already,) the output will be a little different. maybe they'll + * be doing a time window and will need 0-8000 or something. or maybe they'll + * want to clamp it from 0-125. who knows. at any rate, that can all be done + * here. + **************************************************************************/ +void PID::SetOutputLimits(int OUTMin, int OUTMax) +{ + //after verifying that mins are smaller than maxes, set the values + if(OUTMin >= OUTMax) return; + + outMin = OUTMin; + outSpan = OUTMax - OUTMin; +} + +/* SetTunings(...)************************************************************* + * This function allows the controller's dynamic performance to be adjusted. + * it's called automatically from the constructor, but tunings can also + * be adjusted on the fly during normal operation + ******************************************************************************/ +void PID::SetTunings(float Kc, float TauI, float TauD) +{ + //verify that the tunings make sense + if (Kc == 0.0 || TauI < 0.0 || TauD < 0.0) return; + + //we're going to do some funky things to the input numbers so all + //our math works out, but we want to store the numbers intact + //so we can return them to the user when asked. + P_Param = Kc; + I_Param = TauI; + D_Param = TauD; + + //convert Reset Time into Reset Rate, and compensate for Calculation frequency + float tSampleInSec = ((float)tSample / 1000.0); + float tempTauR; + if (TauI == 0.0) + tempTauR = 0.0; + else + tempTauR = (1.0 / TauI) * tSampleInSec; + + + kc = Kc; + taur = tempTauR; + taud = TauD / tSampleInSec; + + cof_A = kc * (1 + taur + taud); + cof_B = kc * (1 + 2 * taud); + cof_C = kc * taud; +} + +/* Reset()********************************************************************* + * does all the things that need to happen to ensure a bumpless transfer + * from manual to automatic mode. this shouldn't have to be called from the + * outside. In practice though, it is sometimes helpful to start from scratch, + * so it was made publicly available + ******************************************************************************/ +void PID::Reset() +{ + + if(UsingFeedForward) + bias = (*myBias - outMin) / outSpan; + else + bias = (*myOutput - outMin) / outSpan; + +} + +/* SetMode(...)**************************************************************** + * Allows the controller Mode to be set to manual (0) or Automatic (non-zero) + * when the transition from manual to auto occurs, the controller is + * automatically initialized + ******************************************************************************/ +void PID::SetMode(int Mode) +{ + if (Mode!=0 && !inAuto) + { //we were in manual, and we just got set to auto. + //reset the controller internals + PID::Reset(); + } + inAuto = (Mode!=0); +} + +/* SetSampleTime(...)******************************************************* + * sets the frequency, in Milliseconds, with which the PID calculation is performed + ******************************************************************************/ +void PID::SetSampleTime(int NewSampleTime) +{ + if (NewSampleTime > 0) + { + //convert the time-based tunings to reflect this change + taur *= ((float)NewSampleTime)/((float) tSample); + taud *= ((float)NewSampleTime)/((float) tSample); + tSample = (unsigned long)NewSampleTime; + + cof_A = kc * (1 + taur + taud); + cof_B = kc * (1 + 2 * taud); + cof_C = kc * taud; + } +} + +/* Compute() ********************************************************************** + * This, as they say, is where the magic happens. this function should be called + * every time "void loop()" executes. the function will decide for itself whether a new + * pid Output needs to be computed + * + * Some notes for people familiar with the nuts and bolts of PID control: + * - I used the Ideal form of the PID equation. mainly because I like IMC + * tunings. lock in the I and D, and then just vary P to get more + * aggressive or conservative + * + * - While this controller presented to the outside world as being a Reset Time + * controller, when the user enters their tunings the I term is converted to + * Reset Rate. I did this merely to avoid the div0 error when the user wants + * to turn Integral action off. + * + * - Derivative on Measurement is being used instead of Derivative on Error. The + * performance is identical, with one notable exception. DonE causes a kick in + * the controller output whenever there's a setpoint change. DonM does not. + * + * If none of the above made sense to you, and you would like it to, go to: + * http://www.controlguru.com . Dr. Cooper was my controls professor, and is + * gifted at concisely and clearly explaining PID control + *********************************************************************************/ +void PID::Compute() +{ + justCalced=false; + if (!inAuto) return; //if we're in manual just leave; + + unsigned long now = millis(); + + //millis() wraps around to 0 at some point. depending on the version of the + //Arduino Program you are using, it could be in 9 hours or 50 days. + //this is not currently addressed by this algorithm. + + + //...Perform PID Computations if it's time... + if (now>=nextCompTime) + { + + Err = *mySetpoint - *myInput; + //if we're using an external bias (i.e. the user used the + //overloaded constructor,) then pull that in now + if(UsingFeedForward) + { + bias = *myBias - outMin; + } + + + // perform the PID calculation. + //float output = bias + kc * ((Err - lastErr)+ (taur * Err) + (taud * (Err - 2*lastErr + prevErr))); + noInterrupts(); + int output = bias + (cof_A * Err - cof_B * lastErr + cof_C * prevErr); + interrupts(); + + //make sure the computed output is within output constraints + if (output < -outSpan) output = -outSpan; + else if (output > outSpan) output = outSpan; + + + prevErr = lastErr; + lastErr = Err; + + + //scale the output from percent span back out to a real world number + *myOutput = output; + + nextCompTime += tSample; // determine the next time the computation + if(nextCompTime < now) nextCompTime = now + tSample; // should be performed + + justCalced=true; //set the flag that will tell the outside world that the output was just computed + + } + + +} + + +/***************************************************************************** + * STATUS SECTION + * These functions allow the outside world to query the status of the PID + *****************************************************************************/ + +bool PID::JustCalculated() +{ + return justCalced; +} +int PID::GetMode() +{ + if(inAuto)return 1; + else return 0; +} + +int PID::GetINMin() +{ + return inMin; +} +int PID::GetINMax() +{ + return inMin + inSpan; +} +int PID::GetOUTMin() +{ + return outMin; +} +int PID::GetOUTMax() +{ + return outMin+outSpan; +} +int PID::GetSampleTime() +{ + return tSample; +} +float PID::GetP_Param() +{ + return P_Param; +} +float PID::GetI_Param() +{ + return I_Param; +} + +float PID::GetD_Param() +{ + return D_Param; +} + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h b/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h new file mode 100644 index 0000000..9e2f9a7 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h @@ -0,0 +1,113 @@ +#ifndef PID_Beta6_h +#define PID_Beta6_h + +class PID +{ + + + public: + + #define AUTO 1 + #define MANUAL 0 + #define LIBRARY_VERSION 0.6 + + //commonly used functions ************************************************************************** + PID(int*, int*, int*, // * constructor. links the PID to the Input, Output, and + float, float, float); // Setpoint. Initial tuning parameters are also set here + + PID(int*, int*, int*, // * Overloaded Constructor. if the user wants to implement + int*, float, float, float); // feed-forward + + void SetMode(int Mode); // * sets PID to either Manual (0) or Auto (non-0) + + void Compute(); // * performs the PID calculation. it should be + // called every time loop() cycles. ON/OFF and + // calculation frequency can be set using SetMode + // SetSampleTime respectively + + void SetInputLimits(int, int); //Tells the PID what 0-100% are for the Input + + void SetOutputLimits(int, int); //Tells the PID what 0-100% are for the Output + + + //available but not commonly used functions ******************************************************** + void SetTunings(float, float, // * While most users will set the tunings once in the + float); // constructor, this function gives the user the option + // of changing tunings during runtime for Adaptive control + + void SetSampleTime(int); // * sets the frequency, in Milliseconds, with which + // the PID calculation is performed. default is 1000 + + void Reset(); // * reinitializes controller internals. automatically + // called on a manual to auto transition + + bool JustCalculated(); // * in certain situations, it helps to know when the PID has + // computed this bit will be true for one cycle after the + // pid calculation has occurred + + + //Status functions allow you to query current PID constants *************************************** + int GetMode(); + int GetINMin(); + int GetINMax(); + int GetOUTMin(); + int GetOUTMax(); + int GetSampleTime(); + float GetP_Param(); + float GetI_Param(); + float GetD_Param(); + + + private: + + void ConstructorCommon(int*, int*, int*, // * code that is shared by the constructors + float, float, float); + + //scaled, tweaked parameters we'll actually be using + float kc; // * (P)roportional Tuning Parameter + float taur; // * (I)ntegral Tuning Parameter + float taud; // * (D)erivative Tuning Parameter + + float cof_A; + float cof_B; + float cof_C; + + //nice, pretty parameters we'll give back to the user if they ask what the tunings are + float P_Param; + float I_Param; + float D_Param; + + + int *myInput; // * Pointers to the Input, Output, and Setpoint variables + int *myOutput; // This creates a hard link between the variables and the + int *mySetpoint; // PID, freeing the user from having to constantly tell us + // what these values are. with pointers we'll just know. + + int *myBias; // * Pointer to the External FeedForward bias, only used + // if the advanced constructor is used + bool UsingFeedForward; // * internal flag that tells us if we're using FeedForward or not + + unsigned long nextCompTime; // * Helps us figure out when the PID Calculation needs to + // be performed next + // to determine when to compute next + unsigned long tSample; // * the frequency, in milliseconds, with which we want the + // the PID calculation to occur. + bool inAuto; // * Flag letting us know if we are in Automatic or not + + // the derivative required for the D term + //float accError; // * the (I)ntegral term is based on the sum of error over + // time. this variable keeps track of that + float bias; // * the base output from which the PID operates + + int Err; + int lastErr; + int prevErr; + + float inMin, inSpan; // * input and output limits, and spans. used convert + float outMin, outSpan; // real world numbers into percent span, with which + // the PID algorithm is more comfortable. + + bool justCalced; // * flag gets set for one cycle after the pid calculates +}; +#endif + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o b/src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o new file mode 100644 index 0000000000000000000000000000000000000000..d36c83d1f9dc69e085ba66efcfecde733b05baf4 GIT binary patch literal 17524 zcmd5^3w%`7nLih!Qd)&Zp{OW`DN86OnVGyuKqCwhkwU!0KCVH{;5*|E0c1+L-I&L=z9cD~R#ER+<=I^j6>6Jsxz8$2lHTxqC2)O=$9F?N9W zhH^rxcg-ELH?%uc+uqc^t@E|c$vfxl^z2-*^VEhlWzUvvFMFfxXv3j~p$+{TF4do| zKU#mNet-S0`lkB2`jz$GdPn{A`o8OXkI2KuV`fAS8n1A>5Rt9MdNU%K#uQ`Jmdq_v zw#?fC?=Kpk&bc&QkH?e7TI137C)?MyztFC{gk<*~sz~b|R<}RjzN7s|?eEPwI=v(K zTJT?k{~63LTUJ(G_I$5C_FbPCygS%i=8sbD-+lGSt0On|-+O-Dk^TMmy}N%1uY^g) z4~%!qj+C7)JHJ{*Usc7LJB)MToT$uJei`PeJb`!Cu&;h#^}^LlR+m;cRBx_+x%!>z zUsr!r{omEDmc=cVE%iKZm}BzCvyUS(?(!$fpDy22uI5!UVBY=-XF|K^-cR=-x{oHD zxsx5(VyrXfhl)a_Cz_5yLwq13aDQMfug5ut$2e;Qw$SJ^N~6jqcxR2cKuRDx@L*tm z;PF6NpplP4){tlAGwN352KcOz+u`e2)v=yeF;Shq&Q+c3JJ|tVwN#g9jk38k`p_2rduau<@+XR`yES!Lt7-Q#KX^o(j|jo~eM{XN~!lk5`seHdc<@5YEV< zhNHdK@!j=_RLkL&yr$)dv2)J;>9t0`IYXzz$5S)Bnk9ZenEx?by2)pw%iFEyjOGP|a9&Fj)fUoJj5C2yReN~EmFigRV1ovI=bD%^?7%IC zJudA+*1FlF-!fk0I#B}!El;)7wLHVC4cSE&RUKdzgZ2bg1Zo3KyaGJsE6Qujo67ao z4w>$DuQsF6$gfydQC;yo&trb&vdZep=PU2reAnjbn;+V$Jn3mC5*fT;d_;RkX`t`= z3skT5A+*E0Y+d%Q2fAJ}erA9U$NGYi+vV$8)wRA0Swr6`ZRb()`fe0#C~GKg$Zb&a z6)hWDFfOhlr6RlH!3s5ISe2z}eATQfMaNa8RApB^DCsNZ0(%O3AOFG5vU*O_^uxxG z;P%a1}+8yEy0%7mOU+>lzvhA&A^bri9vt4rr))5HXrmHx8y;>L^Re=KjVd9eo1tAd6V4K)p$xM#>0bR(yHz0*?# zH1=EU9$vavCY(7@mvHG=6W=}5uJDfWG9QV(SCb6Y(FjPw%C?$UdYlne)!mGzY5XOc zv47&FV?z_MU#S_@j-qCilA->IXF`xfT`3vVFf916?X$L+Ed#fxnN&aNxNLtTpG}^U z(|M}?u}>pz(68@noR&Kq@QAjwwybS4ww1PESJ_(gYK>lVdKMt9*B&we-onl$ou#`P zb^#CXuyxow9_Yvmlmu1>Hu6=cuS&#ov(aFvykN|*3QNWKidlS>K~{Oy71$S0dz+eT zL`PajR@V%^K5!~gQAfXu_#w*8QMzU1Rc@kABK=81N}f1Pbwkz-D%E@}rhUEO@`i?l8huDkoztf4c8-rj0$ozObF^}4`qfoo{T4}^YW ztZf|ewVxngWo1X77N!_al)KAsHpZ5Jul$c?7t8vW53c;Kk!rBefRl+ICm+wEQyZSl z@R)d^#E)$z?;Ww)PBn&_>3Mn1sA%2L+TOam{oS=k)}CH_zPZsTGo}U}33vlbD}*ex zoHLxQ#jWM7>sr6P)mA&awkq&kprNj`?&-j$!1}sXbzlF<`BhFxV~vqUrLov>8MBS2 zt2R}2Rqdsj;r{ZSF@P^qEoG`e1E#t)TnYi~n5d>D_nzKYIS|eiNvZ;S{5; z;+d+Bs@F|#MJCP}12zkEB|rPF|G(BRe*5GaodX%`Pi2zWix+<*m z9u0dnyjsJ}8h%5=ztix?8orJ~sr~gHYxhKaM;ViQkh%_hE|?KAN- z_MwTVvrkQ&Lx($Y#Ze<7!Pqp(yJ`UJf0$Dv2xk$7{WgjF5gspb_x6jiP+({C*a!f+ z>})AW*RhxJb+iBu__PUAP>@A65b4X%k^9-^lPHHUTwiU5HB(p+d#C(m! zeF?88+)VN!W6|+1lGG;uxKWa0+)y2OajNr_c_Hj}?`>{)KXYdj9m@|=Q^ zz`5cdyOaTCXPX#ujmyqnFy-y6i47NQ;oAn9XwoAUU?I5>p zC+8mJw(aC|os^gwenr|TZZq*-Hd12PV3kZ4mP4va=s)b$-B%sh#}`6FV_v-ZJB0XKypHBZB{9c0lTfZ68Uj zY|9`U;@D`O(Yf57(T4Id(jFm$;dUu6aH4UI)Cv76r%Jz@>rdkP%eele9`p+tp%YJJ zaX}~66fUK+j_aiJC|%pR&QwF?BGP6d135bzV_?nVf=-oprSny;GlT2=fa}aKZsyX_ zH7sP%WoJ4bXUaoA_B32d|52|0F0TKtT>o7?#6ieltexrjo`^W#2m!j3{=TvBYdqH< zM>vac_c0MN7;7gP6%*S3d$r%f^=EVaGr0cj9`p+tq5nQpe|LF&gzKElbuQ;RC-_`vK3n3$PhP1W+dv+HejXWM>lT7>vhI274J3Gz%0unyMa2Cd8XJ^?50uug= zePQBr>^ANtWd6+TCO*$RCceOyoA@IA;uVAKtdDWt#C?qzSy*<~&**33{zkTmV~y8M ze1j1(@j#<5omFw!*>{W^O?-uN#8(quCS_s>ua^8<3FAr-4gc3J=K^78@ob9h zv(pJ|zKKU`=dJ{LdW*}>lGtVwTj+_T_*V9kiEV7JiIcUnK?*x4`G~`LiB%jL$>unA z6X(9neYuHYKR+Nbo=xvbj6LVSB&L#OrzD2&UrKyE;l3o8MLO{;7)ZF8Fm>}QGA3eP zbl7RfOcEl5Z`IC4w`s8&s+|XJXOBpE_*E*g@@qKFtNa=-?SWs@C5B(~B&LRWB!*ur zB!*vWC5B%cC5B&Zg#QtKy-4%M$*1gl+@5~CZht2+{Q5**}$*J9H}Qda&(<`=DA+Jiv+(vOP6EqEY{2&@-koYRbCQkE@l7y(gv)% zha`sm3nhmAB@)B_)e^)028m(a4+;Oh?5FAI*M+=t?z20_yZp}7gat)KEHU3(;7ZIb zcKQ8@&f>fhccJ&rq$GwM)#98c0^tf}2oyBgKFVP$tMqJ6F1q-Z>1>S-px5t&2 zz#NX;61T_iE_5t*Iy`QlKf^yVX|yGN+$d_S@#FZv4Due=(qSHN;i6%A-jZC8i_6*K z$IY~5re`K)nj%_n^5wk?Jzl4nBIWWietK~i^DW>VNN4M~-zHcFrAki|yYtYKvl!O3K5OjO&hb8>g}YBW&DbGxoH-`Gn^f|ibTT2|TkJRUYN3MR+EZzAy-1{- zsa!c>kwryqw~68EMP|shaLrn%f@Co!(?F)xx`m2E?}}b2R#^))BOkih>(1jdSmW6e z#K|FQ-%6zbJ=8||lI!()#0Xvl=@jSGOj^H-{ka}Tk)ISSD-x^HN_pkWmlCs(uN|Z> zUCltbKq`_c>N=Z(2(~32!j@DeO`#GhARJ~>Bp{u{rEOLvO;G^bCUl#88+lKB$|)&H zO_tI&@)gn)0w@pj#lAy%CNB4k`)wn)NZg8YiR>Uh(IEynJT9lt^wuW4wWf$cQJunP zYbu{f^qDT&gy(9y=w1Squ!jA>*T*jO-NQQ9hzC?1YGp z>0(4@iVTkMu)w)g)=ElbT(pkpkQ9e2CPl_1Y@*&Nf&vp097!3o@!U(HVEAkv?-D#z zXerQBCOtFXy(nMN=|sDh`dx*2t~|w~Cr>&(cpQ#}B|eujm(qX*>v9(I+D!>pTJAEx zSf(j5fRq(_SW5#aZoagX#};HHCc%U;Fo7O9erHK>m}Rp{US61&nhai!Bga|nEC{p5 zq=J24G@G_)@a>oJQ~7iS%kC>ID)9?7DU>g4Cosk`#)`*m!LsQkeqCU+H7Q+m%yRjQ zyi}a#*wI#NvgjJ`cKS4SGO0|>e8A@}T$Jr{9&LJIc;I)2P{ z7r4Y=GQ%po_0MwRToPsrZ{3CWl=!`Z3q>|^Ha*irHKMj)Xa;~t_T{x?V%D_x$var%y;*XF{5h)>dqlY!_WOh)+OB$1)MFAY$&s2S%1jbhERVH$g?4dGm8cC`fKi&#ENRxlsK0vCCYm#=Uf_=oBqITZ8jx1h7^jPNXg4pDoI^ubf(g^Op)Sn zcUZZ4WJ#};9hM}LmwjPj^&sm`SoVd5)zhszVc92zCwL2e{^AmBp|t1(w&sH6-()$XG7|P3thKe&C6w%>< z$-^HyLDGXm98^_56+9r)dBuXC5I)+7!$L3fs|kCWU-f5a^8>OXzs~y84rc!^%n3(_ zp5p%tq4^tQ@pqh@CHR&V_?@}b`in)IuTZhe<4o|=?--Vl=kz;SLaxup5;)fzV7`3j2-xQ-aw zC11O-(Zs{w(yt_!N-}B;)IeVwy-20cjQFMIwGb6y-#zrb9Vv%>SIa)wH zBe94}=f|+Km-No7A|eC*`V63B_~qCb-KSfbSh zeUpf8qp+bqFg};)okU|TfX25@dx?fD=t`mw5{+*(KtD&cx**d^^oP`s_fP15mFUkk z`JWRVOY2nWKTh<`L@WJY5Istx2T)CBX!Ho8Clif#($Jr-)hu+}9Yx!A#n@E<- z;F<@;2(Ec1RNV0Kj}^E*^a_%U^ypE%!pTqRx$H=0c7*?GB@RbsN5nD3FaD9P)ip;dS6knH|9LgN2s}_aUA8sC|=e z=>ILXKchPq8w6eN(+!yqsl_)lvC%4+Fl64Lb`rI*-+?vs-3npw@ojnz-59%@_L@hiRb%rsezC@1s_{30 zkB1vw+kmB=2Y}^V2MJ@YBXr~2cFc8=ZhYIWG3UkYA3y54ZdTWU%BvO6_Ry zr993X3Qq!-Hax8H^9f^K=-jNyTp*0OV(8h?hi=I9rFJmgDi(6OV0&ex^PK5F08 zWIlq7tmjj}(tdmkuWC4k4wTS8gxZ_w#=LfFN7Ak8Fo7^+z<&sQsdGNC)L8^9*TrJO zkguh7E#1($m)hs(RyvyrLk4_&Q?GO$0hVj?xRm*V+CPCW^`Dpg@$_)NhVIz!$=DKx z&f(PF48C0Z!wAC$$Xmf5F63tthI~D>9`L1KrNGj!Hei|S-N16}0ZsmWP3Ex1KdJFA z0L!u8qWme_VhO{BQPhs48+Oj5HkEGW?^wc+0sjf`r9HL4Qs)O!<~X&bbPhqyGN4hWrkVzgOcU4eCS7VLGNU zil3w5Ur7v`LmD1HYZLO|uaOwOtkdxO8s0|h4)U;VkA{DtVLPn_$lxrSqhYK)#s8_q zIN$zC!{;@eM>Pp~oN=)h6kem@cO=HS_k9hY(C}cY6O6@KI8MW3HH=z?49>?(HHGADzj{RL literal 0 HcmV?d00001 diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h b/src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h new file mode 100644 index 0000000..b9a28e5 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h @@ -0,0 +1,66 @@ + +#if 0 +#ifndef _PID_TABLE_H_ +#define _PID_TABLE_H_ + +#define PB 6 +#define PM 5 +#define PS 4 +#define ZO 3 +#define NS 2 +#define NM 1 +#define NB 0 + +#define DELTA_KP_IDX 0 +#define DELATA_KI_IDX 1 +#define ERR 7 +#define DELTA_ERR 7 + + +const char pid_tbl[2][7][7]={ + { + {PB,PB,PM,PM,PS,ZO,ZO}, + {PB,PB,PM,PS,PS,ZO,ZO}, + {PM,PM,PM,PS,ZO,NS,NS}, + {PM,PM,PS,ZO,NS,NM,NM}, + {PS,PS,ZO,NS,NS,NM,NM}, + {PS,ZO,NS,NM,NM,NM,NB}, + {ZO,ZO,NM,NM,NM,NB,NB}, + }, + + { + {NB,NB,NM,NM,NS,ZO,ZO}, + {NB,NB,NM,NS,NS,ZO,ZO}, + {NB,NM,NS,NS,ZO,PS,PS}, + {NM,NM,NS,ZO,PS,PM,PM}, + {NM,NS,ZO,PS,PS,PM,PB}, + {ZO,ZO,PS,PS,PM,PB,PB}, + {ZO,ZO,PS,PM,PM,PB,PB}, + }, + }; + +const float kp_tbl[ERR] = { + 0.07987, + 0.08974, + 0.09948, + 0.12922, + 0.54896, + 0.70000, + 0.76000, + }; + + +const float ki_tbl[DELTA_ERR] = { + 0.02451, + 0.07353, + 0.14706, + 0.19608, + 0.24510, + 0.30000, + 0.00000, + }; + +#endif _PID_TABLE_H_ + +#endif + diff --git a/src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt b/src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt new file mode 100644 index 0000000..7e6eef6 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt @@ -0,0 +1,38 @@ +####################################### +# Syntax Coloring Map For LiquidCrystal +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### + +PID KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +SetMode KEYWORD2 +Compute KEYWORD2 +SetInputLimits KEYWORD2 +SetOutputLimits KEYWORD2 +SetTunings KEYWORD2 +SetSampleTime KEYWORD2 +JustCalculated KEYWORD2 +Reset KEYWORD2 +GetMode KEYWORD2 +GetINMin KEYWORD2 +GetINMax KEYWORD2 +GetOUTMin KEYWORD2 +GetOUTMax KEYWORD2 +GetSampleTime KEYWORD2 +GetP_Param KEYWORD2 +GetI_Param KEYWORD2 +GetD_Param KEYWORD2 + +####################################### +# Constants (LITERAL1) +####################################### + +AUTO LITERAL1 +MANUAL LITERAL1 diff --git a/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp new file mode 100644 index 0000000..a2e6a26 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp @@ -0,0 +1,191 @@ +/* + PinChangeInt.cpp +*/ + +#include + + +PCintPort::PCintPin PCintPort::PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS]; + + +PCintPort PCintPort::pcIntPorts[] = { + PCintPort(0,PCMSK0), + PCintPort(1,PCMSK1), + PCintPort(2,PCMSK2) +}; + +void PCintPort::addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc) +{ + //int i = 0; + PCintPin* p = PCintPin::pinDataAlloc; + do { + if (!p->PCintMode) { // found an available pin data structure + // now find the next slot to put it in + PCintPin** t = &pcIntPins[0]; + do { + if (!*t) { // we have a slot + // fill in the data + p->PCintMode = mode; + *t = p; + p->PCintFunc = userFunc; + // set the mask + pcmask |= p->PCIntMask = mask; + // enable the interrupt + PCICR |= PCICRbit; + + #ifdef DEBUG + Serial.print("BitMask = "); + Serial.print(mask, BIN); + Serial.print("; "); + Serial.print("slot = "); + Serial.println((int)(t - &pcIntPins[0]), DEC); + #endif + + return; + } + } while (int(++t) < int(&pcIntPins[8])); + } + } while (int(++p) < int(&PCintPin::pinDataAlloc[MAX_PIN_CHANGE_PINS])); +} + +void PCintPort::delPin(uint8_t mask) +{ + PCintPin** t = pcIntPins; + while (*t) { + PCintPin& p = **t; + if (p.PCIntMask == mask) { // found the target + uint8_t oldSREG = SREG; + cli(); + // disable the mask. + pcmask &= ~mask; + // if that's the last one, disable the interrupt. + if (pcmask == 0) { + PCICR &= ~(PCICRbit); + } + p.PCIntMask = 0; + p.PCintMode = 0; + p.PCintFunc = NULL; + do { // shuffle down as we pass through the list, filling the hole + *t = t[1]; + } while (*t); + SREG = oldSREG; + return; + } + t++; + } +} + +/* + * attach an interrupt to a specific pin using pin change interrupts. + */ +void PCintPort::attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode) +{ + uint8_t portNum = digitalPinToPort(pin); + #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + if (((portNum != 2) && (portNum != 11)) + || (userFunc == NULL) + ) { + + #ifdef DEBUG + Serial.println("NOT_A_PORT det"); + #endif + + return; + } + // map pin to PCIR register + uint8_t portIndex = (portNum == 2)?(0):(2); + #else + if ((portNum == NOT_A_PORT) || (userFunc == NULL)) { + return; + } + // map pin to PCIR register + uint8_t portIndex = portNum - 2; + #endif + + #ifdef DEBUG + Serial.print("portNum = "); + Serial.print(portNum, DEC); + Serial.print("; "); + #endif + + PCintPort &port = PCintPort::pcIntPorts[portIndex]; + port.addPin(mode,digitalPinToBitMask(pin),userFunc); +} + +void PCintPort::detachInterrupt(uint8_t pin) +{ + uint8_t portNum = digitalPinToPort(pin); + #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + if ((portNum != 2) && (portNum != 11)) { + //Serial.println("NOT_A_PORT det"); + return; + } + uint8_t portIndex = (portNum == 2)?(0):(2); + PCintPort& port = PCintPort::pcIntPorts[portIndex]; + + #else + if (portNum == NOT_A_PORT) { + //Serial.println("NOT_A_PORT det"); + return; + } + PCintPort& port = PCintPort::pcIntPorts[portNum - 2]; + #endif + + port.delPin(digitalPinToBitMask(pin)); +} + +// common code for isr handler. "port" is the PCINT number. +// there isn't really a good way to back-map ports and masks to pins. +void PCintPort::PCint() { + #ifndef DISABLE_PCINT_MULTI_SERVICE + uint8_t pcifr; + do { + #endif + uint8_t curr = portInputReg; + uint8_t mask = curr ^ PCintLast; + // get the pin states for the indicated port. + // mask is pins that have changed. screen out non pcint pins. + if ((mask &= pcmask) == 0) { + return; + } + PCintLast = curr; + PCintPin** t = pcIntPins; + while (*t) { + PCintPin& p = **t; + if (p.PCIntMask & mask) { // a changed bit + // Trigger interrupt if mode is CHANGE, or if mode is RISING and + // the bit is currently high, or if mode is FALLING and bit is low. + if (p.PCintMode == CHANGE + || ((p.PCintMode == FALLING) && !(curr & p.PCIntMask)) + || ((p.PCintMode == RISING) && (curr & p.PCIntMask)) + ) { + p.PCintFunc(); + } + } + t++; + } + #ifndef DISABLE_PCINT_MULTI_SERVICE + pcifr = PCIFR & PCICRbit; + PCIFR = pcifr; // clear the interrupt if we will process is (no effect if bit is zero) + } while(pcifr); + #endif +} + +#ifndef NO_PORTB_PINCHANGES +ISR(PCINT0_vect) { + PCintPort::pcIntPorts[0].PCint(); +} +#endif + +#ifndef NO_PORTC_PINCHANGES +ISR(PCINT1_vect) { + PCintPort::pcIntPorts[1].PCint(); +} +#endif + +#ifndef NO_PORTD_PINCHANGES +ISR(PCINT2_vect) { + PCintPort::pcIntPorts[2].PCint(); +} +#endif + diff --git a/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h new file mode 100644 index 0000000..c7d2beb --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h @@ -0,0 +1,136 @@ +/* + PinChangeInt.h +*/ + + +#ifndef PinChangeInt_h +#define PinChangeInt_h + +#if defined(ARDUINO) && ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include "PinChangeIntConfig.h" +#ifndef Pins_Arduino_h +#include "pins_arduino.h" +#endif +// This library was inspired by and derived from "johnboiles" (it seems) Main.PCInt Arduino Playground example +// see: http://www.arduino.cc/playground/Main/PcInt + + +// Interrupt management for PCI +/* + * an extension to the interrupt support for arduino. + * add pin change interrupts to the external interrupts, giving a way + * for users to have interrupts drive off of any pin. + * Refer to avr-gcc header files, arduino source and atmega datasheet. + */ + +/* + * Theory: all IO pins on AtmegaX(168/328/1280/2560) are covered by Pin Change Interrupts. + * The PCINT corresponding to the pin must be enabled and masked, and + * an ISR routine provided. Since PCINTs are per port, not per pin, the ISR + * must use some logic to actually implement a per-pin interrupt service. + */ + +/* Pin to interrupt map ON ATmega168/328: + * D0-D7 = PCINT 16-23 = PCIR2 = PD = PCIE2 = pcmsk2 + * D8-D13 = PCINT 0-5 = PCIR0 = PB = PCIE0 = pcmsk0 + * A0-A5 (D14-D19) = PCINT 8-13 = PCIR1 = PC = PCIE1 = pcmsk1 + */ + + +/* Pin to interrupt map ON ATmega1280/2560: + * D50-D53 = PCINT 3-0 = PCIR0 = PB = PCIE0 = pcmsk0 + * D10-D13 = PCINT 4-7 = PCIR0 = PB = PCIE0 = pcmsk0 + * A8-A15 (D62-D69) = PCINT 16-23 = PCIR2 = Pk = PCIE2 = pcmsk2 + + * *******D0 = PCINT 8 = PCIR1 = PE = PCIE1 = pcmsk1********** + * *******D14-D15 = PCINT 10-9 = PJ = PCIE1 = pcmsk1********* + ********NOTE:PCINT 8-15 does NOT available in this library****** + */ + + +/* + Please make any configuration changes in the accompanying PinChangeIntConfig.h file. + This will help avoid having to reset your config in the event of changes to the + library code (just don't replace that file when you update). + +*/ + + +#ifndef MAX_PIN_CHANGE_PINS +#define MAX_PIN_CHANGE_PINS 8 +#endif + +// You can reduce the memory footprint of this handler by declaring that there will be no pin change interrupts +// on any of the three ports. +// define NO_PORTB_PINCHANGES to indicate that port b will not be used for pin change interrupts +// define NO_PORTC_PINCHANGES to indicate that port c will not be used for pin change interrupts +// define NO_PORTD_PINCHANGES to indicate that port d will not be used for pin change interrupts +// If only a single port remains, the handler will be declared inline reducing the size and latency +// of the handler. + +// if their is only one PCInt vector in use the code can be inlined +// reducing latecncy and code size +#define INLINE_PCINT +#if ((defined(NO_PORTB_PINCHANGES) && defined(NO_PORTC_PINCHANGES)) || \ + (defined(NO_PORTC_PINCHANGES) && defined(NO_PORTD_PINCHANGES)) || \ + (defined(NO_PORTD_PINCHANGES) && defined(NO_PORTB_PINCHANGES))) +#undef INLINE_PCINT +#define INLINE_PCINT inline +#endif + +// Provide drop in compatibility with johnboiles PCInt project at +// http://www.arduino.cc/playground/Main/PcInt +#define PCdetachInterrupt(pin) PCintPort::detachInterrupt(pin) +#define PCattachInterrupt(pin,userFunc,mode) PCintPort::attachInterrupt(pin, userFunc,mode) + + + +typedef void (*PCIntvoidFuncPtr)(void); + +class PCintPort { + PCintPort(int index,volatile uint8_t& maskReg) : + #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) + portInputReg(*portInputRegister((index == 0)?(2):(index + 9))), + + #else + portInputReg(*portInputRegister(index + 2)), + #endif + + pcmask(maskReg), + PCICRbit(1 << index), + PCintLast(0) { + for (int i = 0; i < 9; i++) { + pcIntPins[i] = NULL; + } + } +public: + static void attachInterrupt(uint8_t pin, PCIntvoidFuncPtr userFunc, int mode); + static void detachInterrupt(uint8_t pin); + INLINE_PCINT void PCint(); + static PCintPort pcIntPorts[]; + +protected: + class PCintPin { + public: + PCintPin() : + PCintFunc((PCIntvoidFuncPtr)NULL), + PCintMode(0) {} + PCIntvoidFuncPtr PCintFunc; + uint8_t PCintMode; + uint8_t PCIntMask; + static PCintPin pinDataAlloc[MAX_PIN_CHANGE_PINS]; + }; + void addPin(uint8_t mode,uint8_t mask,PCIntvoidFuncPtr userFunc); + void delPin(uint8_t mask); + volatile uint8_t& portInputReg; + volatile uint8_t& pcmask; + const uint8_t PCICRbit; + uint8_t PCintLast; + PCintPin* pcIntPins[9]; // extra entry is a barrier +}; +#endif diff --git a/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h new file mode 100644 index 0000000..c8b8b13 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h @@ -0,0 +1,20 @@ + +// This file is used to separate the changes you make to personalize the +// Pin Change Interrupt library from any future changes to the library itself. +// Ideally it would reside in the folder of the current sketch, but I have not +// figured out how such a file can be included from a library. +// Nothing is required to be defined within this file since default values are +// defined in the primary PinChangeInt.h file. + +// Uncomment the line below to limit the handler to servicing a single interrupt per invocation. +//#define DISABLE_PCINT_MULTI_SERVICE + +// Define the value MAX_PIN_CHANGE_PINS to limit the number of pins that may be +// used for pin change interrupts. This value determines the number of pin change +// interrupts supported for all ports. +//#define MAX_PIN_CHANGE_PINS 2 + +// declare ports without pin change interrupts used +//#define NO_PORTB_PINCHANGES +//#define NO_PORTC_PINCHANGES +//#define NO_PORTD_PINCHANGES diff --git a/src/dcaitirobot/embedded/lib/README b/src/dcaitirobot/embedded/lib/README new file mode 100644 index 0000000..6debab1 --- /dev/null +++ b/src/dcaitirobot/embedded/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into executable file. + +The source code of each library should be placed in a an own separate directory +("lib/your_library_name/[here are source files]"). + +For example, see a structure of the following two libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +and a contents of `src/main.c`: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +PlatformIO Library Dependency Finder will find automatically dependent +libraries scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html