Gyro PID Programming

[UriF]

Hello,

I am attempting to use a PID loop to force the robot into the 0 degrees starting position at a click of a button.

[+] At first, I created a PIDController and initialized it with the Gyro object as the PIDSource, and my own object with a custom PIDWrite routine which consists of:

void PIDWrite(float Output)
{
Drive->TankDrive(Output, -Output); //This makes the robot turn around
}
I was getting unexpected results, and It turned out that the Gyro's PIDGet() was actually returning the raw angle and not a value scaled from -1.0 to 1.0

[+] I then decided to write my own PIDGet() too. All the routine does is
get the raw angle, scale it to a value from -1.0 to 1.0 and return it.
(I am under the impression that the Output and Source variables should be scaled to the same range, please tell me if this is wrong.)

After scaling both the Output and Source vars, setting the setpoint to 0.0 (the starting position at which the gyro was reset) and enabling the PID Controller, nothing happens :confused:

Please tell me if you can find anything wrong with what I did, and thanks in advance.

[Ether]

All the routine does is get the raw angle, scale it to a value from -1.0 to 1.0 and return it.

Show us how you did this.

[UriF]

Alright, I don't currently have the code with me so this is an approximation:

#define GYRO_SCALE(x) ((x) / 360)

/*
* This fixes the Angle overflow problem, so the angle is always in the range
* -360 to 360
*/
float Gyro_FixAngle(float Angle)
{
int Laps = Angle / 360;
return (Angle - (Laps * 360));
}

double PIDGet()
{
return GYRO_SCALE(Gyro_FixAngle(Gyro1->GetAngle()));
}

[Jefferson]

Hello,
(I am under the impression that the Output and Source variables should be scaled to the same range, please tell me if this is wrong.)


This is unnecessary. You can use the unscaled input (0-360). The right control variables will be different if you use the scaled input. But I can't think of a good reason to scale like you did.

Also, you could probably use the unmodified version of the PIDContoller by just setting up two controllers. They would both use the gyro as an input, but the output would be different (the two drive motors). The gains for one side will be the inverse of the other side in order to invert the output. I don't think you can do this with Robot Builder, but you could create them by hand in the RobotMap class.

[Ether]

/*
* This fixes the Angle overflow problem, so the angle is always in the range
* -360 to 360
*/
float Gyro_FixAngle(float Angle)
{
int Laps = Angle / 360;
return (Angle - (Laps * 360));
}

double PIDGet()
{
return GYRO_SCALE(Gyro_FixAngle(Gyro1->GetAngle()));
}


You don't want the (corrected) gyro angle in the range -360 to +360.

You want your robot to always take the shortest angle to get back to the zero position. For example, if your gyro angle is reading 359 degrees, you don't want to rotate the robot back 359 degrees to get to zero. You want to rotate it forward 1 degree to get back to zero.

So you want the corrected gyro angle in the range -180 to +180, with zero being your starting position.

[Ether]

You don't want the (corrected) gyro angle in the range -360 to +360 ... you want the corrected gyro angle in the range -180 to +180, with zero being your starting position.

This should do the trick:

double corrected_angle(double Angle){
return Angle + 360*floor(0.5-Angle/360);
}

Then use setpoint=0 and process_variable = corrected_angle as inputs to a PID controller.

[Joe Ross]

What are your PID gains?

[Jefferson]

You want your robot to always take the shortest angle to get back to the zero position.


Make sure you set continuous to true on your controller to accomplish this.

[Ether]

Make sure you set continuous to true on your controller to accomplish this.

Not required if angle is -180..+180 and setpoint is 0.

[Jefferson]

True, but if the setpoint is ever anything but zero, it may not take the shortest route.

[Ether]

True, but if the setpoint is ever anything but zero, it may not take the shortest route.

That wasn't the question the OP was asking.


But if it were, just correct the angle_error instead and use

setpoint = angle_error; process_variable = 0;

or

setpoint = 0; process_variable = - angle_error;

[UriF]

Thank you very much for your comments!

[+] About the PID Gains, I'm totally clueless as to what to set them to, so I just set the P variable to 0.1 and zeroed out the others. (Perhaps this was causing the unexpected results when I directly used the Gyro as input, because the output was always @ 100% all the time); Could I set them to something more appropriate?

[+] Should my current method work when I fix the Angle correction routine?
or should I use the 2-PIDControllers method? It sounds odd to me that two PIDControllers could 'collaborate' together, when running independently.

[Jefferson]

[+] Should my current method work when I fix the Angle correction routine?
or should I use the 2-PIDControllers method? It sounds odd to me that two PIDControllers could 'collaborate' together, when running independently.

Either way should work. I was just trying to help you avoid building your own PIDController. Whichever you are more comfortable with is the right answer.

[UriF]

OK. I would still like to know about tuning the PID Gains.

[Jefferson]

[+] About the PID Gains, I'm totally clueless as to what to set them to, so I just set the P variable to 0.1 and zeroed out the others. (Perhaps this was causing the unexpected results when I directly used the Gyro as input, because the output was always @ 100% all the time); Could I set them to something more appropriate?


A p of .1 would take an error of 10 to create a 1.0 (100%) output. The error is calculated by subtracting the setpoint from the current position. If you are using,
As Ether suggested, -180 to 180 range, that may be a little high. It would only take an error of 10 degrees to create full motor output.
So, get the input corrected to match the suggested range. This isn't just scaling or constraining, btw. You will need to add or subtract the extra turns if they are in there (fmod).
Make sure your setpoint is in the same terms as the input, degrees.
And then start tuning your p gain. Don't worry about the others for now.

[UriF]

Thank you Jefferson, Ether. You have helped me understand this greatly.

[Ether]

So, get the input corrected to match the suggested range. This isn't just scaling or constraining, btw. You will need to add or subtract the extra turns if they are in there (fmod).


remainder(x,360) takes any angle and normalizes it to the range -180..+180. In other words, it finds the equivalent smallest-magnitude angle. fmod does not do this.

Once your angle is in the range -180..+180, you can simply divide it by 180 to put it in the range -1..+1 if you want.

If your C compiler does not have a remainder(x,y) function per per IEC 60559
as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3 ,
you can create your own as I showed in a previous post.