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
}