/// Sends an analog signal to the motors
/// \param leftSpeed The value to set the motor to. range: [-255,255]
/// \param rightSpeed The value to set the motors to. range: [-255,255]
void Robot::drive(int leftSpeed, int rightSpeed) {
// Make sure the values are in range.
checkRange(&leftSpeed);
checkRange(&rightSpeed);
analogWrite(roboConfig.enA, abs(leftSpeed));
analogWrite(roboConfig.enB, abs(rightSpeed));
setMotorStates(leftSpeed, rightSpeed);
}
void RobotController::changeState(RobotState newState) {
/* don't move the robot while it is failed */
if (failed && newState != ROBOT_STOPPED) {
getEventController()->queueEvent(PUBLISH_EVENT_FAIL);
return;
}
Serial.print("CHANGING STATE -> ");
Serial.println(robotStateToString(newState));
#ifdef WHEEL_CASTER
if (cleaningCaster) {
Serial.println("FAILED -> CLEANING CASTER");
return;
}
#endif
if (newState == ROBOT_TURNING_LEFT || newState == ROBOT_TURNING_RIGHT) {
changeTurningState(true);
} else {
changeTurningState(false);
}
switch (newState) {
case ROBOT_STOPPED:
#ifdef WHEEL_CASTER
if (state != ROBOT_FORWARD &&
state != ROBOT_STOPPED &&
state != ROBOT_CLEANUP &&
state != ROBOT_TURNING_RIGHT) {
setMotorStates(CLEANUP);
newState = ROBOT_CLEANUP;
cleaningCaster = true;
} else {
setMotorStates(STOPPED);
}
#else
setMotorStates(STOPPED);
#endif
if (movingForDistance) {
movingForDistance = false;
}
break;
case ROBOT_FORWARD:
setMotorStates(FORWARD);
break;
case ROBOT_TURNING_LEFT:
#ifdef ONE_WHEEL_ROTATION
motorLeft->setState(STOPPED);
#else
motorLeft->setState(BACKWARD);
#endif
motorRight->setState(FORWARD);
break;
case ROBOT_TURNING_RIGHT:
#ifdef ONE_WHEEL_ROTATION
motorRight->setState(STOPPED);
#else
motorRight->setState(BACKWARD);
#endif
motorLeft->setState(FORWARD);
break;
case ROBOT_BACKWARD:
setMotorStates(BACKWARD);
break;
}
state = newState;
getEventController()->queueEvent(PUBLISH_EVENT_MOVE_STATUS);
}
