BeeStemI.C

00001 #include "Robots/SeaBeeIII/BeeStemI.H"
00002 #include "Util/Timer.H"
00003 #include "Robots/SeaBeeIII/XBox360RemoteControlI.H"
00004 
00005 // ######################################################################
00006 BeeStemI::BeeStemI(int id, OptionManager& mgr, const std::string& descrName, const std::string& tagName) :
00007         RobotBrainComponent(mgr, descrName, tagName), itsStem(new BeeStem3(mgr, "BeeStem3", "BeeStem3", "/dev/ttyUSB0")),
00008         itsUpdateHeading(0),
00009         itsUpdateDepth(0),
00010         itsUpdateSpeed(0),
00011         itsLastUpdateHeading(0),
00012         itsLastUpdateDepth(0),
00013         itsLastUpdateSpeed(0),
00014         mShooterState(ShooterState::Idle),
00015         mDropperState(DropperState::AllIdle),
00016         mFiringDeviceID(FiringDeviceID::Null)
00017 {
00018         addSubComponent(itsStem);
00019         //  addSubComponent(itsKillSwitch);
00020 
00021         //itsKillSwitch->configure("/dev/ttyUSB1",57600);
00022         itsEvolveSleepTime = 0;
00023         mFlags.initFlag = false;
00024         //itsJSValues.resize(8);
00025         itsButValues.resize(20);
00026         // init trigger values
00027         itsJSMappings[XBox360RemoteControl::Keys::Actions::SURFACE] = -100;
00028         itsJSMappings[XBox360RemoteControl::Keys::Actions::DIVE] = -100;
00029         //itsJSValues[4] = -100; //this only works if those axes are properly mapped
00030         //itsJSValues[5] = -100;
00031 }
00032 
00033 // ######################################################################
00034 BeeStemI::~BeeStemI()
00035 {
00036 }
00037 
00038 // ######################################################################
00039 void BeeStemI::initPose()
00040 {
00041         mFlags.initFlag = true;
00042 
00043         itsStemMutex.lock();
00044         itsStem->setPID(0, HEADING_K, HEADING_P, HEADING_I, HEADING_D);
00045         itsStem->setPID(1, DEPTH_K, DEPTH_P, DEPTH_I, DEPTH_D);
00046 
00047         int accelX, accelY, accelZ;
00048         int compassHeading, compassPitch, compassRoll;
00049         int internalPressure, externalPressure;
00050         int headingK, headingP, headingD, headingI, headingOutput;
00051         int depthK, depthP, depthD, depthI, depthOutput;
00052         char killSwitch;
00053 
00054         bool successful = itsStem->getSensors(accelX, accelY, accelZ, compassHeading, compassPitch, compassRoll, internalPressure, externalPressure, itsDesiredHeading, itsDesiredDepth, itsDesiredSpeed, headingK, headingP, headingD, headingI, headingOutput, depthK, depthP, depthD, depthI, depthOutput, killSwitch);
00055 
00056         if (successful)
00057         {
00058                 itsUpdateHeading = compassHeading;
00059                 itsLastUpdateHeading = itsUpdateHeading;
00060                 itsStem->setDesiredHeading(compassHeading);
00061                 itsUpdateDepth = externalPressure;
00062                 itsLastUpdateDepth = itsUpdateDepth;
00063                 itsStem->setDesiredDepth(externalPressure);
00064         }
00065 
00066         itsStemMutex.unlock();
00067 }
00068 
00069 void BeeStemI::registerTopics()
00070 {
00071         LINFO("Registering BeeStem Message");
00072         this->registerPublisher("BeeStemMessageTopic");
00073         registerSubscription("BeeStemMotorControllerMessageTopic");
00074         registerSubscription("XBox360RemoteControlMessageTopic");
00075         registerSubscription("BeeStemConfigTopic");
00076 }
00077 
00078 void BeeStemI::setValuesFromJoystick()
00079 {
00080         int rightVal = itsJSMappings[XBox360RemoteControl::Keys::Actions::SPEED] * -1;//itsJSValues[1]*-1;
00081         int leftVal = itsJSMappings[XBox360RemoteControl::Keys::Actions::SPEED];//itsJSValues[1];
00082 
00083         itsStem->setThruster(BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER, rightVal);
00084         itsStem->setThruster(BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER, leftVal);
00085 
00086         //  itsJSValues[5], itsJSValues[4]
00087         //cout << itsJSMappings[XBox360RemoteControlI::Keys::Axes::Actions::SURFACE] << endl;
00088         //cout << itsJSMappings[XBox360RemoteControlI::Keys::Axes::Actions::DIVE] << endl;
00089         int depthVal = int((float(itsJSMappings[XBox360RemoteControl::Keys::Actions::SURFACE]) + 100.0) / 2.0 - (float(itsJSMappings[XBox360RemoteControl::Keys::Actions::DIVE]) + 100.0) / 2.0);
00090         //cout << depthVal << endl;
00091         //cout << "-----" << endl;
00092 
00093         int depthValRight = depthVal;
00094         int depthValLeft = depthVal;
00095 
00096         //   if(itsButValues[5])
00097         //     {
00098         //       depthValRight = 75;
00099         //       depthValLeft = -75;
00100         //     }
00101         //   else if(itsButValues[4])
00102         //     {
00103         //       depthValRight = -75;
00104         //       depthValLeft = 75;
00105         //     }
00106 
00107         itsStem->setThruster(BeeStem3::MotorControllerIDs::DEPTH_RIGHT_THRUSTER, depthValRight);
00108         itsStem->setThruster(BeeStem3::MotorControllerIDs::DEPTH_LEFT_THRUSTER, depthValLeft);
00109 
00110         //itsJSValues[2], itsJSValues[0]
00111         int frontVal = itsJSMappings[XBox360RemoteControl::Keys::Actions::HEADING] + itsJSMappings[XBox360RemoteControl::Keys::Actions::STRAFE] * -1;
00112         int backVal = itsJSMappings[XBox360RemoteControl::Keys::Actions::HEADING] * -1 + itsJSMappings[XBox360RemoteControl::Keys::Actions::STRAFE] * -1;
00113 
00114         itsStem->setThruster(BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER, frontVal);
00115         itsStem->setThruster(BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER, backVal);
00116 }
00117 
00118 void BeeStemI::evolve()
00119 {
00120         if (!mFlags.initFlag)
00121                 initPose();
00122 
00123         if(mFlags.needsUpdateFromJoystick)
00124         {
00125                 setValuesFromJoystick();
00126                 mFlags.needsUpdateFromJoystick = false;
00127         }
00128 
00129         /****************
00130         ** arm devices **
00131         ****************/
00132 
00133         //first, put all devices at last idle state
00134         mShooterState = mShooterState == ShooterState::Armed ? ShooterState::Idle : mShooterState;
00135         mDropperState = mDropperState == DropperState::Stage1Armed ? DropperState::AllIdle : mDropperState;
00136         mDropperState = mDropperState == DropperState::Stage2Armed ? DropperState::Stage1Idle : mDropperState;
00137 
00138         //then, arm the currently selected device
00139         switch (mFiringDeviceID)
00140         {
00141         case FiringDeviceID::Shooter:
00142                 mShooterState = mShooterState == ShooterState::Idle ? ShooterState::Armed : mShooterState;
00143                 break;
00144         case FiringDeviceID::DropperStage1:
00145                 mDropperState = mDropperState == DropperState::AllIdle ? DropperState::Stage1Armed : mDropperState;
00146                 break;
00147         case FiringDeviceID::DropperStage2:
00148                 mDropperState = mDropperState == DropperState::Stage1Idle ? DropperState::Stage2Armed : mDropperState;
00149                 break;
00150         }
00151 
00152         /*******************************
00153         ** set firing mode on devices **
00154         *******************************/
00155         if(abs(itsJSMappings[XBox360RemoteControl::Keys::Actions::FIRE_DEV]) > 0) //fire key pressed
00156         {
00157                 if (mShooterState == ShooterState::Armed)
00158                 {
00159                         cout << "Shooter set to fire!" << endl;
00160                         mShooterState = ShooterState::Firing;
00161                 }
00162                 else if(mDropperState == DropperState::Stage1Armed)
00163                 {
00164                         cout << "Dropper stage1 set to drop!" << endl;
00165                         mDropperState = DropperState::Stage1Dropping;
00166                 }
00167                 else if(mDropperState == DropperState::Stage2Armed)
00168                 {
00169                         cout << "Dropper stage2 set to drop!" << endl;
00170                         mDropperState = DropperState::Stage2Dropping;
00171                 }
00172         }
00173         else if(abs(itsJSMappings[XBox360RemoteControl::Keys::Actions::FIRE_DEV]) == 0) //fire key released
00174         {
00175                 if (mShooterState == ShooterState::Reset)
00176                 {
00177                         cout << "Shooter reset" << endl;
00178                         mShooterState = ShooterState::Idle;
00179                 }
00180                 else if(mDropperState == DropperState::Stage1Reset)
00181                 {
00182                         cout << "Dropper stage1 reset" << endl;
00183                         mDropperState = DropperState::Stage1Idle;
00184                         /* //uncomment to auto-advance between stage1 and stage2
00185                          *
00186                          * mFiringDeviceID = FiringDeviceID::DropperStage2
00187                          */
00188                 }
00189                 else if(mDropperState == DropperState::Stage2Reset)
00190                 {
00191                         cout << "Dropper stage2 reset" << endl;
00192                         mDropperState = DropperState::AllIdle;
00193                 }
00194         }
00195 
00196         /*****************************************
00197         ** fire devices, then reset firing mode **
00198         *****************************************/
00199         if (mShooterState == ShooterState::Firing)
00200         {
00201                 fireDevice(FiringDeviceID::Shooter);
00202                 mShooterState = ShooterState::Reset;
00203         }
00204         else if(mDropperState == DropperState::Stage1Dropping)
00205         {
00206                 fireDevice(FiringDeviceID::DropperStage1);
00207                 mDropperState = DropperState::Stage1Reset;
00208         }
00209         else if(mDropperState == DropperState::Stage2Dropping)
00210         {
00211                 fireDevice(FiringDeviceID::DropperStage2);
00212                 mDropperState = DropperState::Stage2Reset;
00213         }
00214 
00215         //done
00216 
00217         int accelX, accelY, accelZ;
00218         int compassHeading, compassPitch, compassRoll;
00219         int internalPressure, externalPressure;
00220         int headingK, headingP, headingD, headingI, headingOutput;
00221         int depthK, depthP, depthD, depthI, depthOutput;
00222         char killSwitch;
00223         //  int thruster1,  thruster2,  thruster3,  thruster4,  thruster5,  thruster6;
00224 
00225         itsStemMutex.lock();
00226         bool successful = itsStem->getSensors(accelX, accelY, accelZ, compassHeading, compassPitch, compassRoll, internalPressure, externalPressure, itsDesiredHeading, itsDesiredDepth, itsDesiredSpeed, headingK, headingP, headingD, headingI, headingOutput, depthK, depthP, depthD, depthI, depthOutput, killSwitch);
00227 
00228         itsUpdateMutex.lock();
00229         int tempHeading = itsUpdateHeading;
00230         int tempDepth = itsUpdateDepth;
00231         int tempSpeed = itsUpdateSpeed;
00232         itsUpdateMutex.unlock();
00233 
00234         if (itsLastUpdateHeading != tempHeading)
00235         {
00236                 itsLastUpdateHeading = tempHeading;
00237                 itsStem->setDesiredHeading(tempHeading);
00238         }
00239         if (itsLastUpdateDepth != tempDepth)
00240         {
00241                 itsLastUpdateDepth = tempDepth;
00242                 itsStem->setDesiredDepth(tempDepth);
00243         }
00244         if (itsLastUpdateSpeed != tempSpeed)
00245         {
00246                 itsLastUpdateSpeed = tempSpeed;
00247                 itsStem->setDesiredSpeed(tempSpeed);
00248         }
00249 
00250         itsStemMutex.unlock();
00251         if (successful)
00252         {
00253 
00254                 RobotSimEvents::BeeStemMessagePtr msg = new RobotSimEvents::BeeStemMessage;
00255 
00256                 msg->accelX = accelX;
00257                 msg->accelY = accelY;
00258                 msg->accelZ = accelZ;
00259                 msg->compassHeading = compassHeading;
00260                 msg->compassPitch = compassPitch;
00261                 msg->compassRoll = compassRoll;
00262                 msg->internalPressure = internalPressure;
00263                 msg->externalPressure = externalPressure;
00264                 msg->desiredHeading = itsDesiredHeading;
00265                 msg->desiredDepth = itsDesiredDepth;
00266                 msg->desiredSpeed = itsDesiredSpeed;
00267                 msg->headingK = headingK;
00268                 msg->headingP = headingP;
00269                 msg->headingD = headingD;
00270                 msg->headingI = headingI;
00271                 msg->headingOutput = headingOutput;
00272                 msg->depthK = depthK;
00273                 msg->depthP = depthP;
00274                 msg->depthD = depthD;
00275                 msg->depthI = depthI;
00276                 msg->depthOutput = depthOutput;
00277                 msg->killSwitch = killSwitch;
00278 
00279                 this->publish("BeeStemMessageTopic", msg);
00280 
00281                 //       char* str = new char[32];
00282                 //       sprintf(str, "\n%04d", externalPressure);
00283 
00284                 //       itsKillSwitch->write(str,32);
00285 
00286                 //       delete str;
00287         }
00288         else if (!successful)
00289                 LINFO("Error reading from BeeStem");
00290 
00291 }
00292 
00293 void BeeStemI::fireDevice(int deviceID)
00294 {
00295         switch(deviceID)
00296         {
00297         case FiringDeviceID::Shooter:
00298                 cout << "Firing torpedo!" << endl;
00299                 itsStem->setThruster(BeeStem3::MotorControllerIDs::SHOOTER, 95);
00300                 usleep(50 * 1000);
00301                 itsStem->setThruster(BeeStem3::MotorControllerIDs::SHOOTER, 0);
00302                 mShooterState = ShooterState::Reset;
00303                 break;
00304         case FiringDeviceID::DropperStage1:
00305                 cout << "Dropping first marker!" << endl;
00306                 itsStem->setThruster(BeeStem3::MotorControllerIDs::DROPPER_STAGE1, 95);
00307                 usleep(50 * 1000);
00308                 itsStem->setThruster(BeeStem3::MotorControllerIDs::DROPPER_STAGE1, 0);
00309                 mDropperState = DropperState::Stage1Reset;
00310                 break;
00311         case FiringDeviceID::DropperStage2:
00312                 cout << "Dropping second marker!" << endl;
00313                 itsStem->setThruster(BeeStem3::MotorControllerIDs::DROPPER_STAGE2, 95);
00314                 usleep(50 * 1000);
00315                 itsStem->setThruster(BeeStem3::MotorControllerIDs::DROPPER_STAGE2, 0);
00316                 mDropperState = DropperState::Stage2Reset;
00317                 break;
00318         }
00319 }
00320 
00321 // ######################################################################
00322 void BeeStemI::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00323 {
00324         // Get a XBox360RemoteControl Message
00325         if (eMsg->ice_isA("::RobotSimEvents::JoyStickControlMessage"))
00326         {
00327                 RobotSimEvents::JoyStickControlMessagePtr msg = RobotSimEvents::JoyStickControlMessagePtr::dynamicCast(eMsg);
00328                 LINFO("Got message %d %s %d",msg->axis,msg->axisName.c_str(), msg->axisVal);
00329 
00330                 itsJSMappings[XBox360RemoteControl::Keys::Actions::toInt[msg->axisName]] = msg->axisVal;
00331 
00332                 static bool deviceCycleBtnWaitingReset = false; //prevent it from cycling really f-ing fast through devices
00333                 if(XBox360RemoteControl::Keys::Actions::toInt[msg->axisName] == XBox360RemoteControl::Keys::Actions::ARM_NEXT_DEV)
00334                 {
00335                         if(msg->axisVal > 0 && !deviceCycleBtnWaitingReset)
00336                         {
00337                                 mFiringDeviceID = mFiringDeviceID >= FiringDeviceID::MAX ? FiringDeviceID::Null : mFiringDeviceID + 1;
00338                                 deviceCycleBtnWaitingReset = true;
00339                         }
00340                         else if(msg->axisVal < 0 && !deviceCycleBtnWaitingReset)
00341                         {
00342                                 mFiringDeviceID = mFiringDeviceID <= FiringDeviceID::Null ? FiringDeviceID::MAX : mFiringDeviceID - 1;
00343                                 deviceCycleBtnWaitingReset = true;
00344                         }
00345                         else if(msg->axisVal == 0 && deviceCycleBtnWaitingReset)
00346                         {
00347                                 deviceCycleBtnWaitingReset = false;
00348                                 printf(mFiringDeviceID == FiringDeviceID::Null ? "All devices set to be disarmed\n" : "Device %d set to be armed\n", mFiringDeviceID);
00349                         }
00350                 }
00351 
00352                 if (msg->button >= 0)
00353                 {
00354                         itsButValues[msg->button] = msg->butVal;
00355                 }
00356 
00357                 mFlags.needsUpdateFromJoystick = true;
00358         }
00359         else if (eMsg->ice_isA("::RobotSimEvents::BeeStemMotorControllerMessage"))
00360         {
00361                 RobotSimEvents::BeeStemMotorControllerMessagePtr msg = RobotSimEvents::BeeStemMotorControllerMessagePtr::dynamicCast(eMsg);
00362                 //cout << "setting motor values..." << endl;
00363                 for (unsigned int i = 0; i < msg->values.size(); i++)
00364                 {
00365                         // printf("%d|%d : ", i, msg->values[i]);
00366                         if(msg->mask[i] == 1)
00367                                 itsStem->setThruster(i, msg->values[i]);
00368                 }
00369                 //cout << endl;
00370         }
00371         // Get a BeeStemConfig Message
00372         else if (eMsg->ice_isA("::RobotSimEvents::BeeStemConfigMessage"))
00373         {
00374                 RobotSimEvents::BeeStemConfigMessagePtr msg = RobotSimEvents::BeeStemConfigMessagePtr::dynamicCast(eMsg);
00375 
00376                 //      int h,d,s;
00377                 if (msg->deviceToFire && msg->deviceToFire != FiringDeviceID::Null) //so...yeah we can't fire a device and update PID at the same time, NBD
00378                 {
00379                         fireDevice(msg->deviceToFire);
00380                 }
00381                 else if (msg->enablePID == 1)
00382                 {
00383                         itsStemMutex.lock();
00384                         if (msg->enableVal == 1)
00385                                 itsStem->setPID(3, msg->headingK, msg->headingP, msg->headingI, msg->headingD);
00386                         else
00387                                 itsStem->setPID(2, msg->headingK, msg->headingP, msg->headingI, msg->headingD);
00388                         itsStemMutex.unlock();
00389                 }
00390                 else if (msg->updateHeadingPID == 0 && msg->updateDepthPID == 0)
00391                 {
00392                         itsUpdateMutex.lock();
00393 
00394                         LINFO("Update Pose: %d\n",msg->updateDesiredValue);
00395                         if (msg->updateDesiredValue == 1)
00396                                 itsUpdateHeading = msg->desiredHeading;
00397                         else if (msg->updateDesiredValue == 2)
00398                                 itsUpdateDepth = msg->desiredDepth;
00399                         else if (msg->updateDesiredValue == 3)
00400                                 itsUpdateSpeed = msg->desiredSpeed;
00401 
00402                         itsUpdateMutex.unlock();
00403                 }
00404                 else if (msg->updateDepthPID == 1)
00405                 {
00406                         itsStemMutex.lock();
00407                         itsStem->setPID(0, msg->depthK, msg->depthP, msg->depthI, msg->depthD);
00408                         itsStemMutex.unlock();
00409                 }
00410                 else if (msg->updateHeadingPID == 1)
00411                 {
00412                         itsStemMutex.lock();
00413                         itsStem->setPID(1, msg->headingK, msg->headingP, msg->headingI, msg->headingD);
00414                         itsStemMutex.unlock();
00415                 }
00416         }
00417 }
00418 
00419 void BeeStemI::getMotorControllerMsg(RobotSimEvents::BeeStemMotorControllerMessagePtr & msg, int mc0, int mc1, int mc2, int mc3, int mc4, int mc5, int mc6, int mc7, int mc8)
00420 {
00421         vector<int> values;
00422         values.resize(BeeStem3::NUM_MOTOR_CONTROLLERS);
00423         values[BeeStem3::MotorControllerIDs::FWD_LEFT_THRUSTER] = mc0;
00424         values[BeeStem3::MotorControllerIDs::FWD_RIGHT_THRUSTER] = mc1;
00425         values[BeeStem3::MotorControllerIDs::STRAFE_BACK_THRUSTER] = mc2;
00426         values[BeeStem3::MotorControllerIDs::STRAFE_FRONT_THRUSTER] = mc3;
00427         values[BeeStem3::MotorControllerIDs::DEPTH_LEFT_THRUSTER] = mc4;
00428         values[BeeStem3::MotorControllerIDs::DEPTH_RIGHT_THRUSTER] = mc5;
00429         values[BeeStem3::MotorControllerIDs::SHOOTER] = mc6;
00430         values[BeeStem3::MotorControllerIDs::DROPPER_STAGE1] = mc7;
00431         values[BeeStem3::MotorControllerIDs::DROPPER_STAGE2] = mc8;
00432         msg->values = values;
00433 }
00434