We are using the camera so that the robot goes into a loop and moves the drive wheels until the camera tells us that it is centered. this lets us aim the robot at the light. I can get it to work but I can't get it to do it very fast. this is what i have so far right know.
Code:
if (aiming == 1 | p1_sw_trig == 1)
{
aiming = 1;
Switch1_LED = 1; // aiming light turns on when aiming
if (p4_sw_trig == 1)
{
Switch1_LED = 0;
printf("override button pressed");
aiming = 0;
}
else if (T_Packet_Data.confidence < 5)
{
Switch1_LED = 0;
printf("lost light lock");
aiming = 0;
}
if (PAN_SERVO > PAN_CENTER_PWM_DEFAULT + 20)
{
pwm03 = 127 - aim_gain;
pwm04 = 127 + aim_gain;
}
else if (PAN_SERVO < PAN_CENTER_PWM_DEFAULT - 20)
{
pwm03 = 127 + aim_gain;
pwm04 = 127 - aim_gain;
}
else if (PAN_SERVO < PAN_CENTER_PWM_DEFAULT + 21 & PAN_SERVO > PAN_CENTER_PWM_DEFAULT)
{
pwm03 = 127 + aim_soft_gain;
pwm04 = 127 - aim_soft_gain;
}
else if (PAN_SERVO > PAN_CENTER_PWM_DEFAULT - 21 & PAN_SERVO < PAN_CENTER_PWM_DEFAULT)
{
pwm03 = 127 - aim_soft_gain;
pwm04 = 127 + aim_soft_gain;
}
else
{
Switch1_LED = 0;
printf("TARGET ACQUIRED");
aiming = 0;
}
}
note that
aim_gain = 22 and aim_soft_gain = 15
The code makes the robot overshoot and oscillate back and forth. it takes ten or more seconds to get it to center and this is not very good.
I know there is a way to use a linear or even quadratic formula to increase the speed and to reduce the amount of overshoot instead of what i have i just don't know how to go about programming it. Any help would be vary much appreciated.