Drive Straight C Code using Encoders without PID?

[Chris_Elston]

I am helping our team out again with another problem. We are using 2WD, and trying to make our robot drive straight based on encoder feedback.

We have come up with our own code, but we really don't like it that well and it doesn't update fast enough. What we probablly need is a nice PID, but we don't have room in our controller to use any nice PID controls, like the Kevin W NAV code...

Has anyone come up with some slick ways to monitor your encoders and balance your drive system?

Attached is what we have come up with, works great when your robot is blocked up on blocks but when on the ground, I don't think the the logic can control the "loop" control fast enough and our robot is driving all over the place. Sometimes it goes straight as an arrow, other times not.

Anyone care to share what they got in their pocket for driving straight querying your encoders?

long drv_left_encoder;
long drv_right_encoder;
long drv_diff_left;
long drv_diff_right;
static int DRV_DEADBAND = 5; //only correct left or right side if encoder difference is greater than deadband
static int DRV_BOOST = 10; //turbo boost if button pressed
int print_counter;
int print_counter_before;

relay1_fwd = 0; //turn off lights, not in automous mode anymore.
camera_find_color( 0 ); //turn off vision system.


 p1_y = (p1_y / 3) + ((127 / 3) * 2); //scale down Y axis
 p1_x = (p1_x / 3) + ((127 / 3) * 2); //scale down X axis
 
 left_pwm = Limit_Mix(2000 + p1_y - p1_x + 127); //limit pwm
 right_pwm = Limit_Mix(2000 + p1_y + p1_x - 127); //limit pwm

 drv_left_encoder = Get_Left_Encoder_Count(); //get encoder count
 drv_right_encoder = Get_Right_Encoder_Count(); //get encoder count
 drv_diff_left = drv_left_encoder - drv_right_encoder; //subtract left from right because I dunno if ABS is supported in this dumb PIC
 drv_diff_right = drv_right_encoder - drv_left_encoder; //subtract right from left because I dunno if ABS is supported in this dumb PIC

if ((p1_y <= 135) && (p1_y >= 120)) //reset encoder counts
	{
	Set_Left_Encoder_Count(0);
    Set_Right_Encoder_Count(0);
	drv_diff_left = 0;
	drv_diff_right = 0;
	}

#ifdef DEBUG_DRVMAN //uncomment DEBUG_DRVMAN in robot.c to get this to excute.

if (++print_counter_before >= 2)
	{
	print_counter_before = 0;
	printf( "BY=%d BLE=%d BLD=%d BRE=%d BRD=%d BLPWM=%d bRPWM=%d\r",p1_y,(int)(drv_left_encoder),(int)(drv_diff_left),(int)(drv_right_encoder),(int)(drv_diff_right),left_pwm,right_pwm);
	}

#endif

if ((p1_x <= 135) && (p1_x >= 120)) //not turning
	{
	if (p1_y >= 145) //check to see if we are driving straight forward
		{

		if (drv_diff_left >= DRV_DEADBAND) //increase power to the right drive pwm12
			{
			if (drv_diff_left > DRV_DEADBAND)
				{
				right_pwm = right_pwm + 6;
				}
			if (drv_diff_left > DRV_DEADBAND + 10)
				{
				right_pwm = right_pwm + 8;
				}
			if (drv_diff_left > DRV_DEADBAND + 20)
				{
				right_pwm = right_pwm + 10;
				}
			}
	
		if (drv_diff_right >= DRV_DEADBAND) //increase power to the left drive pwm11
			{
			if (drv_diff_right > DRV_DEADBAND)
				{
				left_pwm = left_pwm + 6;
				}
			if (drv_diff_right > DRV_DEADBAND + 10)
				{
				left_pwm = left_pwm + 8;
				}
			if (drv_diff_right > DRV_DEADBAND + 20)
				{
				left_pwm = left_pwm + 10;
				}
			}
		}

	if (p1_y <= 120) //check to see if we are driving straight backwards
		{
		if (drv_diff_left >= DRV_DEADBAND) //increase power to the right drive pwm12
			{
			if (drv_diff_left > DRV_DEADBAND)
				{
				right_pwm = right_pwm - 3;
				}
			if (drv_diff_left > DRV_DEADBAND + 10)
				{
				right_pwm = right_pwm - 4;
				}
			if (drv_diff_left > DRV_DEADBAND + 20)
				{
				right_pwm = right_pwm - 5;
				}
			}
	
		if (drv_diff_right >= DRV_DEADBAND) //increase power to the left drive pwm11
			{
			if (drv_diff_right > DRV_DEADBAND)
				{
				left_pwm = left_pwm - 3;
				}
			if (drv_diff_right > DRV_DEADBAND + 10)
				{
				left_pwm = left_pwm - 4;
				}
			if (drv_diff_right > DRV_DEADBAND + 20)
				{
				left_pwm = left_pwm - 5;
				}
			}
		}
	}
