Hey guys, I got some help earlier with my P controller and that was awesome! Thanks so much to the guys that did. The P controller was very unstable and unpredictable. Because of that I decided to design a full PID controller. I looked at quite a few guides and came up with a working PID controller, its just I have a few problems that I couldn’t find on the forums.
So my first problem is that the robot arm connected to the PID is very touchy, for example 1 press of the trigger = 3\4 of the arm raised. Now I believe this is just because of tuning, but i’ve found that no matter how I tune it the arm doesn’t raise above this 3\4 of the arm’s total height. I’m thinking this could because of my potentiometer because my input clamp raised all the way to the full height before.
My second problem is that the robot works but as soon as someone lightly taps the trigger to raise the lift and it only raises halfway it doesn’t work any more (the motors just won’t raise). Obviously this could be just from coincidence and over heating but i’m wondering if it could be a programming issue.
Finally in my program you"ll notice I have used a clamp for the error. Now I tried the program with out it because I figured I really didn’t need it because I had the integral in a clamp. Any ways it doesn’t work without the clamp on error. I’m wondering if there is a reason for that.
So here is the code below.
#include "Main.h"
void OperatorControl ( unsigned long ulTime )
{
int error;
int btn;
int pot;
int btn2;
int input;
int iterm;
int output;
int dterm;
int lasterror;
int Kp = 0.5;
int Ki = 0.2;
int Kd = 0.3;
input = 1 ;
while ( 1 ) // Insert Your RC Code Below
{
btn = GetJoystickDigital ( 1 , 6 , 2 ) ; // Gets the value for 6, 2
btn2 = GetJoystickDigital ( 1 , 6 , 1 ) ; // Gets the value for 6,1
pot = GetAnalogInput ( 1 ) ;
if ( btn == 1 ) // Clamp function for input
{
input = input + 1 ;
if ( input > 1023 )
{
input = 1023 ;
}
}
else if ( btn2 == 1 )
{
input = input - 1 ;
if ( input < 0 )
{
input = 0 ;
}
}
error = (input - pot) ; // Proportional
if ( error > 127 )
{
error = 127 ;
}
else if ( error < -127 )
{
error = -127 ;
}
if ( error == 0 )
{
iterm = 0 ;
}
iterm = iterm + error ;
dterm = lasterror - error ;
lasterror = error ;
if ( iterm > 127 ) // If Iterm > outmax
{
iterm = 127 ;
}
else if ( iterm < -127 ) // If Iterm < outmin
{
iterm = -127 ;
}
output = (0.45 * error) + (0 * iterm) + (0.1 * dterm) ;
SetMotor ( 2 , - output ) ; // Motor outputs
SetMotor ( 4 , output ) ;
SetMotor ( 3 , output ) ;
SetMotor ( 5 , - output ) ;
Arcade2 ( 1 , 3 , 1 , 1 , 10 , 1 , 1 ) ; // Controls movement of robot
}
}
I’m not sure how EasyC handles variable types, but one thing that jumps out at me is that you appear to have declared error and output as integers (int). But in calculating output, you have the following:
output = (0.45 * error) + (0 * iterm) + (0.1 * dterm) ;
which I think would require output to be a float, unless you want values to get truncated.
I’m not sure about this because I don’t know EasyC, but it might be something worth looking into.
The other thing you might have a look at: your input variable is going to get increased fairly rapidly because of the speed at which the While loop runs, so I think even the most brief touch of your button will cause the input variable to max out in either one direction or the other. In other words, I think it would take lightning-fast reflexes to get a button-driven input within an intermediate range. The Cortex runs these While loops extremely fast, so I think it would be beyond human capability to get values in the middle.
FullMetalMentor has the right idea, the biggest problem with the code is that there is no delay at the end of the while statement. This means that the button presses cause the “input” variable to rapidly change from 0 to 1023 etc. It also means the the PID math is not working, remember we always use a simplified for of the PID math, it should include a “delta time” factor, but we generally fold that into the constants (ie. Kd x deltaT = Kd’ ). Now obviously the loop does take some finite time to execute but it is so close to 0 that the calculation of the differential term effectively becomes zero (diff = dError/dT, but dT is (almost) 0 and dError will also be (almost) 0 ).
Clipping the error to +/- 127 is not necessary, however, clipping the output sent to the motor is necessary (EasyC complains about motor values out of bounds).
Input will almost certainly need to be clipped at less than 1024 and above 0, the pot should not be operating near the ends of it range, perhaps a range of 100 to 900 would be better.
Here is a slightly reorganized version (untested) of the code that may work a little better. It still needs tuning, ie. the values of Kp, Kd etc. may not be appropriate.
void OperatorControl ( unsigned long ulTime )
{
int error;
int btn;
int pot;
int btn2;
int input;
int iterm;
int output;
int dterm;
int lasterror;
int Kp = 0.5;
int Ki = 0.2;
int Kd = 0.3;
input = 100;
while ( 1 ) // Insert Your RC Code Below
{
btn = GetJoystickDigital ( 1 , 6 , 2 ) ; // Gets the value for 6, 2
btn2 = GetJoystickDigital ( 1 , 6 , 1 ) ; // Gets the value for 6,1
// check values returned by the pot
pot = GetAnalogInput ( 1 ) ;
// Running the loop at 100Hz (Wait(10)) means it will take
// 10 seconds to change from 0 to 1023
// if the increment.decrement is 1
// perhaps use around 4 to speed things up
if ( btn == 1 ) // Clamp function for input
{
input = input + 4 ;
if ( input > 900 )
{
input = 900 ;
}
}
else if ( btn2 == 1 )
{
input = input - 4 ;
if ( input < 100 )
{
input = 100 ;
}
}
error = (input - pot) ; // Proportional
if ( error == 0 )
{
iterm = 0 ;
}
iterm = iterm + error ;
dterm = lasterror - error ;
lasterror = error ;
if ( iterm > 127 ) // If Iterm > outmax
{
iterm = 127 ;
}
else if ( iterm < -127 ) // If Iterm < outmin
{
iterm = -127 ;
}
// calculate motor drive
output = (0.45 * (float)error) + (0 * (float)iterm) + (0.1 * (float)dterm) ;
// clip output
if ( output > 127 )
{
output = 127 ;
}
else if ( output < -127 )
{
output = -127 ;
}
SetMotor ( 2 , - output ) ; // Motor outputs
SetMotor ( 4 , output ) ;
SetMotor ( 3 , output ) ;
SetMotor ( 5 , - output ) ;
Arcade2 ( 1 , 3 , 1 , 1 , 10 , 1 , 1 ) ; // Controls movement of robot
// delay so that PID works
Wait(10);
}
}
Hey guys thanks so much! Fullmetalmentor, sorry I disn’t get back to you as soon as possible, I was busy with school. Thank you for your suggestion about the output and the pid varuables being changed to float values. I had no idea that as int they would truncate. I had a feeling the while loop was going to fast for the input and thus wouldn’t be humanly possible to adjust inbetween. I just wasn’t sure how to caculate how fast it was going and then calculate what the nessescary wait command would be. Thanks to both of you for clearing that up. Jpearman, thanks for explaining to me about merging in the delta t. I just thought that some how dt was just merged into the constant through means of tuning but this now makes much more sense. It made me realize why the controller wasn’t working in the first place! I will test this all tomorrow and report back with the results. Jpearman, I hope you don’t mind me using the reorganized code. If you don’t i’ll be sure to give you full credit for the adjustments.
It’s your code, I made only minor changes. I posted for your benefit so use (and improve 
) as you wish.
Thanks jpearman :). Just a follow up post to the code. Had to make a few adjustments like increase the range of input to 1150 but thats just specific to our robot. Then additionally because of that I increased the input = input to + 5 to get through the 1150 faster. Then from there it’s just going to need some good old fashioned tuning. But the code was quite successful and works quite well with good tuning. Thanks so much to jpearman and fullmetalmentor. Couldn’t have finished this without you guys… Seriously.
) as you wish.