void Sample_Gyro(void)
{
ADCSRA |= (1 << ADSC);
while(!(ADCSRA & 0x10));
gyro_reading = ADCH; // Get the high 8 bits of the results [218]
// Every result is multiplied by 1,000 so that we do not have to result to using
// floating point math. 64 bit math on an 8 bit microcontroller is pretty bad.
if(gyro_reading > GYRO_NEUT) // If we are moving to the right, add to heading
{
current_heading += ((gyro_reading - GYRO_NEUT) * 2); // Multiply difference by two to account for sampling time
// 14800 is 360 degrees, make it so it can only be between 0 and 360 degrees
if(current_heading > 14800)
{
current_heading -= 14800;
}
}
else if(gyro_reading < GYRO_NEUT) // If we are moving to the left, subtract from heading
{
current_heading -= ((GYRO_NEUT - gyro_reading) * 2);
if(current_heading > 14800)
{
current_heading = (14800 - (65535 - current_heading));
}
}
}