My code is Doing fine and going through, the problem comes when the turn loops starts. After going 90 degrees, it stops the loop and has the momentum to swing an extra 30-45 degrees. Also, if anyone sees something I am doing wrong with the loop, please point it out.
2 Likes
#pragma config(Sensor, port2, gyro, sensorVexIQ_Gyro)
#pragma config(Motor, motor1, lDrive, tmotorVexIQ, PIDControl, driveLeft, encoder)
#pragma config(Motor, motor3, lowclaw, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor4, claw, tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor, motor5, hDrive, tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor, motor6, rDrive, tmotorVexIQ, PIDControl, reversed, driveRight, encoder)
#pragma config(Motor, motor10, arm, tmotorVexIQ, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
float tError1 = 0;
float tError2 = 0;
float error = 0;
float forwardKp = 4;
float tDerivative2 = 0;
float tTotalError2 = 0;
float tPrevError2 = 0;
void forward(int distance,float kP,int velocity)
{
resetMotorEncoder(lDrive);
resetMotorEncoder(rDrive);
while(abs(getMotorEncoder(lDrive))<distance)
{
tError1 = (getMotorEncoder(lDrive)-getMotorEncoder(rDrive))*kP;
error = (distance-getMotorEncoder(lDrive))*forwardKp;
setMotorSpeed(lDrive,error-tError1);
setMotorSpeed(rDrive,error+tError1);
}
setMotorSpeed(lDrive,0);
setMotorSpeed(rDrive,0);
}
void turn(int heading,float kP,float kI,float kD)
{
tError2 = heading - SensorValue(gyro);
while(abs(tDerivative2)>0||abs(tError2)>1.5)
{
tError2 = heading - SensorValue(gyro);
tDerivative2 = tError2 - tPrevError2;
tTotalError2 = tTotalError2 + tError2;
setMotorSpeed(lDrive,-(tError2*kP)+(tTotalError2*kI)+(tDerivative2*kD));
setMotorSpeed(rDrive,(tError2*kP)+(tTotalError2*kI)+(tDerivative2*kD));
tPrevError2 = tError2;
}
}
//turn(heading,velocity,kP,kI,kD)
task main()
{
drawLine(5, 5, 10, 10); //To test how far the code goes and when.
forward(500,2,100);
drawLine(15, 10, 20, 15); //To test how far the code goes and when.
turn(-90,1,0,2);
drawLine(25, 15, 30, 20); //To test how far the code goes and when.
sleep(5000);
}
3 Likes
Hmmmm. Your code looks good. I don’t see anything after a cursory look at it.
When I was making my own PD turn loop I remember it did a lot of strange things too like spinning forever and stopping halfway and turning the other way.
If you are using global angle, which I suggest, make sure you calculate the tError correctly. That might be the cause of the issue
Here is my code for a Global Angle turn for reference:
void Turn_to(double target){ //currently a PD loop.
terror = target - getGyroHeading(gyro);
tlasterror = terror;
while(abs(terror) > 1.0||abs(terror-tlasterror)>0.2){
terror = target - getGyroHeading(gyro);
if (terror > 180){
terror = terror - 360;
}
else if (terror < -180){
terror = terror + 360;
}
tderivative = td * (terror - tlastError);
tlastError = terror;
if(terror == 0){
tderivative = 0;
}
power = tp * terror + td * tderivative;
setMotorSpeed(left,power*-1);
setMotorSpeed(right,power*1);
wait1Msec(30);
}
stopMotor(left);
stopMotor(right);
}Good luck in debugging your code!
6 Likes
So it looks like your kp value is set to 1 when turning. This doesn’t really slow down the motors very much as you end up just multiplying by 1 all the time. For kp you will want it to be less than 1 so that the output values sent to the motors continue to get smaller as error reaches 0. If we follow the math for setting speed your final output when error is just greater than 1.5 is rather high. This is also due to the fact that your kd value is 2. I’m pretty sure that defeats the point of the Derivative part. Take some time to tune your values more carefully there are several examples of how to do this on the forum and more mathematical ways online. Remember that a pid loop is only as good as the constants put into it.
7 Likes
Thanks for the advice! I got the code to work, but the while loop is still stopping prematurely and swinging too far forward.
void turn(float heading,float kP,float kI,float kD)
{
tError2 = heading - getGyroDegrees(gyro);
while(abs(tDerivative2)>0||abs(tError2)>0.5)
{
tError2 = heading - getGyroHeading(gyro);
if (tError2 > 180)
{
tError2 = tError2 - 360;
}
else if (tError2 < -180)
{
tError2 = tError2 + 360;
}
tDerivative2 = tError2 - tPrevError2;
tTotalError2 = tPrevError2/2 + tError2;
if(tError2 == 0)
{
tDerivative2 = 0;
}
setMotorSpeed(lDrive,-(tError2*kP)+(tTotalError2*kI)+(tDerivative2*kD));
setMotorSpeed(rDrive,(tError2*kP)+(tTotalError2*kI)+(tDerivative2*kD));
tPrevError2 = tError2;
}
setMotorSpeed(lDrive,0);
setMotorSpeed(rDrive,0);
}
1 Like
Can you please include your PID constants when sending your code (from now on)?
The reason it is stopping prematurely is because you are telling it to stop when the derivative is still quite high. You should probably have it stop when the value is greater than something like 0.3, instead of 0.
One more thing, this chunk of code:
if(tError2 == 0)
{
tDerivative2 = 0;
}
is stopping the loop prematurely every time, no matter how low the minimum derivative is. Taking out these lines and making the minimum derivative smaller should fix your problem.
6 Likes
Here.
turn(-90,0.3,0.1,1.4);
//turn ( to heading , kP , kI , kD )
2 Likes
I would recommend to use just PD first. Integral is tricky and a PD turn is accurate enough
4 Likes
Thanks! My autonomous is still in progress, but I get my points 80% of the time now instead of 25%.
2 Likes
I just want to point out that the constants are mainly based on the unit convention you’re using and it doesn’t make sense to suggest gain values without empirically testing the system. Example, (assuming you’re outputting voltage from the PID controller):
If I wanted to have my PID control how many revolutions the robot makes (one complete circle is one revolution), then my PID constants would need to be much much larger since the error is calculated in terms of revolutions.
Whereas controlling how many degrees the robot turns, the constants would need to be much smaller relative to revolution constants because the numbers I’d be dealing with would likely be one or two orders of magnitudes greater than the revolutions while the motor still accepts voltage.
The point being is that the constants depend mainly on two things: the units you want the controller output to be in and the units the controller input are in. For whatever reason, most people use the same units everywhere so you just assume someone’s constants are off if they don’t look like yours. But, it doesn’t always make sense to use the same units everywhere.
The input:output scales aren’t 1:1 so you must compensate with the gains.
TL;DR get rid of your bias against different controller gains when you’re tuning the controller and make sure the constants make sense for the units you use
7 Likes
This is a fair point. I really didn’t spend too much time looking at units because most people use degrees and rpm or voltage. I don’t know too many people who use revolutions but hey they’re out there. However in op’s original code his gains are set to whole integers including 1 and 0. That combined with their problem of overshoot due to a preemptive loop exit caused me to believe that their final output was set too high because their loop exits only 1.5 degrees from target. But yes everyone’s gains are different.
1 Like
I wasn’t necessarily saying you’re wrong. I just wanted to make it abundantly clear that just choosing arbitrary values for PID constants isn’t as effective as knowing the exact purpose of the constants. The constants not only tune the controller to perform well for each unique system but they also scale the input units to roughly the desired units (though constants are unit-less which might be a little confusing).
5 Likes
Also, to add to what @mvas said, loop delay time is an implicit argument to the calculation of derivative and integral components.
For example, if you change delay time from 30 to 10 msec, then your derivative value is expected to be about 1/3 of the original value (because motor had 1/3 of the time to move from old position to new) and your integral value is going to be 3x of the original (because you will add the error value to the integral accumulator three times as often).
So, to keep everything consistent, you have to account for actual delay time in your final power calculations.
4 Likes