Add libs
This commit is contained in:
parent
2f337d06e0
commit
63f016e00f
src/dcaitirobot
.gitignore
embedded/lib
MotorWheel
PID_Beta6
PinChangeInt
README
1
src/dcaitirobot/.gitignore
vendored
1
src/dcaitirobot/.gitignore
vendored
@ -3,7 +3,6 @@ logs/
|
||||
install/
|
||||
build/
|
||||
bin/
|
||||
lib/
|
||||
log/
|
||||
msg_gen/
|
||||
srv_gen/
|
||||
|
385
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp
Normal file
385
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.cpp
Normal 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
349
src/dcaitirobot/embedded/lib/MotorWheel/MotorWheel.h
Normal 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
|
||||
|
@ -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);
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
|
||||
}
|
||||
|
@ -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
309
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.cpp
Normal 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
113
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.h
Normal 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
BIN
src/dcaitirobot/embedded/lib/PID_Beta6/PID_Beta6.o
Normal file
Binary file not shown.
66
src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h
Normal file
66
src/dcaitirobot/embedded/lib/PID_Beta6/fuzzy_table.h
Normal 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
38
src/dcaitirobot/embedded/lib/PID_Beta6/keywords.txt
Normal 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
191
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.cpp
Normal 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
136
src/dcaitirobot/embedded/lib/PinChangeInt/PinChangeInt.h
Normal 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
|
@ -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
46
src/dcaitirobot/embedded/lib/README
Normal 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
|
Loading…
x
Reference in New Issue
Block a user