Hello everyone. I need help with my code.I am trying to control the position of this motor using arduino Mega and mega moto shield.
My encoder works fine. I am having trouble with PID and converting the PID to PWM (0-255).
I think something is wrong in my loop() since my motor keeps on spinning(it slows down a bit) but never stops at a given input of 1025 counts. Please have a look at it and suggest any changes required. Thanks.
#define encoder0PinA 2
#define encoder0PinB 18
int EnablePin = 8;
int PWMPin1 = 11;
int PWMPin2 = 3;
volatile signed int encoder0Pos = 0;
unsigned long LastTime;
float Input=1025;
float PID_Output, Scaled_PID1, Scaled_PID;
float ErrorSum,ErrorDiff,Error,LastError;
float kp=600;
float ki=0;
float kd=0;
int SampleTime = 1000;
int TimeChange;
unsigned long Now;
void setup()
{
pinMode(encoder0PinA, INPUT);
pinMode(encoder0PinB, INPUT);
pinMode(EnablePin, OUTPUT);
pinMode(PWMPin1, OUTPUT);
pinMode(PWMPin2, OUTPUT);
attachInterrupt(0, doEncoderA, CHANGE);
attachInterrupt(1, doEncoderB, CHANGE);
Serial.begin(9600);
}
//void rotateCCW()
//{
//digitalWrite(EnablePin, HIGH);
//analogWrite(PWMPin2,0);
//analogWrite(PWMPin1,40);
//delay(20);
//}
//void rotateCW()
//{
//digitalWrite(EnablePin, HIGH);
//analogWrite(PWMPin2,40);
//analogWrite(PWMPin1,0);
//delay(20);
//}
void motorSTOP()
{
digitalWrite(EnablePin, LOW);
delay(20);
}
void doEncoderA(){
// look for a low-to-high on channel A
if (digitalRead(encoder0PinA) == HIGH) {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == LOW) {
encoder0Pos++; // CW
}
else {
encoder0Pos--; // CCW
}
}
else // must be a high-to-low edge on channel A
{
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinB) == HIGH) {
encoder0Pos++; // CW
}
else {
encoder0Pos--; // CCW
}
}
//Serial.println(encoder0Pos);
}
void doEncoderB()
{
// look for a low-to-high on channel B
if (digitalRead(encoder0PinB) == HIGH)
{
// check channel A to see which way encoder is turning
if (digitalRead(encoder0PinA) == HIGH)
{
encoder0Pos++; // CW
}
else
{
encoder0Pos--;; // CCW
}
}
// Look for a high-to-low on channel B
else {
// check channel B to see which way encoder is turning
if (digitalRead(encoder0PinA) == LOW) {
encoder0Pos++; // CW
}
else {
encoder0Pos--; // CCW
}
}
}
void PID()
{
Now = millis();
TimeChange = Now - LastTime;
if(TimeChange >= SampleTime)
{
Error = encoder0Pos - Input;
ErrorSum = ErrorSum + Error;
ErrorDiff = Error - LastError;
PID_Output = kp * Error + ki * ErrorSum + kd * ErrorDiff;
LastError = Error;
LastTime = Now;
}
}
void SetTunings(double Kp, double Ki,double Kd)
{
double SampleTimeInSec = ((double)SampleTime)/1000;
kp=Kp;
ki=Ki * SampleTimeInSec;
kd=Kd / SampleTimeInSec;
}
void SetSampleTime(int NewSampleTime)
{
if (NewSampleTime > 0)
{
double ratio = (double)NewSampleTime/ (double)SampleTime;
ki *= ratio;
kd /= ratio;
SampleTime = (unsigned long)NewSampleTime;
}
}
void ReadData()
{
Serial.print("EncoderOutput:");
Serial.println(encoder0Pos);
Serial.print("Error");
Serial.println(Error);
Serial.print("PID_Output");
Serial.println(PID_Output);
Serial.print("Scaled_PID");
Serial.println(Scaled_PID);
Serial.println(" ");
}
void loop()
{
PID();
ReadData();
Scaled_PID1 = PID_Output/1000;
Scaled_PID= Scaled_PID1-100;
if (error>0)
{
digitalWrite(EnablePin, HIGH);
analogWrite(PWMPin2,0);
analogWrite(PWMPin1,Scaled_PID);
delay(20);
}
else
{
digitalWrite(EnablePin, HIGH);
analogWrite(PWMPin2,Scaled_PID);
analogWrite(PWMPin1,0);
delay(20);
}
}
Why is Input a float? You aren't really expecting to get 3.14159 encoder ticks, are you?
If you'd take the time to LOOK at the PID code, before making insulting comments, you'd see that it uses floats for all inputs and outputs, for the simple reasons that it NEEDS the dynamic range, and it works with both analog and digital feedback and process output values. It will work just as well controlling temperature using a thermistor for feedback as it will controlling a servo motor with digital encoder for feedback.
If you'd take the time to LOOK at the PID code, before making insulting comments
Well, of course I did that.
you'd see that it uses floats for all inputs and outputs, for the simple reasons that it NEEDS the dynamic range
What the heck does "dynamic range" mean? The encoder counts using integer arithmetic. The number of encoder ticks is an int. Storing the value in a float does not provide ANY benefit.
The position of the motor is at the correct number of ticks or it isn't. If it is not, the discrepancy is a whole number of ticks SINCE THE ONLY MEANS YOU HAVE FOR MEASURING THE DISCREPANCY USES INTEGERS.
It doesn't make sense to use floats and the resulting ssslllooowww computations when floats are not necessary.
The input is integer. The output is a PWM value - another integer.
PaulS:
Well, of course I did that.
What the heck does "dynamic range" mean? The encoder counts using integer arithmetic. The number of encoder ticks is an int. Storing the value in a float does not provide ANY benefit.
The position of the motor is at the correct number of ticks or it isn't. If it is not, the discrepancy is a whole number of ticks SINCE THE ONLY MEANS YOU HAVE FOR MEASURING THE DISCREPANCY USES INTEGERS.
It doesn't make sense to use floats and the resulting ssslllooowww computations when floats are not necessary.
The input is integer. The output is a PWM value - another integer.
Get over it.
Well, you're such an expert! So you believe you can write a usable PID using only 16-bit math? Then I suggest you do us all a big favor, and write it. Should only take you a few minutes. When you get it working as well as the standard floating point version, PLEASE post it here. I'd LOVE to see it!
I won't hold my breath... The fact that you don't even understand the concept of "dynamic range" as it applies to numerical computation tells me you don't understand how a PID works, and the possible values the intermediate computations can take. I'll give you a hint - a PID that has only integer coefficients, would be 100% completely and totally USELESS on any real-world system. But I'm sure you already know that....
I'll give you a hint - a PID that has only integer coefficients, would be 100% completely and totally USELESS on any real-world system. But I'm sure you already know that....
The coefficients should be floats. The input, output, and time intervals are not. The input is the current encoder position. I have yet to see an encoder that can tell you that 3.14159 steps have been taken.
The setpoint is a number of encoder ticks. 6.28318 doesn't seem like a reasonable number of ticks to ever reach.
The result of the PID computation is being used to set the PWM value for a pin to control the speed of the motor. The analogWrite() function takes an integer value.
PaulS:
The coefficients should be floats. The input, output, and time intervals are not. The input is the current encoder position. I have yet to see an encoder that can tell you that 3.14159 steps have been taken.
The setpoint is a number of encoder ticks. 6.28318 doesn't seem like a reasonable number of ticks to ever reach.
The result of the PID computation is being used to set the PWM value for a pin to control the speed of the motor. The analogWrite() function takes an integer value.
It's clear you just like to argue.... I made it crystal clear in my first post this PID can be used to control ANYTHING!! Many systems WILL have floating point input values. Would you have four completely separate PID libraries, one for integer input/integer output, one for integer input/float output, one for float input/integer output, one for float input/float output, just to satisfy your prejudice against putting integer values into float variables, even though that conversion WOULD happen inside the PID to do the calculations?
Would you have four completely separate PID libraries, one for integer input/integer output, one for integer input/float output, one for float input/integer output, one for float input/float output, just to satisfy your prejudice against putting integer values into float variables, even though that conversion WOULD happen inside the PID to do the calculations?
Of course not. What happens INSIDE the library is it's concern. Mine is using sensible data types to store the data I have.
I'm still waiting to see some debug output from OP to see what the real problem is.
Can you have a look at the void loop()? I need to know whether I am calling the PID() at right time. I also have doubts about scaling PID_Output to PWM.
The problem is after running the code motor keeps on spinning, it doesn't stop.
PaulS:
Of course not. What happens INSIDE the library is it's concern. Mine is using sensible data types to store the data I have.
I'm still waiting to see some debug output from OP to see what the real problem is.
PaulS:
Of course not. What happens INSIDE the library is it's concern. Mine is using sensible data types to store the data I have.
I'm still waiting to see some debug output from OP to see what the real problem is.
You keep claiming to be very familiar with the PID, but it is painfully clear you are not. Take a moment to look at the constructor for the PID class, then tell me what you would suggest the OP do differently....
then tell me what you would suggest the OP do differently....
I've already made that abundantly clear. Store integral values in integral variables. I don't know how I can make that clearer to you.
OP: Look at your values for Scaled_PID. The error keeps getting larger, and the PID process is telling you that you need to correct it. But Scaled_PID is not changing.
You apply Scaled_PID to one motor (speed control pin) if error is positive and to a different motor (speed control pin) is the error is negative. That doesn't make sense. How many motors do you have? What are they doing?
PaulS:
I've already made that abundantly clear. Store integral values in integral variables. I don't know how I can make that clearer to you.
OP: Look at your values for Scaled_PID. The error keeps getting larger, and the PID process is telling you that you need to correct it. But Scaled_PID is not changing.
You apply Scaled_PID to one motor (speed control pin) if error is positive and to a different motor (speed control pin) is the error is negative. That doesn't make sense. How many motors do you have? What are they doing?
CLEARLY, you have not bothered to LOOK at the constructor for PID.... If he changes his encoder value to int, the PID WILL NOT WORK! So, let me put thin in language you can understand: Please get off your high horse, and stop giving stupid, wrong advice!
CLEARLY, you have not bothered to LOOK at the constructor for PID....
I don't need to.
If he changes his encoder value to int, the PID WILL NOT WORK!
Horseshit. If the function expects a float, and it is called with an int, the int will be promoted to a float and the function will never know that it was passed an int.
So, let me put thin in language you can understand: Please get off your high horse, and stop giving stupid, wrong advice!
I think you are the one that needs to follow that advice.
But, I'm done here. YOU haven't offered any advice about OPs problem. So I expect that you will start doing so, and help OP solve the problem, Mr. Fucking Expert.
To the OP, is the kP set at 600? and the KI, is what? You may not even need the kD. Try a more conservative setting of kP = 0.03, kI = 0.001.
As for the float vs the integer, in my logic why use a data type that requires unnecessary processor work. Work = Time. For a PID control algorithm, time is everything. I use a simple int based PID to control a machine that generates around 30,000 psi, or 2000 bar. The only floats are the P, I, D, and the output.
I have made the changes suggested by you guys(float to int ). I think my PID() is correct. Can you guys have a look at this part, whether it is correct or not? (Just want to make sure before I start tuning it)