Gyro PID Programming

[Ether]

Originally Posted by UriF

/*
 * 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.