Okay, I have something mocked up now.. How does this look??
if (swt_camera)
{
// If the image is not center, check with the allowable error range
if (pan_error > PAN_ALLOWABLE_ERROR_DEFAULT)
{
// Attempt to turn left
pwm_rightDrive_cim = pwm_rightDrive_cim = 200;
pwm_leftDrive_cim = pwm_leftDrive_fp = 120;
}
else if(pan_error < -1 * PAN_ALLOWABLE_ERROR_DEFAULT)
{
// Attempt to turn right
pwm_rightDrive_cim = pwm_rightDrive_cim = 120;
pwm_leftDrive_cim = pwm_leftDrive_fp = 200;
}
else
{
// Set drive motors to neutral
pwm_rightDrive_cim = pwm_rightDrive_cim = 127;
pwm_leftDrive_cim = pwm_leftDrive_fp = 127;
// Tell the controller that it is now on target
Tracking_State += STATE_PAN_ON_TARGET;
}
}
Should this work?