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