// gyro-guided skid steer         (1/26/2011 revB  Ether)

// can be used with any skid-steer vehicle with separate control of right and left wheel speeds (eg 6WD)

// vehicle turns and goes in the commanded direction at the commanded speed



// driver interface for single 2-axis joystick

// Let Xj, Yj be the outputs of a single 2-axis joystick.  Then:


speed_command = max(abs(Xj),abs(Yj));

direction_command = atan2(Xj,-Yj)*(180/pi);  // angle in degrees CW from "straight downfield".
                                             // warning Excel users!  atan2() per "standard" definition


//find the shortest forward-driving rotation required to get to the desired angle:

angle_error = direction_command - gyro_angle; // gyro must also be in degrees CW from "straight downfield"

while(angle_error>180)angle_error-=360;

while(angle_error<-180)angle_error+=360;



// un-comment the following two lines of code if you don't want the robot to do a 180 when you try to back up:

// if(angle_error>90){angle_error-=180; speed_command = -speed_command;}

// else if(angle_error<-90){angle_error+=180; speed_command = -speed_command;}



// form the rotation rate command:

clockwise_command = K*angle_error; // K is a tuning constant.  start with K=0.01 and increase from there.

if(clockwise_command)>2 clockwise_command=2;

else if(clockwise_command)<-2 clockwise_command=-2;



// calculate the left and right wheel speeds:

left_wheel_speed  = speed_command + clockwise_command;

right_wheel_speed = speed_command - clockwise_command;



// limit the wheel speed commands to +/-1:

if(left_wheel_speed)>1 left_wheel_speed = 1;

else if(left_wheel_speed)<-1 left_wheel_speed = -1;

if(right_wheel_speed)>1 right_wheel_speed = 1;

else if(right_wheel_speed)<-1 right_wheel_speed = -1;