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),le ft_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),le ft_pwm,right_pwm);
}

#endif
}

[Alan Anderson]

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]

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.. :(


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.




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?




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.





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]

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 2and

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]

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]

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]

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 2and

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]

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.

[Dave Flowerday]

I've been told, and have deduced from compiler errors that #defines are essentially copy-paste definitions for the compiler.
Yes. #defines are handled by the pre-processor before the file ever gets compiled. So what really happens is that the preprocessor finds all occurances of the #defined name and replaces it with whatever it was #defined to, and then runs the compiler on the result. The compiler never even sees the names you've assigned to your #defines.

[gnormhurst]

The compiler never even sees the names you've assigned to your #defines.And compiler error messages reflect that: the compiler will point to a line of code and object to some text that isn't in that line of code -- it's in the #define

#define PI 3.14.159 // syntax error in macro

..

circum = PI * dia;
If the compiler quotes the code it doesn't like, it will quote "3.14.159", not "PI" while referring to the line "circum = PI * dia;". That's because it doesn't see "PI", only "3.14.159".

[the_undefined]

Just another Idea:
Did you play arround with the Code Optimizing Features that the C18 Compiler has? I know you can get at least 10-15% more Space using that but I've also heard it could cause weird errors. But if you ask some people that used that before they can probably tell you what's safe to turn on and what you have to keep unoptimized ...