else //if we are turning, then don't try to drive straight
	{
	Set_Left_Encoder_Count(0);
    Set_Right_Encoder_Count(0);
	drv_diff_left = 0;
	drv_diff_right = 0;
	}

/*
if (p1_y >= 130 && p1_sw_trig) //forward boost, will be used later for turbo boost pushing power
	{
	pwm11 = left_pwm + DRV_BOOST;
	pwm12 = right_pwm + DRV_BOOST;
	}
*/
/*
pwm11 = left_pwm;
pwm12 = right_pwm;

#ifdef DEBUG_DRVMAN //uncomment DEBUG_DRVMAN in robot.c to get this to excute.

if (++print_counter >= 2)
	{
	print_counter = 0;
	printf( "AY=%d ALE=%d ALD=%d ARE=%d ARD=%d ALPWM=%d ARPWM=%d\r",p1_y,(int)(drv_left_encoder),(int)(drv_diff_left),(int)(drv_right_encoder),(int)(drv_diff_right),left_pwm,right_pwm);
	}

#endif
}

[Alan Anderson]

Originally Posted by chakorules

Has anyone come up with some slick ways to monitor your encoders and balance your drive system?

Last year, we had a string of matches where the robot swerved badly to one side when it was being commanded to go straight. We thought we were having drivetrain balance or other drag issues. I whipped up a simple algorithm that compared the accumulated wheel counts on each side, and for each count they differed, it scaled up one side's motor control by a percentage while scaling down the other side by the same percentage. In bench testing it worked great, matching both side's wheel speeds regardless of how much drag was placed on either side.

On the field, it more often than not resulted in the robot lurching downfield briefly and coming to a complete halt, and we were very pressed for time, so rather than spend a lot of effort debugging the software we abandoned the scheme. It turns out the code was fine, and the problem was actually with the gearbox, where a poorly held bearing was letting the gears disengage whenever it tried to move suddenly in reverse. Our autonomous setup happened to involve starting the robot at full speed in reverse to "back up" down the field to a turning point. One side would fail to drive, and the algorithm dutifully increased power to the disengaged drivetrain while decreasing power to the working one. The error would quickly reach the point where the working side was being throttled all the way back to zero.

[Kevin Watson]

Originally Posted by chakorules

Has anyone come up with some slick ways to monitor your encoders and balance your drive system?

Rich Petras' PID code already has velocity control built-in. Why not just use what's already there?

-Kevin

[Chris_Elston]

Great suggestion, if you can tell us how to cram 40K of program into a 32K CPU?

GYROs, and PID had to go in the Nav code, we got so much other code that we need with using the camera that we can't fit it all in there....once we intergrated the encoders, camera into the navigation code, and using a multi pointer struct commands to drive our automous mode, our program had to go on a slim fast diet...

That's why we can't use the PID code that we deleted..

Originally Posted by Kevin Watson

Rich Petras' PID code already has velocity control built-in. Why not just use what's already there?

-Kevin

[Kingofl337]

Take out some of the comments.

[gnormhurst]

Are you using any floating point calculations? Even multiplying by a constant that has a decimal point in it constitutes a floating point operation. Find ways to use integers instead.

For example: Need to multiply by pi? Forget 3.14159! Multiply by 355, then divide by 113. Make sure you do it in that order (don't divide first -- the answer is different).

[Chris_Elston]

Hi Norm,

Thanks for bringing that up and reminding everyone that floating point also sucks the life out of these controllers, but nope we are not using an floating point. We wanted too SOOOOO bad. We wanted to use an X,Y, Theta position system, but it would require SIN and COS. We did try the one math library in the whitepapers section, even deleted some math we wasn't using be it was still too much on the controller.

Originally Posted by gnormhurst

Are you using any floating point calculations? Even multiplying by a constant that has a decimal point in it constitutes a floating point operation. Find ways to use integers instead.

For example: Need to multiply by pi? Forget 3.14159! Multiply by 355, then divide by 113. Make sure you do it in that order (don't divide first -- the answer is different).

[Chris_Elston]

I hope you mean DEBUG printf statements right? Comments aren't suppose to be stored in the controller to my knowledge...??? Are they?

Originally Posted by Kingofl337

Take out some of the comments.

[probizzle]

A quick thing to point out.

You are reading the map files and not looking at the pure size of the hex files right?
Because if you are just looking at the hex files, the size does not reflect usage of the 32k space.

I guess if you guys know about it, will be helpful to someone else. But VERY important to keep in mind.

[Chris_Elston]

Good point too. But actually we are looking at the LST file.

When you complie, go to FILE, OPEN and change your extension browse to .LST. Then find the list file in your project folder.

Find the last HEX entry in the left side. Should be no bigger than 7FFF.

Use Windows Calculator, (change to scientific mode) to convert HEX to DEC.

Actually what will happen first is your complier will give you and error something like this.

Can not fit robot.io or something like that..actually I should have this error memorizes because I've seen it like 100 times already.

Originally Posted by probizzle

A quick thing to point out.

You are reading the map files and not looking at the pure size of the hex files right?
Because if you are just looking at the hex files, the size does not reflect usage of the 32k space.

I guess if you guys know about it, will be helpful to someone else. But VERY important to keep in mind.

[gnormhurst]

Originally Posted by chakorules

I hope you mean DEBUG printf statements right? Comments aren't suppose to be stored in the controller to my knowledge...??? Are they?

Nope. They don't go farther than the source files.

And I recently was shown (here) that the difference between

#define BEGIN  0
#define MIDDLE 1
#define END    2

and

enum
{
  BEGIN,
  MIDDLE,
  END
}

is that the enum{} actually allocates integer storage, but the #defines are just literal constants. I suppose that means that #defines use less RAM than enum{}.

[gnormhurst]

Originally Posted by chakorules

Thanks for bringing that up and reminding everyone that floating point also sucks the life out of these controllers, but nope we are not using an floating point. We wanted too SOOOOO bad. We wanted to use an X,Y, Theta position system, but it would require SIN and COS. We did try the one math library in the whitepapers section, even deleted some math we wasn't using be it was still too much on the controller.

I figured you knew that floating point is evil, but I also figured that others might benefit from hearing it. Congratulations on avoiding the temptation!

To all who flinch at the "inaccuracies" of integer math: engineering is the art of compromise. sin(x) = x, when x is small. If it's good enough, it just may be perfect.

Just say no to floating point!

[Alan Anderson]

Originally Posted by chakorules

We wanted to use an X,Y, Theta position system, but it would require SIN and COS.

A relatively small lookup table lets you use sines and cosines with sufficient precision to get by, and it takes approximately no time to compute when compared to even a CORDIC implementation.

[probizzle]

Originally Posted by gnormhurst

Nope. They don't go farther than the source files.

And I recently was shown (here) that the difference between

#define BEGIN  0
#define MIDDLE 1
#define END    2

and

enum
{
  BEGIN,
  MIDDLE,
  END
}

is that the enum{} actually allocates integer storage, but the #defines are just literal constants. I suppose that means that #defines use less RAM than enum{}.

I've been told, and have deduced from compiler errors that #defines are essentially copy-paste definitions for the compiler.

Am I correct?

[Alan Anderson]

Originally Posted by probizzle

I've been told, and have deduced from compiler errors that #defines are essentially copy-paste definitions for the compiler.

Am I correct?

You're correct, but you're not quite going far enough. In every C compiler I've studied, #defines are handled by a preprocessor. They take effect before the actual compiler ever sees the text in the first place.