Hello everybody,
What am I doing?
I'm building a boom barrier gate using a linear actuator -Usually it's a DC motor & gearboxes- and I'm using Arduino and BTS7960 motor driver. There's also a quadrature encoder attached to the output shaft that generates 800 pulses per revoltion.
General description of the code:
The program below behaves as expected, it waits for user input for the desired opening angle or the setPosition (eg. 200 pulses for 90 degrees clock-wise).
When I first run the program and the arm is down (closed) currentPosition is always zero.
The problem:
First I enter 200 and the gates moves up to 90 degress (Fine)
If I wanna close it, I enter 0 and the gate moves down to the original position (Also fine)
But the behavior changes when I try to increase the (I factor) gain to make it reach the setPosition faster:
I enter 200 and the gates moves up faster to 90 degress (Fine).
When I try entering 0 the serial monitor shows that it's going down at 255 PWM and the set position 0 BUT THE MOTOR DOESN"T BUDGE!
It only works when I try decreasing the set position in steps (First 160, then 120, etc all the way down to 0) I couldn't confirm it but it looks like it doesn't accept error values larger than 50?
I tried commenting out the emergency shut-down feature but that also didn't work.
Your thoughts?
//Encoder library stuff
#include <Encoder.h>
Encoder myEncoder(2,3); //YLW, GREEN(for 24vdc motor)
//BTS7960 connections
#define BTS7960_RPWM 9 //Right PWM pin BTS7960 bridge
#define BTS7960_LPWM 10 //Left PWM pin BTS7960 bridge
//PID Stuff:
double Kp = 2; //proportional gain
double Ki = .5; //integral gain
double Kd = 1; //derivative gain
double setPosition;
double setPositionOld;
double currentPosition;
double currentPositionOld;
double pwmSignalOutput;
double error;
double previousError;
double compundError;
double differenceError;
int errorWindow = 2; //tolerance to shut off the motor
int emergencyShutdown = 4;
int maxPWM_Output = 255;
int minPWM_Output = 20;
int timeDifference=0;
//Global variables
int userInput; //Used to store the user's desired distance from the serial window
unsigned long currentTime;
unsigned long oldTime;
unsigned long elapsedTime;
//Physical Inputs
int btnOpen = 5; //Navy
int btnClose = 6; //Green
//int limitSwitchUp = 7;
//int limitSwitchDown = 8;
void setup()
{
pinMode(BTS7960_RPWM,OUTPUT);
pinMode(BTS7960_LPWM,OUTPUT);
pinMode(btnOpen,INPUT_PULLUP);
pinMode(btnClose,INPUT_PULLUP);
Serial.begin(9600);
Serial.setTimeout(10);
Serial.println("Enter a value between -800 & +800"); //My particular rotary encoder generates 800 pulses per full revolution.
}
void loop()
{
//Wait for button presses
if (digitalRead(btnOpen) == LOW) openFunction();
if (digitalRead(btnClose) == LOW) closeFunction();
//Wait for user input
if (Serial.available()> 0)
{
userInput = Serial.parseInt();
setPosition = userInput;
currentTime = millis();
}
//Calls the main function
PID_Control();
}
void openFunction() //Physical buttons to move the arm up
{
Serial.println("Open button pressed");
while(digitalRead(btnOpen) == LOW)
{
analogWrite(BTS7960_RPWM,180);
}
analogWrite(BTS7960_LPWM, 0);
digitalWrite(BTS7960_RPWM, 0);
setPosition = myEncoder.read(); //To counter the effect when the PID controller wants to move the motor back to the last value stored in setPosition
}
void closeFunction() //Physical buttons to move the arm down
{
Serial.println("Close button pressed");
while (digitalRead(btnClose) == LOW)
{
analogWrite(BTS7960_LPWM,180);
}
analogWrite(BTS7960_LPWM, 0);
digitalWrite(BTS7960_RPWM, 0);
setPosition = myEncoder.read(); //To counter the effect when the PID controller wants to move the motor back to the last value stored in setPosition
}
void PID_Control()
{
currentPosition = myEncoder.read();
error = setPosition - currentPosition;
pwmSignalOutput =abs(error) * Kp;
pwmSignalOutput+=timeDifference * Ki;
pwmSignalOutput+=abs(setPositionOld - setPosition) * Kd;
timeDifference++;
if (pwmSignalOutput < minPWM_Output && pwmSignalOutput > 0) pwmSignalOutput = minPWM_Output; //saturates the output
if (pwmSignalOutput > maxPWM_Output) pwmSignalOutput = maxPWM_Output; //saturates the output
if (pwmSignalOutput < 0) pwmSignalOutput = 0; //saturates the output
else
{
if (abs(error) < errorWindow) //Shuts off the motor if the error is within accpeted tolerance
{
analogWrite(BTS7960_LPWM, 0);
digitalWrite(BTS7960_RPWM, 0);
pwmSignalOutput = 0;
timeDifference = 0;
error=0;
}
/*
if (abs(currentPositionOld - currentPosition) < emergencyShutdown && pwmSignalOutput == maxPWM_Output && timeDifference>50) //Overload protection
{
pwmSignalOutput = 0;
timeDifference = 0;
}
*/
else //time to move the motor up or down
{
if (error > 0) //Is the difference positive? Go up!
{
analogWrite(BTS7960_LPWM, 0);
delayMicroseconds(10);
analogWrite(BTS7960_RPWM, 0);
delayMicroseconds(10);
analogWrite(BTS7960_RPWM, pwmSignalOutput);
//User monitor feedback
Serial.print("Going up : ");
Serial.print(myEncoder.read());
Serial.print(" PWM : ");
Serial.print(pwmSignalOutput);
Serial.print(" Set Position : ");
Serial.print(setPosition);
Serial.print(" Elapsed: ");
Serial.println(millis()-currentTime);
}
if (error < 0) //Is the difference negative? Go down!
{
analogWrite(BTS7960_LPWM, 0);
delayMicroseconds(10);
analogWrite(BTS7960_RPWM, 0);
delayMicroseconds(10);
analogWrite(BTS7960_LPWM, 255 - pwmSignalOutput);
//User monitor feedback
Serial.print("Going down : ");
Serial.print(myEncoder.read());
Serial.print(" PWM : ");
Serial.print(pwmSignalOutput);
Serial.print(" Set Position : ");
Serial.print(setPosition);
Serial.print(" Elapsed: ");
Serial.println(millis()-currentTime);
}
}
}
setPositionOld = setPosition;
currentPositionOld = setPosition;
delay(15); //just in case
}
Thanks in advance,
Amin
