[Mike Bortfeldt]

Lynx34,

A fairly simple method to make your robot drive straight can be done with minimal changes in your code. Since you are already counting gear tooth pulses when running your "drive straight" code, just take the difference between your left & right and add/subtract that number from your base PWM value (which would be your joystick value)

When you are in your drive straight code, use this:

if (left_joystick > 127)
{
left_motor = left_joystick;
right_motor = left_joystick + (gt_left - gt_right);
}
else
{
left_motor = left_joystick;
right_motor = left_joystick - (gt_left - gt_right);
}

You can try using a multiplier (or divider) on the (gt_left - gt_right) depending on how quickly you are counting pulses, and you would also have to make sure the right_motor doesn't go below 0 or above 255. We used this fairly successfully in 2004 when we wanted to drive straight in autonomous and used gear tooth sensors. What this code will do is cause your robot to curve very slightly to the right/left at the beginning of the "drive straight" code until the power to the motors evens out the speed. You only want to reset the gt_left and gt_right when you are no longer using the "drive straight" code (instead of every 1/2 second), which in your case would be when the left and right joystick values are more than 5 apart. (Also notice that I am ignoring the right_joystick in the above code. This is on purpose as I am using the left_joystick as the base speed since by this time we know they are less than 5 apart).

There are, of course, many other ways to drive straight, but this may be good enough for you without going into PID loops, gyros and the like.

Mike