# Redshift Project Depot

## FRC 2016 (Stronghold) => Robot Software => Topic started by: Louis L on February 05, 2016, 08:31:12 PM

Title: Using the gyro to turn the robot
Post by: Louis L on February 05, 2016, 08:31:12 PM
If you're using the gyro by directly hooking it up to the drivetrain and relying on the WPIlib to do the heavy lifting, that's great. But what if you're doing some independent coding using the gyro?

Below is my transcription from my chicken scratched paper notes. I'm putting it here as much for my benefit as anyone elses'

<TBD>
Title: Re: Using the gyro to turn the robot
Post by: Louis L on February 05, 2016, 10:09:12 PM

<had to take a break and watch Monty Python and the Holy Grail!>

So this code sniplet is what I used for a function that turns the robot pointed in an arbitrary direction to another arbitrary direction. The directions are all referenced from the initial compass heading where 0 is straight ahead.

Want = the direction we want to go to
At = the direction we're pointed at

// normalize to +/- headings where '+' is a right turn and '-' is a left turn
if (Want < At)
Want += 360
Diff = Want - At

// Adjust to +/- 180
if (Diff > 180)
Diff -= 360
if (Diff < -180)
Diff += 360

// Move wheels
if (Diff < -margin)
turn left
if (Diff > +margin)
turn right