// This function sets CCSR forward speed and turn speed, but applies a filter, such that
// speed does not jump to target speed directly. Calling this function will only bring the
// speed one step closer to target fwd and turn speeds, and the step size is based on a
// quantization function. The function returns 1 if target speeds are reached, 0 otherwise
// We may have to call this function multiple times with the same argument before target speeds are reached
//
// targetSpeed = [-255..255], negative is backward.
// delta = [0..511], this is the positive delta between MOTOR1 and targetSpeed, and the negative delta between
// MOTOR2 and targetSpeed, thus determining turn speed
//
// e.g. targetSpeed = 120, delta = 10 (forward momentum with slight turn to right)
// motor1 = 110
// motor2 = 130
// e.g. targetSpeed = 0, delta = 100 (in-place turn around Z-axis)
// motor1 = 100
// motor2 = -100
int speedFiltered(int targetSpeed, int delta) {
int diff1, diff2;
int increment1, increment2;
int targetReached1, targetReached2;
int targetSpeedMotor1, targetSpeedMotor2;
targetReached1 = targetReached2 = 0;
targetSpeedMotor1 = targetSpeed + delta;
targetSpeedMotor2 = targetSpeed - delta;
diff1 = ccsrState.speedMotor1 - targetSpeedMotor1;
diff2 = ccsrState.speedMotor2 - targetSpeedMotor2;
// Quentization function using MOTOR_SPEEDUP_STEPS steps
if(abs(diff1) < MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS) {
increment1 = -diff1;
targetReached1 = 1;
}
else if (diff1<0) {
increment1 = MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS;
targetReached1 = 0;
}
else {
increment1 = -MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS;
targetReached1 = 0;
}
if(abs(diff2) < MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS) {
increment2 = -diff2;
targetReached2 = 1;
}
else if (diff2<0) {
increment2 = MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS;
targetReached2 = 0;
}
else {
increment2 = -MAX_MOTOR_SPEED/MOTOR_SPEEDUP_STEPS;
targetReached2 = 0;
}
if (diff1 != 0) {
setMotorspeed(ccsrState.speedMotor1 + increment1, MOTOR1);
}
if (diff2 != 0) {
setMotorspeed(ccsrState.speedMotor2 + increment2, MOTOR2);
}
#ifdef DEBUG
if((diff1 !=0) || (diff2!=0)){
printf("motor1: %d motor2: %d\n", ccsrState.speedMotor1, ccsrState.speedMotor2);
}
#endif
return targetReached1 && targetReached2;
}
void manualRightTurn()
{
setMotorspeed(-150,0);
setMotorSpeed(150,1);
delay(150);
setBoth(0);
}