bool PerformMovement() { float rangeCM = GetRangeToGoal(); LogRoutine("Pilot: Range = %.0f", rangeCM); if (rangeCM < arrivalRange) { SendMotorCommand(0,0,0,0); LogRoutine("Pilot: Route done"); pilotState = PILOT_STATE_DONE; } //start required orientation desiredCompassHeading = ComputeBearing(nextPosition); if (PerformOrient()) { //no turn needed //send move command int motorRange = (rangeCM > motorMaxCM ? motorMaxCM : rangeCM); SendMotorCommand(motorRange, motorRange, motorSpeed, pilotFlags); motorsRunTimeout = motorRange * timeoutPerCM + 5; //TODO: Adjust port & starboard to correct residual heading error } return false; }
bool PerformOrient() { //initiate turn to 'desiredCompassHeading' //calculate angle error float angleError = (360 + desiredCompassHeading - pose.orientation.heading) % 360; if (angleError > 180) angleError -= 360; //if close, report done if (abs(angleError) < arrivalHeading) { SendMotorCommand(0,0,0,0); LogRoutine("Pilot: Orient done"); pilotState = PILOT_STATE_DONE; return true; } else { LogRoutine("Pilot: angle error %f", angleError); //send turn command // float range = abs(angleError * FIDO_RADIUS * M_PI / 180.0); // //send turn command // if (angleError > 0) { // SendMotorCommand((int)range, (int)-range, SlowSpeed, pilotFlags); // } else { // SendMotorCommand((int)-range, (int)range, SlowSpeed, pilotFlags); // } // motorsRunTimeout = range * timeoutPerCM + 5; return false; } }
bool CancelPilotOperation(PilotState_enum newState) { //cancel any current operation //assumes we already have the mutex //returns true if stop sent switch(pilotState) { case PILOT_STATE_FORWARD: case PILOT_STATE_BACKWARD: case PILOT_STATE_ORIENT: case PILOT_STATE_MOVE: case PILOT_STATE_FORWARD_SENT: case PILOT_STATE_BACKWARD_SENT: case PILOT_STATE_ORIENT_SENT: case PILOT_STATE_MOVE_SENT: //send stop message SendMotorCommand(0,0,0,0); LogRoutine("Pilot %s Cancelled.", pilotStateNames[pilotState]); pilotState = newState; return true; //stop sent break; default: return false; break; } }
// Get all the settings to be used for this layer, and send the tray deflection // command if a non-zero deflection has been requested; bool PrinterStateMachine::HandlePressCommand() { _pPrintEngine->GetCurrentLayerSettings(); if (_pPrintEngine->GetTrayDeflection() == 0) return false; SendMotorCommand(PRESS_COMMAND); return true; }
// Send the command to the motor controller that moves to the home position. void PrinterStateMachine::SendHomeCommand() { // send the Home command to the motor controller SendMotorCommand(HOME_COMMAND); }
ActionResult_enum AutopilotAction(PilotAction_enum _action) { ActionResult_enum result = ACTION_FAIL; //critical section int s = pthread_mutex_lock(&pilotStateMtx); if (s != 0) { ERRORPRINT("Pilot: pilotStateMtx lock %i", s); } switch(_action) { //simple actions case PILOT_FAST_SPEED: motorSpeed = FastSpeed; result = ACTION_SUCCESS; break; case PILOT_MEDIUM_SPEED: motorSpeed = MediumSpeed; result = ACTION_SUCCESS; break; case PILOT_SLOW_SPEED: motorSpeed = SlowSpeed; result = ACTION_SUCCESS; break; case PILOT_RESET: //cancel any current operation CancelPilotOperation(PILOT_STATE_IDLE); pilotEngaged = false; result = ACTION_SUCCESS; break; default: { if (pilotEngaged) { //monitor on-going operation switch(pilotState) { case PILOT_STATE_FORWARD: //short distance move using encoders only case PILOT_STATE_BACKWARD: //ditto case PILOT_STATE_ORIENT: //monitoring motion using compass case PILOT_STATE_MOVE: //monitoring move using pose msg case PILOT_STATE_FORWARD_SENT: case PILOT_STATE_BACKWARD_SENT: case PILOT_STATE_ORIENT_SENT: //monitoring motion using compass case PILOT_STATE_MOVE_SENT: //monitoring move using pose msg result = ACTION_RUNNING; break; case PILOT_STATE_DONE: //move complete pilotEngaged = false; result = ACTION_SUCCESS; break; case PILOT_STATE_FAILED: //move failed pilotEngaged = false; result = ACTION_FAIL; break; case PILOT_STATE_IDLE: //ready for a command case PILOT_STATE_INACTIVE: //motors disabled default: pilotEngaged = false; result = ACTION_FAIL; lastLuaCallReason = "Inactive"; break; } } else if (pilotState != PILOT_STATE_INACTIVE) { //start new action //verify pre-requisites switch (_action) { case PILOT_FAST_SPEED: motorSpeed = FastSpeed; result = ACTION_SUCCESS; break; case PILOT_MEDIUM_SPEED: motorSpeed = MediumSpeed; result = ACTION_SUCCESS; break; case PILOT_SLOW_SPEED: motorSpeed = SlowSpeed; result = ACTION_SUCCESS; break; case PILOT_RESET: result = ACTION_SUCCESS; break; case PILOT_ORIENT: case PILOT_TURN_LEFT: case PILOT_TURN_RIGHT: case PILOT_TURN_LEFT_90: case PILOT_TURN_RIGHT_90: case PILOT_TURN_N: case PILOT_TURN_S: case PILOT_TURN_E: case PILOT_TURN_W: if (VerifyCompass()) { result = ACTION_RUNNING; } else { result = ACTION_FAIL; lastLuaCallReason = "Compass"; } break; case PILOT_ENGAGE: if (VerifyCompass()) { result = ACTION_RUNNING; } else { result = ACTION_FAIL; lastLuaCallReason = "NoIMU"; } break; case PILOT_MOVE_FORWARD: case PILOT_MOVE_BACKWARD: case PILOT_MOVE_FORWARD_10: case PILOT_MOVE_BACKWARD_10: result = ACTION_RUNNING; break; default: LogError("Pilot action: %i", _action); result = ACTION_FAIL; lastLuaCallReason = "BadCode"; break; } if (result == ACTION_RUNNING) { //prepare goal switch (_action) { case PILOT_ORIENT: desiredCompassHeading = ComputeBearing(nextPosition); break; case PILOT_TURN_LEFT: desiredCompassHeading = (360 + pose.orientation.heading - CalculateRandomTurn()) % 360; break; case PILOT_TURN_RIGHT: desiredCompassHeading = (pose.orientation.heading + CalculateRandomTurn()) % 360; break; case PILOT_TURN_LEFT_90: desiredCompassHeading = (pose.orientation.heading - 90) % 360; break; case PILOT_TURN_RIGHT_90: desiredCompassHeading = (pose.orientation.heading + 90) % 360; break; case PILOT_TURN_N: desiredCompassHeading = 0; break; case PILOT_TURN_S: desiredCompassHeading = 180; break; case PILOT_TURN_E: desiredCompassHeading = 90; break; case PILOT_TURN_W: desiredCompassHeading = 270; break; case PILOT_ENGAGE: desiredCompassHeading = ComputeBearing(nextPosition); default: break; } //start movement switch (_action) { case PILOT_ORIENT: case PILOT_TURN_LEFT: case PILOT_TURN_RIGHT: case PILOT_TURN_LEFT_90: case PILOT_TURN_RIGHT_90: case PILOT_TURN_N: case PILOT_TURN_S: case PILOT_TURN_E: case PILOT_TURN_W: LogRoutine("Pilot: Orient to: %i", desiredCompassHeading); if (PerformOrient()) { result = ACTION_SUCCESS; } else { pilotState = PILOT_STATE_ORIENT_SENT; } break; case PILOT_ENGAGE: LogRoutine("Pilot: Move to: %fN, %fE", nextPosition.longitude, nextPosition.latitude); if (PerformMovement()) { result = ACTION_SUCCESS; } else { pilotState = PILOT_STATE_MOVE_SENT; } break; case PILOT_MOVE_FORWARD: { int distance = CalculateRandomDistance(); SendMotorCommand(distance, distance, SlowSpeed, pilotFlags); motorsRunTimeout = distance * timeoutPerCM + 5; LogRoutine("Pilot: Move forward"); pilotState = PILOT_STATE_FORWARD_SENT; } break; case PILOT_MOVE_BACKWARD: { int distance = CalculateRandomDistance(); SendMotorCommand(-distance, -distance, SlowSpeed, pilotFlags); motorsRunTimeout = distance * timeoutPerCM + 5; LogRoutine("Pilot: Move backward"); pilotState = PILOT_STATE_BACKWARD_SENT; } break; case PILOT_MOVE_FORWARD_10: { int distance = 10; SendMotorCommand(distance, distance, SlowSpeed, pilotFlags); motorsRunTimeout = distance * timeoutPerCM + 5; LogRoutine("Pilot: Move forward 10"); pilotState = PILOT_STATE_FORWARD_SENT; } break; case PILOT_MOVE_BACKWARD_10: { int distance = 10; SendMotorCommand(-distance, -distance, SlowSpeed, pilotFlags); motorsRunTimeout = distance * timeoutPerCM + 5; LogRoutine("Pilot: Move backward 10"); pilotState = PILOT_STATE_BACKWARD_SENT; } break; default: break; } if (result == ACTION_RUNNING) pilotEngaged = true; } } else { result = ACTION_FAIL; lastLuaCallReason = "Inactive"; } } break; } s = pthread_mutex_unlock(&pilotStateMtx); if (s != 0) { ERRORPRINT("Pilot: pilotStateMtx unlock %i", s); } //end critical section return result; }