/// 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); }