Accurate Drive Foward Function


Hello everyone,

I am trying to make an extremely accurate function that will drive the robot a specified number of inches. This is currently what I have.

void driveFoward(int inches){
	SensorValue(quadRight) = 0;
	float distance = inches/(4*PI) * 360;
	float Kp = 1.5;
	float error;
	float proporition;
		error = distance - SensorValue(quadRight); 
		proporition = error * Kp;
		motor[frontLeft] = motor[backLeft] = proporition;
		motor[frontRight] = motor[backRight] = proporition;


I want to make it more accurate. Should I implement a few more things such as auto correction and a D controller?


Consider a few things:

You’re not checking the two sides against each other, nor are you checking a gyroscope, to make sure you’re actually driving straight forward.

You may run into problems with your proportion. proportion and error are measured in ticks. If you’re 3" away, you’re still roughly (I’m just estimating 3 for pi in my head as I type.) 90 ticks away. You then multiply by 1.5 to get 135, still over the maximum of 127. So you’re still running at full power until closer than 3" away. I’m not sure you’re dropping in power fast enough not to overshoot. Adjusting Kp could help, as could adding other terms (Ki, Kd). Or, since you’re already dropping motor power so the shift won’t be huge, throw the motors a bit in the opposite direction for just a moment right as you get there to brake.


I just put a random value for Kp, it will probably be very small once I tune it. I might also add a Kd term to prevent overshoot or a brake. Which would you recommend?


I’ve gotten a consistent 84 point programming skills on my bot, and I don’t even use PID for the driving. All you’ve really got to do is 1) make sure you have an encoder on both sides of the drive that you’re checking, 2) use a gyroscope to make sure it stays straight, and 3) use a slight brake at the end of the distance you drive to prevent overshoot.

The most important of all of those I would say is the gyroscope, because it makes sure you don’t get knocked off course on the off chance you run into a cone or something. I just add the value of the gyroscope to the left side motors, and subtract it from the right side motors. Works wonders.


On the off chance. That’s some good route planning, straight driving, and turns.

I assume you’re using the difference between the heading before you start driving and the measured heading as you drive. Good approach. And that is, by the way, a P controller.


Hey Naman,
One thing I would recommend definitely add autocorrection, as that works really well. However, add a derivative term and brake as @callen and @Mystellianne were saying. The autocorrection can be big when driving long distances or turning frequently to make sure you don’t go off course, but it can be time consuming if your function is very inaccurate in the first place. Get it to work beautifully first, and then add autocorrection for unforeseen errors.


Watch out for “very small” how you’re using it, too. For example, let’s say you use 0.01 for Kp. Around 3" away you would then run at roughly 90*0.01=0.9. The robot wouldn’t even make it to the destination. What minimum value to the motor is needed to overcome friction enough to even have the robot move slowly? That will give you an indicator. When your value drops below that, the robot will just drift a tiny bit and stop.


Yeah the gyroscope is essentially on a P controller (forgot to mention a kP of 0.5), but the actual driving itself is straight forward and brake at the end, no PID needed.

And thank you, I’d like to think my route is pretty well planned :stuck_out_tongue:


Thank you for all the advice. Here is what I have done based on the feedback given.

void driveFoward(int inches){
	SensorValue(quadRight) = 0;
	SensorValue(gyro) = 0;
	float distance = inches/(4*PI) * 360;
	float Kp = 1.1; // will tune this
	float error;
	float proporition;

	while(SensorValue(quadRight) < distance){
		error = distance - SensorValue(quadRight); 
		proporition = error * Kp;
		motor[frontLeft] = motor[backLeft] = proporition + SensorValue(gyro);
		motor[frontRight] = motor[backRight] = proporition - SensorValue(gyro);

	motor[frontLeft] = motor[backLeft] = -50;
	motor[frontRight] = motor[backRight] = -50;
	motor[frontLeft] = motor[backLeft] = 0;
	motor[frontRight] = motor[backRight] = 0;

I have added the gyro so the robot will drive straight. I also added a brake of a 1/5 second.


Looks fine to me. If you notice the gyro stabilization making it oscillate too much, feel free to add a multiplier (just like a Kp) to make it less impactful. And because the brake depends on the inertia your robot has, you may have to adjust the wait time or the motor power depending on whether not it’s too much or too little. But the form of the code looks fine :slight_smile:


@Mystellianne what sort of filtering are you using if any for the gyro? Our gyro gets thrown off very badly when we line up against the wall. Also, does it make a very large difference if we don’t have it near the turning point of our chassis?


I’ve actually been looking into a filter that uses angular velocity instead of angular position, but none of my programming right now uses a filter. To be honest, I just reset the gyro before every turn. I know it sounds like that would just throw it off more in the long run, but if you align yourself with the wall/bar and make sure your gyro is calibrated correctly, it’s actually extremely accurate. A lot of people use two gyros and average them to try and eliminate some drift from one or the other, but I don’t have the money for another gyro so lol

As for the positioning of the gyro, it doesn’t have to be at the center of rotation, but it would be nice if it was. Mine is slightly off center, and it works really well anyway.


For our kinds of turns I’ve seen no measurable difference between locating at the center of rotation and located at the perimeter. If you have error, that is very unlikely to be the reason, in my experience. The sensor will still work if it is being moved in the x-y plane (translated) while it is turning. It doesn’t notice the “translational” part of the movement, just the rotational component. And that’s the same no matter how far out from the pivot point it is placed.