From 947d00e7838ecf991c09630c72207b430a710026 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 26 Apr 2020 15:30:35 +0200 Subject: [PATCH] led improvements and try improving motor controller --- .../mixercontroller_w5100_pio/src/main.cpp | 199 ++++++++++++------ 1 file changed, 130 insertions(+), 69 deletions(-) diff --git a/controller/mixercontroller_w5100_pio/src/main.cpp b/controller/mixercontroller_w5100_pio/src/main.cpp index 796cfcd..4dfa9b9 100644 --- a/controller/mixercontroller_w5100_pio/src/main.cpp +++ b/controller/mixercontroller_w5100_pio/src/main.cpp @@ -29,6 +29,7 @@ void setMuteChannel(uint8_t i, boolean state); void publishCurrentSetVolume(); void publishAllStates(int pn, String pTopicname, boolean (*pgetBit) (uint8_t)); void changeRelaisByNumber(uint8_t pn, String pTopicPrefix, String pTopic, String pspayload, void (*psetXChannel) (uint8_t, boolean)); +float getSetVolume(); #define LEDPIN 9 //PB1 = D9 = Pin15 Adafruit_NeoPixel leds = Adafruit_NeoPixel(9, LEDPIN, NEO_GRB + NEO_KHZ800); @@ -48,13 +49,17 @@ EthernetClient ethClient; PubSubClient mqttClient; String mqttdevname="audiomixer/"; +bool flag_publishCurrentSetVolume=false; + long last_send=0; +#define MAXIMUMMQTTSENDINTERVAL 100 #define MQTTRECONNECTDELAY 5000 unsigned long last_mqttreconnectattempt=0; + //Serial long last_serialdebug=0; #define INTERVAL_SERIALDEBUG 200 @@ -84,6 +89,7 @@ uint16_t srbits=0; #include Encoder volEnc(PIN_ENCA,PIN_ENCB); float encoderMultiplier=4.0; +int volEncVel=0; //Servo stuff #define PIN_MOTOR_IN1 PD5 //to L293(pin2) Motor IN1 @@ -91,18 +97,22 @@ float encoderMultiplier=4.0; //#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; +int _motormove; #define PIN_POT A0 //A0 = PC0, reference potentiometer wiper #define DEADZONE_POTI 10 //maximum allowed error. stop when reached this zone #define POT_MIN 45 //minimum value pot can reach #define POT_MAX 950 //maximum value pot can reach -#define POTIFILTER 0.6 //0 to 1. 1 means old value stays forever -#define MAX_MOTOR_PWM 255 //0 to 255. Maximum pwm to output +#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever +#define MAX_MOTOR_PWM 192 //0 to 255. Maximum pwm to output 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 +int last_potidifference=0; +int potidifference=0; //gets overwritten at start of each motorcheck + //#define MOTOR_STOP(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,LOW); //#define MOTOR_LEFT(); srWrite(SRPIN_MOTOR_IN1,LOW); srWrite(SRPIN_MOTOR_IN2,HIGH); //#define MOTOR_RIGHT(); srWrite(SRPIN_MOTOR_IN1,HIGH); srWrite(SRPIN_MOTOR_IN2,LOW); @@ -117,24 +127,23 @@ boolean poti_reachedposition=true; //set to true if position reached. after that //Motorcheck long last_motorcheck=0; -#define INTERVAL_MOTORCHECK 100 //check motor movement every x ms +#define INTERVAL_MOTORCHECK 50 //check motor movement every x ms //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=4.0; -float motorI=0.5; +float motorP=2.0; +float motorI=0.05; +float motorD=1; float potidifference_integral=0; -#define MOTORI_ANTIWINDUP 200 //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 //Motor starts moving at about speed=80 long last_potidifferenceLow=0; #define DEADZONETIMEUNTILREACHED 250 //time [ms] poti read value has to be inside of deadzone to set reachedposition flag (and stop regulating) -//error -boolean motorerror=false; //Menu system uint8_t menu_mode=0; //0= volume set mode, 1=mute output selection, 2=output group selection @@ -327,10 +336,10 @@ void loop() { //Read Encoder to velocity "volEncVel" - int volEncVel=0; + int _volEnc=volEnc.read(); if (_volEnc!=0){ //encoder moved - volEncVel=_volEnc; + volEncVel+=_volEnc; volEnc.write(0); //reset value } @@ -342,20 +351,23 @@ void loop() { poti_set+=volEncVel*encoderMultiplier; //change poti set value poti_set=constrain(poti_set, POT_MIN,POT_MAX); poti_reachedposition=false; - publishCurrentSetVolume(); + flag_publishCurrentSetVolume=true; + volEncVel=0; //reset vel for next loop break; case 1: case 2: //mute or group selection menu_selectedChannel+=127; //offset to compensate negative values - menu_selectedChannel+=volEncVel; + menu_selectedChannel+=volEncVel/2; //every encoder detend is +-2 if (menu_selectedChannel<127){ menu_selectedChannel=127; //lower limit (0) - }else if (menu_selectedChannel > MENU_MAXCHANNEL+1) { //max channel and one extra for "nothing selected" + }else if (menu_selectedChannel > 127+MENU_MAXCHANNEL+1) { //max channel and one extra for "nothing selected" menu_selectedChannel=127+MENU_MAXCHANNEL+1; //upper limit } - menu_selectedChannel-=127; //take out offset - + + if (volEncVel/2 != 0) { //if value change was at least one detend + volEncVel=0; //reset vel for next loop + } break; } @@ -363,58 +375,93 @@ void loop() { } + if (flag_publishCurrentSetVolume && loopmillis-last_send>MAXIMUMMQTTSENDINTERVAL){ + flag_publishCurrentSetVolume=false; + last_send = loopmillis; + publishCurrentSetVolume(); + } + + //Motor Movement Routine ################# - if (!motorerror) //motor not stuck etc + if (loopmillis-last_motorcheck>INTERVAL_MOTORCHECK) { - 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; - potidifference_integral=constrain(potidifference_integral,-MOTORI_ANTIWINDUP,MOTORI_ANTIWINDUP); //constrain - _motormove=potidifference*motorP+potidifference_integral; - motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM); - 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(); - } - } - + last_motorcheck=loopmillis; - if (abs(potidifference)DEADZONETIMEUNTILREACHED) { - poti_reachedposition=true; - } - + last_potidifference = potidifference; //save last difference + 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 + _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 + _motormove=potidifference*motorP + potidifference_integral + motorD*(last_potidifference-potidifference); + motorspeed=constrain(abs(_motormove), 0,MAX_MOTOR_PWM); + + 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{ - last_potidifferenceLow = 0; + MOTOR_STOP(); } } + + if ( (potidifference>0 && last_potidifference<0) || (potidifference<0 && last_potidifference>0) ) { //different signs. potidifference has crossed 0. set value overshoot + potidifference_integral=0; //reset integral to stop further overshoot + } + + + /* + Serial.print(" diff="); + Serial.print(potidifference); + Serial.print(" iVal="); + Serial.print(potidifference_integral); + Serial.print(" dVal="); + Serial.print((last_potidifference-potidifference)*motorD); + Serial.print(" motormove="); + Serial.print(_motormove); + if (poti_reachedposition) { + Serial.print("!"); + } + Serial.println(""); + */ + + + + + + if (abs(potidifference)DEADZONETIMEUNTILREACHED) { + poti_reachedposition=true; + } + + }else{ + last_potidifferenceLow = 0; + } + } } + @@ -423,9 +470,15 @@ void loop() { switch(menu_mode) { case 0: //volume - for(uint8_t i=0;i last_serialdebug+INTERVAL_SERIALDEBUG){ last_serialdebug=loopmillis; @@ -478,8 +532,10 @@ void loop() { Serial.print(poti_set); Serial.print(" is="); Serial.print(poti_read); - Serial.print(" mspeed="); - Serial.print(motorspeed); + //Serial.print(" mspeed="); + //Serial.print(motorspeed); + Serial.print(" motormove="); + Serial.print(_motormove); Serial.print(" iVal="); Serial.print(potidifference_integral); if (poti_reachedposition) { @@ -487,6 +543,7 @@ void loop() { } Serial.println(""); } + */ if (loopmillis%5001==0) { //TODO: remove when working Serial.println(loopmillis); //alive print. for debugging @@ -595,7 +652,7 @@ void setMuteInt(uint8_t i) { srbits = ((i<