t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add libs

Browse Source
This commit is contained in:
Tim Korjakow 2023-07-10 22:31:15 +08:00
parent 2f337d06e0
commit 63f016e00f
15 changed files with 1793 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/dcaitirobot/.gitignore vendored
View File

@ -3,7 +3,6 @@ logs/
install/
build/
bin/
lib/
log/
msg_gen/
srv_gen/

385
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp Normal file
View File

@ -0,0 +1,385 @@
#include<MotorWheel.h>
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()<endTime;) {
PIDRegulate();
if(debug && (millis()%500==0)) debugger();
if(endTime-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();
}

349
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h Normal file
View File

@ -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<Arduino.h>
//#include<WProgram.h>
#include<PID_Beta6.h>
#include<PinChangeInt.h>
#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

31
src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample/PIDSample.pde Normal file
View File

@ -0,0 +1,31 @@
/********************************************************
* PID Simple Example
* Reading analog input 0 to control analog PWM output 3
********************************************************/
#include <PID_Beta6.h>
//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);
}

55
src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample2/PIDSample2.pde Normal file
View File

@ -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 <PID_Beta6.h>
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);
}

54
src/dcaitirobot/embedded/lib/PID_Beta6/Examples/PIDSample3/PIDSample3.pde Normal file
View File

@ -0,0 +1,54 @@
#include <fuzzy_table.h>
#include <PID_Beta6.h>
/***************************************************
* 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 <PID_Beta6.h>
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);
}

309
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp Normal file
View File

@ -0,0 +1,309 @@
#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif
#include <PID_Beta6.h>
#include "fuzzy_table.h"
#include <wiring_private.h>
#include <HardwareSerial.h>
/* 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;
}

113
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h Normal file
View File

@ -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

BIN
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o Normal file
View File

Binary file not shown.

66
src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h Normal file
View File

@ -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

38
src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt Normal file
View File

@ -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

191
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp Normal file
View File

@ -0,0 +1,191 @@
/*
PinChangeInt.cpp
*/
#include <PinChangeInt.h>
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

136
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h Normal file
View File

@ -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

20
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeIntConfig.h Normal file
View File

@ -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

46
src/dcaitirobot/embedded/lib/README Normal file
View File

@ -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 <Foo.h>
#include <Bar.h>
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