impove motor anti windup

This commit is contained in:
interfisch 2020-05-03 13:04:34 +02:00
parent 587fd4fcd3
commit edff630fb9
1 changed files with 13 additions and 3 deletions

View File

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