Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
	}
}
Ejemplo n.º 3
0
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);
}
Ejemplo n.º 6
0
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;
}