From 5f1665c2d7309c7ff57a778932d62349ba656de8 Mon Sep 17 00:00:00 2001 From: Fisch Date: Wed, 1 Apr 2020 13:01:07 +0200 Subject: [PATCH] implement PI controller for motor movement --- .../mixercontroller_w5100_pio/src/main.cpp | 130 ++++++++++++++---- 1 file changed, 104 insertions(+), 26 deletions(-) diff --git a/controller/mixercontroller_w5100_pio/src/main.cpp b/controller/mixercontroller_w5100_pio/src/main.cpp index 269c27c..5a6c1fc 100644 --- a/controller/mixercontroller_w5100_pio/src/main.cpp +++ b/controller/mixercontroller_w5100_pio/src/main.cpp @@ -1,3 +1,11 @@ +/* + * Ideas/TODO: + * POT_MIN, POT_MAX as variable with calibration procedure. Drive slowly to both ends until value does not get lower. + * Motor error checking. Timeout overall (if regulation fails or stuck). Timeout movement (motor is tunring but no change in poti value detected). Move right direction. + * Relais switching (selection and muting) + * MQTT topics +*/ + #include #include @@ -67,6 +75,7 @@ float encoderMultiplier=4.0; #define PIN_MOTOR_IN2 PD6 //to L293(pin7) Motor IN2 //#define SRPIN_MOTOR_IN1 1 //L293(pin2) Motor IN1 -- moved to atmega pin //#define SRPIN_MOTOR_IN2 2 //L293(pin7) Motor IN2 -- moved to atmega pin +uint8_t motorspeed=0; #define PIN_POT A0 //A0 = PC0, reference potentiometer wiper #define DEADZONE_POTI 5 //maximum allowed error. stop when reached this zone @@ -74,7 +83,7 @@ float encoderMultiplier=4.0; #define POT_MAX 1010 //maximum value pot can reach #define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever -int poti_set=512; //set value +int poti_set; //set value, initial value will be read from poti int poti_read=0; //read value from poti boolean poti_reachedposition=true; //set to true if position reached. after that stop turning @@ -85,28 +94,37 @@ boolean poti_reachedposition=true; //set to true if position reached. after that #define MOTOR_STOP(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,LOW); #define MOTOR_LEFT(); digitalWrite(PIN_MOTOR_IN1,LOW); digitalWrite(PIN_MOTOR_IN2,HIGH); #define MOTOR_RIGHT(); digitalWrite(PIN_MOTOR_IN1,HIGH); digitalWrite(PIN_MOTOR_IN2,LOW); +#define MOTOR_LEFT_PWM(); digitalWrite(PIN_MOTOR_IN1,LOW); analogWrite(PIN_MOTOR_IN2,motorspeed); +#define MOTOR_RIGHT_PWM(); analogWrite(PIN_MOTOR_IN1,motorspeed); digitalWrite(PIN_MOTOR_IN2,LOW); #define MOTOR_TURNING() (digitalRead(PIN_MOTOR_IN1) != digitalRead(PIN_MOTOR_IN2)) //Motorcheck long last_motorcheck=0; #define INTERVAL_MOTORCHECK 100 //check motor movement every x ms -int poti_read_last=0; -int motor_vel=0; //analog read units per second -#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active -#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate -long last_motorTooSlow=0; //typically 0 +//int poti_read_last=0; +//int motor_vel=0; //analog read units per second //TODO: reintroduce into code or remove +//#define MINIMUM_MOTORVEL 20 //minimum velocity motor should turn wenn active +//#define MOTOR_FAILTIME 500 //in ms. if motor did not turn fox x amount of time at least with MINIMUM_MOTORVEL an error will initiate +//long last_motorTooSlow=0; //typically 0 + +float motorP=1.0; +float motorI=0.1; +float potidifference_integral=0; +#define MOTORI_ANTIWINDUP 32 //maximum value for (potidifference_integral*motorI). time depends on INTERVAL_MOTORCHECK + +long last_potidifferenceLow=0; +#define DEADZONETIMEUNTILREACHED 500 //time [ms] poti read value has to be inside of deadzone to set reachedposition flag (and stop regulating) //error -uint8_t error=0; -#define NOERROR 0 -#define MOTORDIDNOTTURN 1 +boolean motorerror=false; void setup() { pinMode(PIN_BUTTON,INPUT_PULLUP); button_knob = Button(); pinMode(PIN_POT,INPUT); + pinMode(SRLATCH, OUTPUT); pinMode(SRCLOCK, OUTPUT); @@ -162,6 +180,10 @@ void setup() { Serial.println("Ethernet disabled"); } + + poti_set=analogRead(PIN_POT); + Serial.print("Poti value="); Serial.println(poti_set); + Serial.println("Ready"); last_send = millis(); @@ -218,20 +240,8 @@ void loop() { //Inputs ################################################### poti_read=poti_read*POTIFILTER + (1.0-POTIFILTER)*analogRead(PIN_POT); //read poti - /* - if (!digitalRead(PIN_BUTTON)){ //button pressed - if (button_released){ - button_released=false; //flag: not released - if(loopmillis-last_button_released > BUTTON_RELEASE_DEBOUNCE){ - button_flag=true; - } - } - }else if(!button_flag && !button_released){ //button released and flag has been cleared - last_button_released=loopmillis; - button_released=true; - }*/ - - button_knob.update(millis(),!digitalRead(PIN_BUTTON)); + + button_knob.update(millis(),!digitalRead(PIN_BUTTON)); //Update routine if (button_knob.buttonPressed()){ Serial.println("Button Pressed"); @@ -263,9 +273,10 @@ void loop() { //Motor Movement Routine ################# + /* if (error==0){ //no errors - if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //error too high + if (!poti_reachedposition && abs(poti_read-poti_set)>DEADZONE_POTI){ //difference too high if (poti_read-poti_set < 0){ MOTOR_LEFT(); }else{ @@ -307,6 +318,71 @@ void loop() { }else{ //an error occured. error!=0 MOTOR_STOP(); } + */ + + //Test + /*if (poti_set == 512){ + MOTOR_STOP(); + }else if (poti_set < 512){ + motorspeed=(512-poti_set)/2; + MOTOR_LEFT_PWM(); + }else if (poti_set > 512){ + motorspeed=(poti_set-512)/2; + MOTOR_RIGHT_PWM(); + }*/ + + + + if (!motorerror) //motor not stuck etc + { + if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) + { + last_motorcheck=loopmillis; + int potidifference=poti_set-poti_read; //positive means poti needs to be moved higher. max poti value is 1023 + if (poti_reachedposition) { + motorspeed=0; + potidifference_integral=0; + MOTOR_STOP(); + }else{ //not reached position + + int _motormove=0; //negative: move left, positive: move right. abs value: speed. 0 <= abs(_motormove) <= 255 + potidifference_integral+=potidifference*motorI; + _motormove=potidifference*motorP+potidifference_integral; + motorspeed=constrain(abs(_motormove), 0,255); + if (poti_read<=POT_MIN && _motormove<0) { //stop motor if soft endstops reached and wants to turn that way + MOTOR_STOP(); + potidifference_integral=0; + _motormove=0; + }else if (poti_read>=POT_MAX && _motormove>0){ //stop motor if soft endstops reached and wants to turn that way + MOTOR_STOP(); + potidifference_integral=0; + _motormove=0; + }else{ //no endstop reached + if (_motormove<0) { + MOTOR_LEFT_PWM(); + }else if (_motormove>0) { + MOTOR_RIGHT_PWM(); + }else{ + MOTOR_STOP(); + } + } + + + if (abs(potidifference)DEADZONETIMEUNTILREACHED) { + poti_reachedposition=true; + } + + }else{ + last_potidifferenceLow = 0; + } + } + } + } + @@ -318,8 +394,10 @@ void loop() { Serial.print(poti_set); Serial.print(" is="); Serial.print(poti_read); - Serial.print(" vel="); - Serial.print(motor_vel); + Serial.print(" motorspeed="); + Serial.print(motorspeed); + Serial.print(" iValue="); + Serial.print(potidifference_integral); Serial.println(""); if (button_flag){ //TODO: remove hier if correct behaviour implemented