Quote:
|
Originally Posted by Kingofl337
Now the robot is shipped I'm kind of tinkering with last years robot. This robot has some form of loss in the drive train on the left side and causes it to drift intermittently with the 2005 navigation code when driving strait. The encoders work but they don't seem to be used when driving strait. Anyone have some code I can try to adjust PWM's based on encoder counts?
|
Here is something our team was toying with...never got it finished and debugged totally...but if your using encoders this is a start for you.
Code:
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
*/
}