implement PI controller for motor movement
This commit is contained in:
parent
cd3d4237fe
commit
5f1665c2d7
|
|
@ -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 <Adafruit_NeoPixel.h>
|
||||
|
|
@ -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)<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(" 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue