void CASW_Simple_Alien::WhatToDoNext(float delta)
{
if (m_iState == ASW_SIMPLE_ALIEN_ATTACKING)
{
if (!GetEnemy() || !CanSee(GetEnemy()) || GetEnemy()->GetHealth() <= 0)
{
LostEnemy(); // clears our enemy and sets us back to idling
}
else
{
// head towards enemy
SetMoveTarget(GetChaseDestination(GetEnemy()));
PerformMovement(delta);
}
SetNextThink( gpGlobals->curtime + 0.1f );
}
else if (m_iState == ASW_SIMPLE_ALIEN_IDLING)
{
// look for a new enemy?
FindNewEnemy();
if (GetEnemy())
SetNextThink( gpGlobals->curtime + 0.1f );
else
SetNextThink( gpGlobals->curtime + 0.5f );
}
else if (m_iState == ASW_SIMPLE_ALIEN_DEAD)
{
// don't need to think again
}
// todo: if in moving to dest state, then move towards that dest, etc.
}
void CASW_Simple_Alien::AlienThink()
{
float delta = gpGlobals->curtime - m_fLastThinkTime;
StudioFrameAdvance();
DispatchAnimEvents(this);
UpdateYaw(delta);
if (m_bMoving)
{
PerformMovement(delta);
SetNextThink( gpGlobals->curtime + 0.1f );
}
else
{
WhatToDoNext(delta); // this will set the next think interval based on what the alien decides to do next
}
m_fLastThinkTime = gpGlobals->curtime;
}
//thread to send updates to the motor processor
void *AutopilotThread(void *arg) {
int priorPilotState; //used to cancel notifications
// PowerState_enum powerState = POWER_STATE_UNKNOWN;
DEBUGPRINT("Pilot thread ready");
//loop
while (1) {
//critical section
int s = pthread_mutex_lock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx lock %i", s);
}
priorPilotState = pilotState; //track state for Conditions
if (reviewProgress & ~motorsBusy)
{
//review what's happening
switch (pilotState)
{
//Orienting is just a turn to a specific compass direction
case PILOT_STATE_ORIENT:
PerformOrient();
break;
case PILOT_STATE_MOVE:
PerformMovement();
break;
default:
break;
}
}
//update notifications
if (priorPilotState != pilotState) {
LogRoutine("Pilot State: %s", pilotStateNames[pilotState]);
switch (priorPilotState) {
case PILOT_STATE_IDLE:
case PILOT_STATE_INACTIVE:
break;
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
case PILOT_STATE_ORIENT:
case PILOT_STATE_MOVE:
ps_cancel_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_DONE:
ps_cancel_condition(PILOT_IDLE);
break;
case PILOT_STATE_FAILED:
ps_cancel_condition(PILOT_FAILED);
break;
}
switch (pilotState) {
case PILOT_STATE_IDLE:
case PILOT_STATE_INACTIVE:
ps_cancel_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_FORWARD_SENT:
case PILOT_STATE_BACKWARD_SENT:
case PILOT_STATE_ORIENT_SENT:
case PILOT_STATE_MOVE_SENT:
case PILOT_STATE_FORWARD:
case PILOT_STATE_BACKWARD:
case PILOT_STATE_ORIENT:
case PILOT_STATE_MOVE:
ps_set_condition(PILOT_ENGAGED);
break;
case PILOT_STATE_DONE:
ps_set_condition(PILOT_IDLE);
break;
case PILOT_STATE_FAILED:
case PILOT_STATE_ABORT:
ps_set_condition(PILOT_FAILED);
break;
}
}
s = pthread_mutex_unlock(&pilotStateMtx);
if (s != 0)
{
ERRORPRINT("Pilot: pilotStateMtx unlock %i", s);
}
//end critical section
usleep(100000);
}
}
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;
}