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
|
||||
|
||||
float motorP=2.0;
|
||||
float motorI=0.05;
|
||||
float motorI=0.2;
|
||||
float motorD=1;
|
||||
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
|
||||
|
||||
long last_potidifferenceLow=0;
|
||||
|
@ -402,8 +404,16 @@ void loop() {
|
|||
}else{ //not reached position
|
||||
_motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255
|
||||
|
||||
|
||||
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);
|
||||
motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM);
|
||||
|
||||
|
|
Loading…
Reference in New Issue