Code:
//*************************************************************//*************************************************************
// motor API // motor API
//*************************************************************//*************************************************************
#define MAXMOTORS 10 // maximum number of motors
#define MAXMOTORSLOCAL 2 // maximum number of local motors
#define MAXPWMRANGE 255 // maximum software-pwm range (0-255)
// motor control structure
typedef struct {
// electrical motor pins
uint8_t pind1, pind2, pinpwm; // dir + pwm L293 H-Bridge type
uint8_t pinQa, pinQb; // rotary enc pins Qa,Qb
// pwm and encoder values
int16_t dirpwm;
int32_t motenc, oldenc; // rotary encoder values
// PID
pthread_t tid;
// PID custom target values
int32_t target; // set target
int16_t tarpwm; // motor target speed
// PID custom regulation parameters
double P; // P: basic propotional to error
double I; // I: integral: avoid perish
double D; // D: derivative: avoid oscillating
double precis; // error precision to target
int32_t regtime; // PID loop time
double damp; // damp the integral memory
int8_t cont; // target: continue or hit once
// internal control variables
int16_t runstate; // monitors runstate
int16_t outp; // PID control output value
int16_t maxout; // max output (max motor pwr)
int32_t read; // current sensor reading
double err; // current error
double integr; // integral of errors
double speed; // current speed
int8_t stopPIDcontrol; // flag for external termination
} tEncMotor;
tEncMotor motor[MAXMOTORS];
#define motorLeft motor[0]
#define motorRight motor[1]
// motor runstates:
#define OUT_REGSTATE_NULL 0
#define OUT_REGSTATE_COAST 2
#define OUT_REGSTATE_BRAKE 3
#define OUT_REGSTATE_EMERG_STOP 5
#define OUT_REGSTATE_ON 8
#define OUT_REGSTATE_RAMPUP 9
#define OUT_REGSTATE_RAMPDOWN 10
#define OUT_REGSTATE_PIDIDLE 15
#define OUT_REGSTATE_ACTIVE 16
#define OUT_REGSTATE_PIDSTART 17
#define OUT_REGSTATE_PIDEND 18
#define OUT_REGSTATE_PIDHOLD 19
#define OUT_REGSTATE_PIDHOLDCLOSE 20
//*************************************************************
#define motorCoast(nr) motorOn(nr, 0) // alias for motor coast
//*************************************************************
inline void motorBrake(uint nr, int dirpwm) { // brake by pwm power
int pwm;
pwm = abs(dirpwm);
digitalWrite(motor[nr].pind1, HIGH);
digitalWrite(motor[nr].pind2, HIGH);
motor[nr].dirpwm = pwm;
softPwmWrite(motor[nr].pinpwm, pwm); // brake power always > 0
}
//*************************************************************
inline void motorOn(uint nr, int dirpwm) { // motor On (nr, dir_pwm)
int dir, pwm; // signed pwm:
if(dirpwm > 0) dir = +1; // pwm > 0: forward
else if(dirpwm < 0) dir = -1; // pwm < 0: reverse
else dir = 0; // pwm = 0: coast
if(! _REMOTE_OK_) dirpwm=0;
pwm = abs(dirpwm);
if(dir> 0) {
digitalWrite( motor[nr].pind1, HIGH);
digitalWrite( motor[nr].pind2, LOW);
}
else
if(dir==0) {
digitalWrite( motor[nr].pind1, LOW);
digitalWrite( motor[nr].pind2, LOW);
}
else {
digitalWrite( motor[nr].pind1, LOW);
digitalWrite( motor[nr].pind2, HIGH);
}
motor[nr].dirpwm = dirpwm;
softPwmWrite( motor[nr].pinpwm, pwm);
}
//*************************************************************//*************************************************************
// PID control // PID control
//*************************************************************//*************************************************************
// forward: motor API functions
inline void RotatePIDtoTarget (uint nr, int32_t Target, double RotatSpeed); // approach absolute target once
inline void RotatePIDdegrees (uint nr, int32_t Target, double RotatSpeed); // turn relative degrees
inline void RotatePIDcontinue (uint nr, int32_t Target, double RotatSpeed); // approach target continuously
inline void StopPIDcontrol (uint nr);
inline void PIDinit(); // P=0.4, I=0.4, D=10.0
// simple customized PID setting:
inline void SetPIDparam(uint nr, double P,double I,double D);
// extended customized parameter setting:
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp);
//*************************************************************
inline void PIDcleanup(uint nr) {
motorCoast(nr);
motor[nr].runstate = OUT_REGSTATE_NULL;
motor[nr].speed = 0;
motor[nr].outp = 0;
motor[nr].cont = 0;
}
//*************************************************************
void * PID_calc(void *arg) {
double aspeed, damp, PWMpwr, readold, errorold, tprop;
int32_t readstart, cmax, cmin; // for monitoring
int32_t starttime, runtime, clock, dtime; // timer
int regloop;
// arg nach index casten
unsigned nr = (unsigned)arg;
motor[nr].runstate = OUT_REGSTATE_PIDSTART ; // reg state: RAMPUP
motor[nr].read = motor[nr].motenc; // get current encoder reading
motor[nr].err = motor[nr].target - motor[nr].read; // error to target
readstart = motor[nr].read;
regloop = 1;
damp=0; // damp the integral memory
starttime= millis();
// appoach target
_Astart:
motor[nr].runstate = OUT_REGSTATE_ACTIVE; // run state: RUNNING
do {
pthread_testcancel();
if (motor[nr].stopPIDcontrol) {
PIDcleanup(nr);
return NULL;
}
dtime = millis() - clock;
clock = millis();
runtime = clock - starttime;
tprop = dtime/20.0;
if ((motor[nr].err==errorold)&& (abs(motor[nr].err)>motor[nr].precis)) damp=1; // stalling
else
damp=motor[nr].damp;
motor[nr].integr = (damp * motor[nr].integr) + motor[nr].err;
if((motor[nr].integr) > 3*motor[nr].maxout) motor[nr].integr = 3*motor[nr].maxout; // cut away
else
if((motor[nr].integr) <-3*motor[nr].maxout) motor[nr].integr =-3*motor[nr].maxout;
PWMpwr= (motor[nr].P*motor[nr].err) + (motor[nr].I*motor[nr].integr)*tprop + (motor[nr].D*(motor[nr].err-errorold))/tprop;
if(PWMpwr > motor[nr].maxout) PWMpwr= motor[nr].maxout; // forward maxout
else
if(PWMpwr < -motor[nr].maxout) PWMpwr= -motor[nr].maxout; // reverse maxout
motor[nr].speed= (motor[nr].read-readold)*100/dtime; // rotat speed [degrees/100ms]
aspeed = abs(motor[nr].speed) ;
if (abs(PWMpwr) > motor[nr].tarpwm ) {
PWMpwr = sign(PWMpwr) * motor[nr].tarpwm ;
}
motor[nr].outp = round(PWMpwr);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// PID regulation !
motorOn(nr, motor[nr].outp); // action !
delay(motor[nr].regtime); // wait regulation time
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
readold = motor[nr].read; // save old sensor
errorold = motor[nr].err; // save old error
motor[nr].read = motor[nr].motenc; // get new encoder value
motor[nr].err = motor[nr].target-motor[nr].read; // new error to target
if (motor[nr].read>cmax) cmax=motor[nr].read; // monitor overshooting
else
if (motor[nr].read<cmin) cmin=motor[nr].read; // monitor overshooting
if ((motor[nr].cont)&& (abs(motor[nr].err)<=motor[nr].precis))
motor[nr].runstate = OUT_REGSTATE_PIDSTART ;
else motor[nr].runstate = OUT_REGSTATE_ACTIVE;
if (motor[nr].cont) continue;
if (abs(motor[nr].err)<=motor[nr].precis) {
regloop +=1 ;
motor[nr].runstate = OUT_REGSTATE_PIDEND ;
}
if (motor[nr].stopPIDcontrol) {
PIDcleanup(nr);
return NULL;
}
} while ((abs(motor[nr].err)>=motor[nr].precis) && (regloop<=5)); // target reached
motorCoast(nr); // finished - stop motor
motor[nr].runstate = OUT_REGSTATE_PIDEND; // run state: RAMPDOWN
motor[nr].outp=0;
delay(50);
motor[nr].read = motor[nr].motenc;
regloop=1;
if (motor[nr].read>cmax) cmax=motor[nr].read; // detect overshooting
if (motor[nr].read<cmin) cmin=motor[nr].read; // detect overshooting
motor[nr].err = motor[nr].target-motor[nr].read;
if (motor[nr].stopPIDcontrol) {
PIDcleanup(nr);
return NULL;
}
if ((abs(motor[nr].err)>motor[nr].precis)) {goto _Astart;}
motor[nr].runstate=0;
delay(1); //runstate = IDLE
return (NULL);
}
//*************************************************************
inline void RotatePID(uint nr, int32_t Target, double RotatSpeed, int8_t cont) {
if( !motor[nr].tid && motor[nr].runstate) {
motor[nr].runstate=0; // repair
delay(1);
}
if( ( motor[nr].runstate ) || ( motor[nr].tid ) ) {
motor[nr].stopPIDcontrol = 1;
delay(1);
while( motor[nr].runstate || motor[nr].tid ); // wait for PID task to terminate
delay(1);
PIDcleanup(nr);
}
// init new PID structure
motor[nr].runstate = 1; // set runstate: PID active
// custom init PID [nr]
motor[nr].target = Target; // assign target
motor[nr].tarpwm = abs(RotatSpeed); // assign max rotation speed
motor[nr].cont=cont; // cont vs. hit once
// Reset PID control defaults
motor[nr].outp = 0; // PID control output value
motor[nr].maxout = MAXPWMRANGE; // absolute max possible output (max pwr)
motor[nr].read = 0; // current reading
motor[nr].err = 0; // current error
motor[nr].integr = 0; // integral of errors
motor[nr].speed = 0; // current speed
motor[nr].stopPIDcontrol = 0; // flag for external termination
// start PID control task
pthread_create( & motor[nr].tid, // id speichern in dem dazugehörigen Array-Element
0, // Default Attribute (threads cancelbar)
PID_calc, // Name der PID-Kalkulationsroutinge
(void *) nr); // der Index des Array-Elements für eine PID Struktur,
// die mit dem Motorindex gleich ist.
// run pthread task in detached mode, auto-cleanup
pthread_detach(motor[nr].tid);
}
//*************************************************************
inline void StopPIDcontrol (uint nr) {
if (motor[nr].tid) { // stop PID task if already running
motor[nr].stopPIDcontrol = 1;
}
}
//*************************************************************
// custom PID parameters Extended
inline void SetPIDparamEx(uint nr, double P, double I, double D, double prec, int16_t regtime, double damp) {
motor[nr].P = P; // P: propotional to error
motor[nr].I = I; // I: avoid perish
motor[nr].D = D; // D: derivative: avoid oscillating
motor[nr].precis = prec; // error precision to target
motor[nr].regtime = regtime; // PID regulation time
motor[nr].damp = damp; // damp error integral
}
//*************************************************************
// custom PID parameters
inline void SetPIDparam(uint nr, double P, double I, double D) {
motor[nr].P = P; // P: propotional to error
motor[nr].I = I; // I: integral: avoid perish
motor[nr].D = D; // D: derivative: avoid oscillating
}
//*************************************************************
inline void PIDinit() {
for (uint nr=0; nr < MAXMOTORS; ++nr) {
SetPIDparamEx(nr, 0.40, 0.40, 10.0, 1.0, 5, 0.75); // p,i,d, precis, reg_time, damp
delay(1);
}
}
//*************************************************************
inline void RotatePIDtoTarget(uint nr, int32_t Target, double RotatSpeed) {
RotatePID(nr, Target, RotatSpeed, false);
delay(1);
}
//*************************************************************
inline void RotatePIDcont(uint nr, int32_t Target, double RotatSpeed) {
RotatePID(nr, Target, RotatSpeed, true);
delay(1);
}
//*************************************************************
inline void RotatePIDdegrees(uint nr, int32_t Target, double RotatSpeed) {
RotatePID(nr, Target+motor[nr].motenc, RotatSpeed, false);
delay(1);
}
//*************************************************************
// ^^^^^^^^^^^^^^ PID end ^^^^^^^^^^^^^
//*************************************************************
Zitieren
Lesezeichen