impove motor anti windup
This commit is contained in:
parent
587fd4fcd3
commit
edff630fb9
|
@ -135,10 +135,12 @@ long last_motorcheck=0;
|
||||||
//long last_motorTooSlow=0; //typically 0
|
//long last_motorTooSlow=0; //typically 0
|
||||||
|
|
||||||
float motorP=2.0;
|
float motorP=2.0;
|
||||||
float motorI=0.05;
|
float motorI=0.2;
|
||||||
float motorD=1;
|
float motorD=1;
|
||||||
float potidifference_integral=0;
|
float potidifference_integral=0;
|
||||||
#define MOTORI_ANTIWINDUP 100 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
|
//#define MOTORI_ANTIWINDUP 100 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK
|
||||||
|
#define MOTOR_ANTIWINDUP
|
||||||
|
#define MOTOR_ANTIWINDUP_RANGE 200 //0 to 1024
|
||||||
//Motor starts moving at about speed=80
|
//Motor starts moving at about speed=80
|
||||||
|
|
||||||
long last_potidifferenceLow=0;
|
long last_potidifferenceLow=0;
|
||||||
|
@ -402,8 +404,16 @@ void loop() {
|
||||||
}else{ //not reached position
|
}else{ //not reached position
|
||||||
_motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255
|
_motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255
|
||||||
|
|
||||||
|
|
||||||
potidifference_integral+=potidifference*motorI;
|
potidifference_integral+=potidifference*motorI;
|
||||||
potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain
|
|
||||||
|
#ifdef MOTOR_ANTIWINDUP
|
||||||
|
if (abs(potidifference)>MOTOR_ANTIWINDUP_RANGE) { //outside integral range
|
||||||
|
potidifference_integral=0; //reset
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
//potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain
|
||||||
_motormove=potidifference*motorP + potidifference_integral + motorD*(last_potidifference-potidifference);
|
_motormove=potidifference*motorP + potidifference_integral + motorD*(last_potidifference-potidifference);
|
||||||
motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
|
motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue