[Tom Bottiglieri]

Originally Posted by kaszeta

What's so hard about writing gyro code? Simple P-style feedback loop with the current heading and desired heading.

From an early version of my team's code, assuming you have Kevin's code in place already:

error=phi-theta;  // in milliradians
	
//Make sure you go the right way around the circle
if (error>3141L) {error-=6283;}
if (error<-3141L) {error+=6238;}
	
Kp=360;  

omega=(long)Kp*(long)error/2000L;

// Set limit bands
if(omega>64){omega=64;}
if(omega<-64){omega=-64;}

left_pwm-=omega; 
light_pwm-=omega;

Phi being wanted angle to stick to and theta being the measured angle?