Attempting to Drive Straight with Gear Tooth Sensors and Gyroscope

Technical Programming
1

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.

2

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

3

Thank you very much… I’m going to try this out tomorrow as we’ve already cleaned up for the night. My guess is we’ll be playing around with multiplier constants on that (gt_left - gt_right) quantity and I have this itching feeling that your idea is too simple to work, but if you’ve used it before I figure it’s worth a shot.

4

I struggled with using sensors to drive straight earlier this year, so I understand what you’re going through. What you probably want is PD control, since integral control isn’t really necessary for velocity.

Since I am not educated in the ways of easyC, I will have to explain this in English and pseudocode instead.

PD control is not straightforward when it comes to design, but it is easy to see how it works once it is implemented. The first thing to do is convert units. Get your robot up on blocks and find the maximum counts your sensors will read per slow loop (check forwards and backwards, both wheels/sides). This maximum count will be the max speed of the robot (ticks per slow loop is the unit).

MaxTicks = max number of encoder ticks per slow loop

Now convert the joystick values to be in encoder ticks per slow loop…

p1_y is the joystick value for this example

Vdes = desired velocity in encoder ticks per slow loop

Vdes = (p1_y - 127) * MaxTicks / 127

Subtracting 127 changes the neutral point to zero. Multiplying by MaxTicks/127 converts units.

Now that you have the desired speed for the wheels in units you can use, PD control can be applied.

There are two parts to velocity PD control: Proportional and Derivative.

Proportional control increases the speed based on the difference between desired and actual speed.

Ticks = amount of encoder ticks recorded since last slow loop

Error = Vdes - C

Derivative control dampens the overshoot caused by proportional control (it takes more time to accelerate the robot than it does to build up a large error).

ErrorPrev = previous value of Error

DeltaError = Error - ErrorPrev

(set ErrorPrev = Error after calculating DeltaError)

Now that you have the components of the PD routine, you can assemble them.

pwm01 = the motor whose speed you are setting

Kp = Proportional constant of proportionality
Kd = Derivative constant of proportionality

The constants above should be tweaked until everything works, and DEFINITELY put your robot up on blocks until it stops doing weird things. Don’t be surprised if you hear some nasty sounds from your gearboxes when you inadvertently use a bad set of values, just disable the robot and try again. I got away with using Kp = 4 and Kd = 1, but your values probably will be different.

pwm01 = pwm01 + Error / Kp + DeltaError / Kd

Finally, make sure you don’t let the value for the motor get outside of the range you want it to have, since this can cause undesired problems.

Another thing to watch out for: I don’t know how picky EasyC is about variable types, but MPLAB/C is VERY picky, and I left out all of the typecasting necessary to make this work.

5

usbcd36,

I like your solution as well; it seems to have a bit more precision, but I have a few questions. I have a few guesses as to what a ‘slow loop’ is, but I’m not real positive, could you clear up a bit on that? I’m a programmer by experience, not by formal education, so common terms and vocabulary sometimes (frequently) go over my head.

I’m assuming ticks are reads by the gear tooth sensor. Also, for the ErrorPrev variable, is that previous as in the previous cycle throug hthe code or previous on some kind of time interval?

Since this is tank driving where each joystick directly affects the respective motor, how will their assignments differ?

Thanks again for the reply.

6

Okay, so I tried Mike’s solution and with a few tweaks got it to correct itself as it’s driving straight, but the corrections are made far too slow. THat is, it swerves right then left, then right, then left, etc. and the overall displacement is actually relatively forward. However, it does this far too slow and the swerves are really wide. If it were to do this much more quickly and with tighter swerves, then it should be going quite quickly, in a very straight motion.

I’m concerned that this may be due to the cycle of code being too long. Is there any way to quicken the pace of the controller via optimization methods, or do I just have to figure out another way of doing this entirely?

7

Linx34,

A few qestions… What rate are you receiving gear tooth pulses (how many pulses per foot of travel)? Are you monitoring your PWM output, and if so, what kind of swing are you getting while you are “driving straight”? The reason I ask is that you could either be overcorrecting and the robot is not able to respond quick enough so you are seeing it oscillate arround the “correct” PWM value, or it could be that you are not seeing a difference between the left & right gear tooth sensors quick enough, although in this case I would expect it to take too long to actually start driving stright, but once it gets there, it should be fairly good. Just checking, but you are not zeroing your gear tooth counter while running the code correct? (and the joystick difference doesn’t occasionally go above 5 while going straight) Also make sure are not running your PWM outputs near the limits (<30 or >225) as you may near the limits of the Victor or motor.

Mike

8

Nevermind. It was just a low battery. Thanks for the reply anyway though. Now we’re running really straight with the only issue being that it will swerve with low probability of being at the same time in the path of travel. That is, it will swerve, but not always at the same time.

This inconsistency leads me to believe that it is no longer a code issue, but rather a traction issue. I may be able to correct it via code with the gyro, but it would be easier to increase traction somehow. I’d describe that issue further, but I’m thinking that it’s a question for a different board on the forum.