[MrRoboSteve]

In our 2012 robot, we used a simple proportional control system to turn the robot on heading. It uses a higher power level to turn close to the desired heading, and then lowers the power to avoid overrunning the desired heading. We also had a tolerance around the desired heading of a couple degrees where we considered the robot to be on heading.

All of the constants here are values you will need to determine experimentally.

Initialize at start of turn:

start_heading = current gyro heading
goal_heading = desired heading (calculated from your desired turn and the start heading)

Each loop:

turn_rate = 0 (this ranges from -1 to 1 and is the desired power to drive train)
current_heading = current gyro heading
angle_difference = current_heading - goal_heading

// angle_difference here might be positive or negative, indicating a turn in either direction

If (abs(angle_difference) > 10) then
turn_rate = .7
// when the angle difference is > 10 degrees, turn at .7 power.
Else if (abs(angle_difference) > 3 and abs(angle_difference) < 10)
turn_rate = .6
// when you are within 10 degrees of desired heading, turn at lower
// power to prevent overrunning the desired heading
Else
// Consider heading within 3 degrees of desired heading to be on heading
turn_rate = 0
End

// Motor inputs

// Depending on your motor configuration and how the gyro is mounted, you may need to reverse the motor controls.

If (angle_difference > 0) then
turn_left(turn_rate)
Else
turn_right(turn rate)
end