int FlightTasks::switchTask(int new_task_index)
{
// make sure we are in range of the enumeration before casting
if (static_cast<int>(FlightTaskIndex::None) <= new_task_index &&
static_cast<int>(FlightTaskIndex::Count) > new_task_index) {
return switchTask(FlightTaskIndex(new_task_index));
}
switchTask(FlightTaskIndex::None);
return -1;
}
/**
* Switch to the next task in the available list (for testing)
* @return true on success, false on error
*/
int switchTask() { return switchTask(static_cast<int>(_current_task_index) + 1); }
//-----------------------------------------------------------------------------
void CChatterboxCtrl::updateData ( float dt )
{
float distance;
mTime += dt;
mElapsedTime += dt;
mVoltageLpf = mVoltageLpf + ( dt / TAU_VOLTAGE_LPF )
* ( mPowerPack->getVoltage() - mVoltageLpf );
// update position estimate
estimateRobotPose();
//mDrivetrain->setEnabled(false);
if (mWheelDrop->isAnyTriggered() ) {
mState = FLYING;
}
switch (mState) {
case START:
if ( mDrivetrain->getOIMode() != CB_MODE_FULL ) {
mDrivetrain->setDefaultOIMode ( CB_MODE_FULL );
}
if (mTracker->isValid() ) {
mOdometry->setPose(mTracker->getPose() );
mEstRobotPose = mTracker->getPose();
}
if (mElapsedTime > 5.0 ) {
if (mPuckLoad == 0)
mState = GOTO_SOURCE;
else
mState = GOTO_SINK;
}
break;
case GOTO_SOURCE:
if (mFgStateChanged)
mWaypointId = 0;
if (mVoltageLpf < CHARGING_VOLTAGE) {
mState = GOTO_CHARGER;
}
//if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) {
if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSourceWaypointVector) ) == COMPLETED) {
mTravelTime += mElapsedTime;
mState = LOADING;
}
else if (mCurrentTaskPtr->getSourcePose().distance(mEstRobotPose) < SOURCE_PROXIMITY) {
mState = WAIT_AT_SOURCE;
}
break;
case WAIT_AT_SOURCE:
if ( (mElapsedTime > MIN_SOURCE_WAIT_TIME) &&
(drand48() < GIVE_UP_PROBABILITY) ) {
switchTask();
}
if ( driveTo(mCurrentTaskPtr->getSourcePose(), 0.2, distance ) == COMPLETED ) {
mTravelTime += mElapsedTime;
mState = LOADING;
}
break;
case GOTO_SINK:
if (mFgStateChanged)
mWaypointId = 0;
//if ( driveTo(mCurrentTaskPtr->getSourcePose(), distance ) == COMPLETED ) {
if ( actionFollowWaypointVector(&(mCurrentTaskPtr->mSinkWaypointVector) ) == COMPLETED) {
//if ( driveTo(mCurrentTaskPtr->getSinkPose(), distance ) == COMPLETED ) {
mTravelTime += mElapsedTime;
mState = UNLOADING;
}
break;
case GOTO_CHARGER:
if (actionGotoCharger() == COMPLETED)
mState = DOCKING;
break;
case DOCKING:
if (actionDocking() == COMPLETED)
break;
case UNDOCKING:
if (actionUndocking() == COMPLETED)
break;
case CHARGING:
if (actionCharging() == COMPLETED)
break;
case LOADING:
if (actionLoading() == COMPLETED)
mState = GOTO_SINK;
break;
case UNLOADING:
if (actionUnloading() == COMPLETED) {
mState = GOTO_SOURCE;
}
break;
case FLYING:
mDrivetrain->stop();
if (not mWheelDrop->isAnyTriggered() ) {
mState = START;
}
break;
default:
PRT_WARN1("Unknown FSM state %d ", mState);
mState = START;
break;
} // switch
if (mTracker->isValid() ) {
mLights->setLight(0, GREEN);
mLights->setLight(3, GREEN);
}
else {
mLights->setLight(0, RED);
mLights->setLight(3, RED);
}
mDataLogger->write( mTime );
if ( mPrevState != mState) {
mPrevState = mState;
mElapsedTime = 0;
mFgStateChanged = true;
rprintf("state %s, task %s\n", StateNames[mState].c_str(),
mCurrentTaskPtr->getName().c_str());
mPowerPack->print();
}
else {
mFgStateChanged = false;
}
}
void FlightTasks::_updateCommand()
{
// lazy subscription to command topic
if (_sub_vehicle_command < 0) {
_sub_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
}
// check if there's any new command
bool updated = false;
orb_check(_sub_vehicle_command, &updated);
if (!updated) {
return;
}
// get command
struct vehicle_command_s command;
orb_copy(ORB_ID(vehicle_command), _sub_vehicle_command, &command);
// check what command it is
FlightTaskIndex desired_task = switchVehicleCommand(command.command);
if (desired_task == FlightTaskIndex::None) {
// ignore all unkown commands
return;
}
// switch to the commanded task
int switch_result = switchTask(desired_task);
uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
// if we are in/switched to the desired task
if (switch_result >= 0) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
// if the task is running apply parameters to it and see if it rejects
if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
// if we just switched and parameters are not accepted, go to failsafe
if (switch_result == 1) {
switchTask(FlightTaskIndex::ManualPosition);
cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
}
}
}
// send back acknowledgment
vehicle_command_ack_s command_ack = {};
command_ack.command = command.command;
command_ack.result = cmd_result;
command_ack.result_param1 = switch_result;
command_ack.target_system = command.source_system;
command_ack.target_component = command.source_component;
if (_pub_vehicle_command_ack == nullptr) {
_pub_vehicle_command_ack = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _pub_vehicle_command_ack, &command_ack);
}
}
//-----------------------------------------------------------------------------
CChatterboxCtrl::CChatterboxCtrl ( ARobot* robot )
: ARobotCtrl ( robot )
{
CTask* task;
ADrivetrain2dof* drivetrain;
// Initialize robot
mTime = 0.0;
mState = START;
mAvoidCount = 0;
mElapsedTime = 0.0;
mPuckLoad = 0;
mFgPoseEstValid = true;
mTrackerHeadingTime = INFINITY;
mTrackerPositionTime = INFINITY;
mTravelTime = 0.0;
mLoadingTime = 0.0;
mUnloadingTime = 0.0;
mStateName = StateNames[mState];
// Get robot devices
mRobot->findDevice ( mPowerPack, "CB:powerpack" );
mRobot->findDevice ( drivetrain, "CB:drivetrain" );
mRobot->findDevice ( mTextDisplay, "CB:textdisplay" );
mRobot->findDevice ( mBumper, "CB:bumper" );
mRobot->findDevice ( mButton, "CB:button" );
mRobot->findDevice ( mLights, "CB:lights" );
mRobot->findDevice ( mWheelDrop, "CB:wheeldrop" );
mRobot->findDevice ( mLowSideDriver, "CB:lowsidedriver" );
mRobot->findDevice ( mFrontFiducial, "CB:front_fiducial" );
mRobot->findDevice ( mTopFiducial, "CB:top_fiducial" );
mRobot->findDevice ( mPhoto, "CB:photosensor" );
mRobot->findDevice ( mCliffSensor, "CB:cliff" );
mRobot->findDevice ( mIr, "CB:ir" );
mDrivetrain = ( CCBDrivetrain2dof* ) drivetrain;
mTracker = new CAutolabTracker("Tracker", mRobot->getName(),
"192.168.1.116", 6379);
//mTracker = new CAutolabTracker("Tracker", "cb18",
// "192.168.1.116", 6379);
mRedisClient = CRedisClient::getInstance("192.168.1.116", 6379);
mRobot->addDevice(mTracker);
// Configure devices
mVoltageLpf = mPowerPack->getVoltage();
mLights->setBlink( DOT, true, 1.0 );
mDrivetrain->setTranslationalAccelerationLimit( CLimit( -INFINITY, 0.5) );
mDrivetrain->setRotationalAccelerationLimit( CLimit(-INFINITY, INFINITY) );
mDrivetrain->setDefaultOIMode( CB_MODE_FULL );
mOdometry = mDrivetrain->getOdometry();
mDataLogger = CDataLogger::getInstance( "chatterbox.log", OVERWRITE );
mDataLogger->setInterval( 0.1 );
mOdometry->startLogging("");
mTracker->startLogging("");
mEstRobotPose.setName("EstRobotPose");
mDataLogger->addVar(&mEstRobotPose, "EstRobotPose");
mDrivetrain->setEnabled(true);
//*********************************************************
// Create Blue task
task = new CTask("blue", BLUE, CPose2d(4.5, 2.5, 0.0),
CPose2d(5.0, 9.2, 0.0) );
task->mSourceWaypointVector.push_back(CPose2d(5.5, 5.0, 0.0));
task->mSourceWaypointVector.push_back(CPose2d(5.5, 2.5, 0.0));
task->mSourceWaypointVector.push_back(CPose2d(4.5, 2.5, 0.0)); // source
task->mSinkWaypointVector.push_back(CPose2d(4.0, 5.0, 0.0));
task->mSinkWaypointVector.push_back(CPose2d(4.0, 9.0, 0.0));
task->mSinkWaypointVector.push_back(CPose2d(5.0, 9.2, 0.0)); // sink
mTaskVector.push_back(task);
//*********************************************************
// Create Purple task
task = new CTask( "purple", CRgbColor(255,0,255),
CPose2d(1.2, 2.0, 0.0),
CPose2d(2.0, 8.0, 0.0) );
task->mSourceWaypointVector.push_back(CPose2d(3.0, 5.0, 0.0));
task->mSourceWaypointVector.push_back(CPose2d(3.0, 2.0, 0.0));
task->mSourceWaypointVector.push_back(CPose2d(1.2, 2.0, 0.0)); // source
task->mSinkWaypointVector.push_back(CPose2d(1.0, 5.0, 0.0));
task->mSinkWaypointVector.push_back(CPose2d(1.0, 8.0, 0.0));
task->mSinkWaypointVector.push_back(CPose2d(2.0, 8.0, 0.0)); // sink
mTaskVector.push_back(task);
mCurrentTaskPtr = NULL;
switchTask();
rprintf("Power: "); mPowerPack->print();
}
