00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "SeaBee/SubController.H"
00039 #include "Component/ModelOptionDef.H"
00040 #include "Devices/DeviceOpts.H"
00041 #include "Util/Assert.H"
00042 #include "Image/DrawOps.H"
00043
00044 #define CMD_DELAY 6000
00045 #define INT_CLVL_PRESSURE 225
00046
00047 const ModelOptionCateg MOC_SeaBee_Controller = {
00048 MOC_SORTPRI_3, "SeaBee controller related options" };
00049
00050 const ModelOptionDef OPT_SeaBeeSimMode =
00051 { MODOPT_FLAG, "SeabeeSimMode", &MOC_SeaBee_Controller, OPTEXP_CORE,
00052 "Run in simulator mode",
00053 "seabee-sim-mode", '\0', "", "false" };
00054
00055 namespace
00056 {
00057 class SubControllerPIDLoop : public JobWithSemaphore
00058 {
00059 public:
00060 SubControllerPIDLoop(SubController* subCtrl)
00061 :
00062 itsSubController(subCtrl),
00063 itsPriority(1),
00064 itsJobType("controllerLoop")
00065 {}
00066
00067 virtual ~SubControllerPIDLoop() {}
00068
00069 virtual void run()
00070 {
00071 ASSERT(itsSubController);
00072 while(1)
00073 {
00074 itsSubController->sendHeartBeat();
00075 itsSubController->updatePID();
00076 }
00077 }
00078
00079 virtual const char* jobType() const
00080 { return itsJobType.c_str(); }
00081
00082 virtual int priority() const
00083 { return itsPriority; }
00084
00085 private:
00086 SubController* itsSubController;
00087 const int itsPriority;
00088 const std::string itsJobType;
00089 };
00090 }
00091
00092
00093
00094 SubController::SubController(OptionManager& mgr,
00095 const std::string& descrName,
00096 const std::string& tagName,
00097 const bool simulation):
00098
00099 ModelComponent(mgr, descrName, tagName),
00100 itsSimulation(&OPT_SeaBeeSimMode, this, ALLOW_ONLINE_CHANGES),
00101 itsDesiredPitch(30),
00102 itsDesiredRoll(0),
00103 itsDesiredHeading(20),
00104 itsDesiredDepth(200),
00105 itsDesiredSpeed(0),
00106 itsDesiredTurningSpeed(0),
00107 itsCurrentPitch(0),
00108 itsCurrentRoll(0),
00109 itsCurrentHeading(0),
00110 itsCurrentDepth(-1),
00111 itsCurrentSpeed(0),
00112 speedScale("speedScale", this, 1, ALLOW_ONLINE_CHANGES),
00113 itsSpeedScale(speedScale.getVal()),
00114 depthRatio("depthRatio", this, 1.3, ALLOW_ONLINE_CHANGES),
00115 itsDepthRatio(depthRatio.getVal()),
00116 itsPitchPID(0.5f, 0.0, 0.0, -20, 20),
00117 itsRollPID(0.0f, 0, 0, -20, 20),
00118 headingP("headingP", this, 2.0, ALLOW_ONLINE_CHANGES),
00119 headingI("headingI", this, 0, ALLOW_ONLINE_CHANGES),
00120 headingD("headingD", this, 0, ALLOW_ONLINE_CHANGES),
00121 itsHeadingPID(headingP.getVal(), headingI.getVal(), headingD.getVal(), -20, 20),
00122 depthP("depthP", this, -13.0, ALLOW_ONLINE_CHANGES),
00123 depthI("depthI", this, -0.5, ALLOW_ONLINE_CHANGES),
00124 depthD("depthD", this, -8.0, ALLOW_ONLINE_CHANGES),
00125 itsDepthPID(depthP.getVal(), depthI.getVal(), depthD.getVal(), -20, 20),
00126 itsCurrentThruster_Up_Left(0),
00127 itsCurrentThruster_Up_Right(0),
00128 itsCurrentThruster_Up_Back(0),
00129 itsCurrentThruster_Fwd_Right(0),
00130 itsCurrentThruster_Fwd_Left(0),
00131 setCurrentThruster_Up_Left("ThrusterUpLeft", this, 0, ALLOW_ONLINE_CHANGES),
00132 setCurrentThruster_Up_Right("ThrusterUpRight", this, 0, ALLOW_ONLINE_CHANGES),
00133 setCurrentThruster_Up_Back("ThrusterUpBack", this, 0, ALLOW_ONLINE_CHANGES),
00134 setCurrentThruster_Fwd_Right("ThrusterFwdRight", this, 0, ALLOW_ONLINE_CHANGES),
00135 setCurrentThruster_Fwd_Left("ThrusterFwdLeft", this, 0, ALLOW_ONLINE_CHANGES),
00136 pitchP("pitchP", this, 0, ALLOW_ONLINE_CHANGES),
00137 pitchI("pitchI", this, 0, ALLOW_ONLINE_CHANGES),
00138 pitchD("pitchD", this, 0, ALLOW_ONLINE_CHANGES),
00139 rollP("rollP", this, 0, ALLOW_ONLINE_CHANGES),
00140 rollI("rollI", this, 0, ALLOW_ONLINE_CHANGES),
00141 rollD("rollD", this, 0, ALLOW_ONLINE_CHANGES),
00142 motorsOn("motorsOn", this, false, ALLOW_ONLINE_CHANGES),
00143 pidOn("pidOn", this, true, ALLOW_ONLINE_CHANGES),
00144 guiOn("guiOn", this, true, ALLOW_ONLINE_CHANGES),
00145 depthPIDDisplay("Depth PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00146 pitchPIDDisplay("Pitch PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00147 headingPIDDisplay("Heading PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00148 rollPIDDisplay("Roll PID Disp", this, false, ALLOW_ONLINE_CHANGES),
00149
00150 setDepthValue("DepthValue", this, 450, ALLOW_ONLINE_CHANGES),
00151 setPitchValue("PitchValue", this, itsDesiredPitch, ALLOW_ONLINE_CHANGES),
00152 setRollValue("RollValue", this, 0, ALLOW_ONLINE_CHANGES),
00153 setHeadingValue("HeadingValue", this, 20, ALLOW_ONLINE_CHANGES),
00154 itsPIDImage(256, 256, ZEROS),
00155 itsSubImage(256, 256, ZEROS),
00156 itsPrevDepth(0),
00157 itsPrevPrevDepth(0),
00158 itsDepthCount(0)
00159 {
00160 if (itsSimulation.getVal())
00161 {
00162 itsBeeStemSim = nub::soft_ref<BeeStemSim>(new BeeStemSim(mgr));
00163 addSubComponent(itsBeeStemSim);
00164
00165 }
00166 else
00167 {
00168 itsBeeStemTiny = nub::soft_ref<BeeStemTiny>(new BeeStemTiny(mgr,"BeeStemTiny", "BeeStemTiny", "/dev/ttyS1"));
00169 addSubComponent(itsBeeStemTiny);
00170
00171 itsForwardCam = nub::ref<V4L2grabber>(new V4L2grabber(mgr));
00172 itsForwardCam->exportOptions(MC_RECURSE);
00173 addSubComponent(itsForwardCam);
00174
00175 itsBottomCam = nub::ref<V4L2grabber>(new V4L2grabber(mgr));
00176 itsBottomCam->exportOptions(MC_RECURSE);
00177 addSubComponent(itsBottomCam);
00178 }
00179
00180 }
00181
00182 void SubController::start1()
00183 {
00184 if (!(itsSimulation.getVal()))
00185 {
00186 itsForwardCam->setModelParamVal("FrameGrabberDevice",std::string("/dev/video0"));
00187 itsBottomCam->setModelParamVal("FrameGrabberDevice",std::string("/dev/video1"));
00188 }
00189 }
00190
00191 void SubController::start2()
00192 {
00193
00194 killMotors();
00195 sleep(1);
00196
00197
00198 itsThreadServer.reset(new WorkThreadServer("SubController",1));
00199 itsThreadServer->setFlushBeforeStopping(false);
00200 rutz::shared_ptr<SubControllerPIDLoop> j(new SubControllerPIDLoop(this));
00201 itsThreadServer->enqueueJob(j);
00202
00203 setHeading(getHeading());
00204
00205 if (!(itsSimulation.getVal()))
00206 {
00207 itsForwardCam->startStream();
00208 itsBottomCam->startStream();
00209 }
00210
00211 }
00212
00213
00214 SubController::~SubController()
00215 {
00216
00217
00218 }
00219
00220 void SubController::sendHeartBeat()
00221 {
00222
00223 usleep(10000);
00224 }
00225
00226
00227 void SubController::initSensorVals()
00228 {
00229 itsBeeStemTiny->getSensors(itsCurrentHeading,
00230 itsCurrentPitch,
00231 itsCurrentRoll,
00232 itsCurrentDepth,
00233 itsCurrentIntPressure);
00234 }
00235
00236
00237 bool SubController::setHeading(int heading)
00238 {
00239
00240 heading = heading % 360;
00241
00242 itsDesiredHeading = heading;
00243 return true;
00244 }
00245
00246
00247 bool SubController::setPitch(int pitch)
00248 {
00249 itsDesiredPitch = pitch;
00250 return true;
00251 }
00252
00253 bool SubController::setDepth(int depth)
00254 {
00255 itsDesiredDepth = depth;
00256 return true;
00257 }
00258
00259
00260
00261 bool SubController::setRoll(int roll)
00262 {
00263 itsDesiredRoll = roll;
00264 return true;
00265 }
00266
00267 bool SubController::setSpeed(int speed)
00268 {
00269 itsDesiredSpeed = speed;
00270 itsCurrentSpeed = itsDesiredSpeed;
00271 return true;
00272 }
00273
00274 bool SubController::setTurningSpeed(int speed)
00275 {
00276 itsDesiredTurningSpeed = speed;
00277 return true;
00278 }
00279
00280
00281
00282
00283 void SubController::updateHeading(unsigned int heading)
00284 {
00285 itsCurrentHeading = heading;
00286 }
00287
00288 void SubController::updatePitch(int pitch)
00289 {
00290 itsCurrentPitch = pitch;
00291 }
00292
00293 void SubController::updateRoll(int roll)
00294 {
00295 itsCurrentRoll = roll;
00296 }
00297
00298 void SubController::updateDepth(unsigned int depth)
00299 {
00300
00301 itsAvgDepth.push_back(depth);
00302
00303 if (itsAvgDepth.size() > 40)
00304 {
00305
00306 long avg = 0;
00307 for(std::list<int>::iterator itr=itsAvgDepth.begin(); itr != itsAvgDepth.end(); ++itr)
00308 avg += *itr;
00309 itsAvgDepth.pop_front();
00310
00311 itsCurrentDepth = avg/itsAvgDepth.size();
00312 }
00313 }
00314
00315 void SubController::updatePID()
00316 {
00317
00318 if (itsSimulation.getVal())
00319 itsBeeStemSim->getSensors(itsCurrentHeading, itsCurrentPitch, itsCurrentRoll, itsCurrentDepth, itsCurrentIntPressure);
00320 else
00321 itsBeeStemTiny->getSensors(itsCurrentHeading, itsCurrentPitch, itsCurrentRoll, itsCurrentDepth, itsCurrentIntPressure);
00322
00323 if(guiOn.getVal())
00324 {
00325 genPIDImage();
00326 genSubImage();
00327 }
00328
00329
00330 if (pidOn.getVal())
00331 {
00332
00333 int desiredHeading;
00334
00335 if(itsCurrentHeading >= 180) {
00336 desiredHeading = itsDesiredHeading + 360 - itsCurrentHeading;
00337 }
00338 else {
00339 desiredHeading = itsDesiredHeading - itsCurrentHeading;
00340 }
00341
00342 while(desiredHeading > 360)
00343 desiredHeading -= 360;
00344 while(desiredHeading < 0)
00345 desiredHeading += 360;
00346
00347 if(desiredHeading >180)
00348 desiredHeading = -(360 - desiredHeading);
00349
00350 float headingCorrection = (float)itsHeadingPID.update(desiredHeading, 0);
00351 float pitchCorrection = itsPitchPID.update((float)itsDesiredPitch, (float)itsCurrentPitch);
00352
00353 float depthCorrection = itsDepthPID.update((float)itsDesiredDepth, (float)itsCurrentDepth);
00354
00355 if (depthCorrection > 75) depthCorrection = 75;
00356 if (depthCorrection < -75) depthCorrection = -75;
00357
00358
00359
00360 int polCorr = 1;
00361
00362 if (!itsSimulation.getVal())
00363 {
00364 polCorr = -1;
00365 }
00366
00367 int thruster_Fwd_Left;
00368 int thruster_Fwd_Right;
00369 if(itsDesiredTurningSpeed != 0)
00370 {
00371 thruster_Fwd_Left = polCorr*(THRUSTER_FWD_LEFT_THRESH - (int)(itsDesiredTurningSpeed) - itsCurrentSpeed);
00372 thruster_Fwd_Right = THRUSTER_FWD_RIGHT_THRESH + (int)(itsDesiredTurningSpeed) - itsCurrentSpeed;
00373 }
00374 else
00375 {
00376 thruster_Fwd_Left = polCorr*(THRUSTER_FWD_LEFT_THRESH - (int)(headingCorrection) - itsCurrentSpeed);
00377 thruster_Fwd_Right = THRUSTER_FWD_RIGHT_THRESH + (int)(headingCorrection) - itsCurrentSpeed;
00378 }
00379
00380 itsCurrentThruster_Fwd_Left = (int)(thruster_Fwd_Left * itsSpeedScale);
00381 itsCurrentThruster_Fwd_Right = (int)(thruster_Fwd_Right * itsSpeedScale);
00382
00383 if (itsDesiredDepth != -1)
00384 {
00385 int thruster_Up_Left = -1*(THRUSTER_UP_LEFT_THRESH + (int)((depthCorrection/itsDepthRatio) + pitchCorrection));
00386 int thruster_Up_Right = THRUSTER_UP_RIGHT_THRESH + (int)((depthCorrection/itsDepthRatio) + pitchCorrection);
00387 int thruster_Up_Back = THRUSTER_UP_BACK_THRESH + (int)(depthCorrection - pitchCorrection);
00388
00389 itsCurrentThruster_Up_Left = (int)(thruster_Up_Left * itsSpeedScale);
00390 itsCurrentThruster_Up_Right = (int)(thruster_Up_Right * itsSpeedScale);
00391 itsCurrentThruster_Up_Back = (int)(thruster_Up_Back * itsSpeedScale);
00392
00393
00394
00395
00396 }
00397 }
00398
00399
00400
00401 if (!motorsOn.getVal())
00402 {
00403 itsCurrentThruster_Up_Left = 0;
00404 itsCurrentThruster_Fwd_Left = 0;
00405 itsCurrentThruster_Up_Back = 0;
00406 itsCurrentThruster_Fwd_Right = 0;
00407 itsCurrentThruster_Up_Right = 0;
00408 }
00409
00410 if (itsSimulation.getVal())
00411 itsBeeStemSim->setThrusters(
00412 itsCurrentThruster_Up_Left,
00413 itsCurrentThruster_Fwd_Left,
00414 itsCurrentThruster_Up_Back,
00415 itsCurrentThruster_Fwd_Right,
00416 itsCurrentThruster_Up_Right);
00417 else
00418 itsBeeStemTiny->setThrusters(
00419 itsCurrentThruster_Up_Left,
00420 itsCurrentThruster_Fwd_Left,
00421 itsCurrentThruster_Up_Back,
00422 itsCurrentThruster_Fwd_Right,
00423 itsCurrentThruster_Up_Right);
00424
00425
00426
00427 }
00428
00429 void SubController::setThruster(int thruster, int val)
00430 {
00431
00432 switch(thruster)
00433 {
00434 case THRUSTER_UP_LEFT: itsCurrentThruster_Up_Left = val; break;
00435 case THRUSTER_UP_RIGHT: itsCurrentThruster_Up_Right = val; break;
00436 case THRUSTER_UP_BACK: itsCurrentThruster_Up_Back = val; break;
00437 case THRUSTER_FWD_RIGHT: itsCurrentThruster_Fwd_Right = val; break;
00438 case THRUSTER_FWD_LEFT: itsCurrentThruster_Fwd_Left = val; break;
00439 default: LINFO("Invalid motor %i", thruster); break;
00440 }
00441
00442 }
00443
00444 void SubController::setIntPressure(unsigned int pressure)
00445 {
00446 itsCurrentIntPressure = pressure;
00447 }
00448
00449
00450 void SubController::killMotors()
00451 {
00452
00453 }
00454
00455
00456 void SubController::paramChanged(ModelParamBase* const param, const bool valueChanged, ParamClient::ChangeStatus* status)
00457 {
00458
00459
00460 if (param == &pitchP && valueChanged)
00461 itsPitchPID.setPIDPgain(pitchP.getVal());
00462 else if(param == &pitchI && valueChanged)
00463 itsPitchPID.setPIDIgain(pitchI.getVal());
00464 else if(param == &pitchD && valueChanged)
00465 itsPitchPID.setPIDDgain(pitchD.getVal());
00466
00467
00468 else if(param == &rollP && valueChanged)
00469 itsRollPID.setPIDPgain(rollP.getVal());
00470 else if(param == &rollI && valueChanged)
00471 itsRollPID.setPIDIgain(rollI.getVal());
00472 else if(param == &rollD && valueChanged)
00473 itsRollPID.setPIDDgain(rollD.getVal());
00474
00475
00476 else if(param == &headingP && valueChanged)
00477 itsHeadingPID.setPIDPgain(headingP.getVal());
00478 else if(param == &headingI && valueChanged)
00479 itsHeadingPID.setPIDIgain(headingI.getVal());
00480 else if(param == &headingD && valueChanged)
00481 itsHeadingPID.setPIDDgain(headingD.getVal());
00482
00483
00484 else if(param == &depthP && valueChanged)
00485 itsDepthPID.setPIDPgain(depthP.getVal());
00486 else if(param == &depthI && valueChanged)
00487 itsDepthPID.setPIDIgain(depthI.getVal());
00488 else if(param == &depthD && valueChanged)
00489 itsDepthPID.setPIDDgain(depthD.getVal());
00490 else if(param == &setDepthValue && valueChanged)
00491 setDepth(setDepthValue.getVal());
00492 else if(param == &setPitchValue && valueChanged)
00493 setPitch(setPitchValue.getVal());
00494 else if(param == &setRollValue && valueChanged)
00495 setRoll(setRollValue.getVal());
00496 else if(param == &setHeadingValue && valueChanged)
00497 setHeading(setHeadingValue.getVal());
00498
00499
00500 else if(param == &setCurrentThruster_Up_Left && valueChanged)
00501 setThruster(THRUSTER_UP_LEFT, setCurrentThruster_Up_Left.getVal());
00502 else if(param == &setCurrentThruster_Up_Right && valueChanged)
00503 setThruster(THRUSTER_UP_RIGHT, setCurrentThruster_Up_Right.getVal());
00504 else if(param == &setCurrentThruster_Up_Back && valueChanged)
00505 setThruster(THRUSTER_UP_BACK, setCurrentThruster_Up_Back.getVal());
00506 else if(param == &setCurrentThruster_Fwd_Left && valueChanged)
00507 setThruster(THRUSTER_FWD_LEFT, setCurrentThruster_Fwd_Left.getVal());
00508 else if(param == &setCurrentThruster_Fwd_Right && valueChanged)
00509 setThruster(THRUSTER_FWD_RIGHT, setCurrentThruster_Fwd_Right.getVal());
00510
00511 else if(param == &speedScale && valueChanged)
00512 itsSpeedScale = speedScale.getVal();
00513 else if(param == &depthRatio && valueChanged)
00514 itsDepthRatio = depthRatio.getVal();
00515
00516
00517 }
00518
00519
00520
00521 void SubController::genPIDImage()
00522 {
00523 static int x = 0;
00524
00525 int depthErr, pitchErr, headingErr, rollErr;
00526
00527 depthErr = (itsDesiredDepth - itsCurrentDepth);
00528 pitchErr = (itsDesiredPitch - itsCurrentPitch);
00529 headingErr = (itsDesiredHeading - itsCurrentHeading);
00530 rollErr = (itsDesiredRoll - itsCurrentRoll);
00531
00532 while (headingErr <= -180) headingErr += 360;
00533 while (headingErr > 180) headingErr -= 360;
00534
00535
00536
00537 int depth_y = (256/2) + (depthErr*2);
00538 if (depth_y > 255) depth_y = 255;
00539 if (depth_y < 0) depth_y = 0;
00540
00541 int pitch_y = (256/2) + (pitchErr*2);
00542 if (pitch_y > 255) pitch_y = 255;
00543 if (pitch_y < 0) pitch_y = 0;
00544
00545 int heading_y = (256/2) + (headingErr*2);
00546 if (heading_y > 255) heading_y = 255;
00547 if (heading_y < 0) heading_y = 0;
00548
00549 int roll_y = (256/2) + (rollErr*2);
00550 if (roll_y > 255) roll_y = 255;
00551 if (roll_y < 0) roll_y = 0;
00552
00553
00554 if (!x)
00555 {
00556 itsPIDImage.clear();
00557 drawLine(itsPIDImage, Point2D<int>(0, 256/2), Point2D<int>(256, 256/2), PixRGB<byte>(255,0,0));
00558 }
00559 if(depthPIDDisplay.getVal()) itsPIDImage.setVal(x,depth_y,PixRGB<byte>(0,255,0));
00560 if(pitchPIDDisplay.getVal()) itsPIDImage.setVal(x,pitch_y,PixRGB<byte>(255,0,0));
00561 if(headingPIDDisplay.getVal()) itsPIDImage.setVal(x,heading_y,PixRGB<byte>(0,0,255));
00562 if(rollPIDDisplay.getVal()) itsPIDImage.setVal(x,roll_y,PixRGB<byte>(255,255,0));
00563
00564 x = (x+1)%256;
00565
00566 }
00567
00568 void SubController::genSubImage()
00569 {
00570 itsSubImage.clear();
00571 drawCircle(itsSubImage, Point2D<int>(128,128), 100, PixRGB<byte>(255,0,0));
00572 int x = (int)(100.0*cos((itsCurrentHeading-90)*(M_PI/180)));
00573 int y = (int)(100.0*sin((itsCurrentHeading-90)*(M_PI/180)));
00574 drawArrow(itsSubImage, Point2D<int>(128,128), Point2D<int>(128+x,128+y), PixRGB<byte>(0,255,0));
00575
00576 int dx = (int)(100.0*cos((itsDesiredHeading-90)*(M_PI/180)));
00577 int dy = (int)(100.0*sin((itsDesiredHeading-90)*(M_PI/180)));
00578 drawArrow(itsSubImage, Point2D<int>(128,128), Point2D<int>(128+dx,128+dy), PixRGB<byte>(0,0,255));
00579
00580 }
00581
00582 const Image<PixRGB<byte> > SubController::getImage(int camera)
00583 {
00584
00585 if (itsSimulation.getVal())
00586 {
00587 return itsBeeStemSim->getImage(camera);
00588 }
00589 else
00590 {
00591 GenericFrame inputFrame;
00592 Image<PixRGB<byte> > img;
00593
00594 switch(camera)
00595 {
00596 case 1:
00597 inputFrame = itsBottomCam->readFrame();
00598 if(!inputFrame.initialized()) return Image<PixRGB<byte> >();
00599
00600 img = inputFrame.asRgb();
00601 break;
00602
00603 case 2:
00604 inputFrame = itsForwardCam->readFrame();
00605 if(!inputFrame.initialized()) return Image<PixRGB<byte> >();
00606
00607 img = inputFrame.asRgb();
00608 break;
00609
00610 default:
00611 return Image<PixRGB<byte> >();
00612 }
00613
00614 img = rescale(img, 320, 240);
00615
00616 return img;
00617 }
00618
00619 return Image<PixRGB<byte> >();
00620
00621 }
00622
00623 const Dims SubController::peekDims()
00624 {
00625 return Dims(320,240);
00626 }
00627
00628 bool SubController::isSimMode()
00629 {
00630 return itsSimulation.getVal();
00631 }
00632
00633 int SubController::getHeadingErr()
00634 {
00635 return abs(itsDesiredHeading - itsCurrentHeading);
00636 }
00637
00638 int SubController::getDepthErr()
00639 {
00640 return abs(itsDesiredDepth - itsCurrentDepth);
00641 }
00642
00643
00644
00645
00646
00647
1.6.3