implement PI controller for motor movement

This commit is contained in:
interfisch 2020-04-01 13:01:07 +02:00
parent cd3d4237fe
commit 5f1665c2d7
1 changed files with 104 additions and 26 deletions

View File

@ -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 <Arduino.h> #include <Arduino.h>
#include <Adafruit_NeoPixel.h> #include <Adafruit_NeoPixel.h>
@ -67,6 +75,7 @@ float encoderMultiplier=4.0;
#define PIN_MOTOR_IN2 PD6 //to L293(pin7) Motor IN2 #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_IN1 1 //L293(pin2) Motor IN1 -- moved to atmega pin
//#define SRPIN_MOTOR_IN2 2 //L293(pin7) Motor IN2 -- 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 PIN_POT A0 //A0 = PC0, reference potentiometer wiper
#define DEADZONE_POTI 5 //maximum allowed error. stop when reached this zone #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 POT_MAX 1010 //maximum value pot can reach
#define POTIFILTER 0.8 //0 to 1. 1 means old value stays forever #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 int poti_read=0; //read value from poti
boolean poti_reachedposition=true; //set to true if position reached. after that stop turning boolean poti_reachedposition=true; //set to true if position reached. after that stop turning
@ -85,22 +94,30 @@ 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_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_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_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)) #define MOTOR_TURNING() (digitalRead(PIN_MOTOR_IN1) != digitalRead(PIN_MOTOR_IN2))
//Motorcheck //Motorcheck
long last_motorcheck=0; long last_motorcheck=0;
#define INTERVAL_MOTORCHECK 100 //check motor movement every x ms #define INTERVAL_MOTORCHECK 100 //check motor movement every x ms
int poti_read_last=0; //int poti_read_last=0;
int motor_vel=0; //analog read units per second //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 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 //#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 //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 //error
uint8_t error=0; boolean motorerror=false;
#define NOERROR 0
#define MOTORDIDNOTTURN 1
void setup() { void setup() {
pinMode(PIN_BUTTON,INPUT_PULLUP); pinMode(PIN_BUTTON,INPUT_PULLUP);
@ -108,6 +125,7 @@ void setup() {
pinMode(PIN_POT,INPUT); pinMode(PIN_POT,INPUT);
pinMode(SRLATCH, OUTPUT); pinMode(SRLATCH, OUTPUT);
pinMode(SRCLOCK, OUTPUT); pinMode(SRCLOCK, OUTPUT);
pinMode(SRDATA, OUTPUT); pinMode(SRDATA, OUTPUT);
@ -162,6 +180,10 @@ void setup() {
Serial.println("Ethernet disabled"); Serial.println("Ethernet disabled");
} }
poti_set=analogRead(PIN_POT);
Serial.print("Poti value="); Serial.println(poti_set);
Serial.println("Ready"); Serial.println("Ready");
last_send = millis(); last_send = millis();
@ -218,20 +240,8 @@ void loop() {
//Inputs ################################################### //Inputs ###################################################
poti_read=poti_read*POTIFILTER + (1.0-POTIFILTER)*analogRead(PIN_POT); //read poti 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()){ if (button_knob.buttonPressed()){
Serial.println("Button Pressed"); Serial.println("Button Pressed");
@ -263,9 +273,10 @@ void loop() {
//Motor Movement Routine ################# //Motor Movement Routine #################
/*
if (error==0){ //no errors 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){ if (poti_read-poti_set < 0){
MOTOR_LEFT(); MOTOR_LEFT();
}else{ }else{
@ -307,6 +318,71 @@ void loop() {
}else{ //an error occured. error!=0 }else{ //an error occured. error!=0
MOTOR_STOP(); 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)<DEADZONE_POTI) {
if (last_potidifferenceLow==0){
last_potidifferenceLow=loopmillis; //save millis first time entered deadzone
}
if (loopmillis-last_potidifferenceLow>DEADZONETIMEUNTILREACHED) {
poti_reachedposition=true;
}
}else{
last_potidifferenceLow = 0;
}
}
}
}
@ -318,8 +394,10 @@ void loop() {
Serial.print(poti_set); Serial.print(poti_set);
Serial.print(" is="); Serial.print(" is=");
Serial.print(poti_read); Serial.print(poti_read);
Serial.print(" vel="); Serial.print(" motorspeed=");
Serial.print(motor_vel); Serial.print(motorspeed);
Serial.print(" iValue=");
Serial.print(potidifference_integral);
Serial.println(""); Serial.println("");
if (button_flag){ //TODO: remove hier if correct behaviour implemented if (button_flag){ //TODO: remove hier if correct behaviour implemented