Attempting to Drive Straight with Gear Tooth Sensors and Gyroscope

[Lynx34]

Team 1716 is attempting for the first year to use feedback (specifically gear tooth sensors and a gyroscope) in order to drive perfectly straight (for either hybrid mode or when the joysticks are in close proximity in teleoperated, assuming that the driverwants to go straight).

We are using easyC and have tried using a proportional adjustment set in a loop to alter the motor outputs in a tank drive setup. Specifically, when the joysticks are within 5 values of each other, the the "drivestraight" subroutine is activated. At this time the gear tooth sensors start counting. The following code is a sample of the algorithm.

if (gt_left > gt_right) // compares the gear tooth sensors
{
left_motor = 127 + ((left_joystick - 127) / (gt_left / gt_right));
right_motor = right_joystick;
}
else if (gt_right > gt_left)
{
right_motor = 127 + ((right_joystick - 127) / (gt_right / gt_left));
left_motor = left_joystick;
}

In this subroutine, the gear tooth sensors are re-zeroed every half second. Our current result is a large arcing curve to the right (though sometimes when we've changed the code its consistently to the left) when going forward, but a nearly perfect straight drive when going in reverse.

Of course, our worst case scenario is to simply switch poles on our joysticks and drive backwards, but with the way we're distributing weight, we'd rather not.

We've done some reading on PID loops as a solution to such a problem, and as far as we can tell, we have the "P" part down, but cannot quite comprehend the I or D sections as to what they do or how to implement them.

We also have a gyroscope mounted that we tried to include but with little success. If it helps anyone help us, it is mounted and fully operational, so it can be used.

We appreciate any help given to us. Also, we apologize for using easyC.