SubController.C

Go to the documentation of this file.
00001 /*!@file SeaBee/SubController.C  Control motors and pid */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/SubController.C $
00035 // $Id: SubController.C 10794 2009-02-08 06:21:09Z itti $
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   //setup pid loop thread
00198   itsThreadServer.reset(new WorkThreadServer("SubController",1)); //start a single worker thread
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   //killMotors();
00218 }
00219 
00220 void SubController::sendHeartBeat()
00221 {
00222   //  itsBeeStem->setHeartBeat();
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   //<TODO mmontalbo> check for heading greater than 360
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       //float rollCorrection = itsRollPID.update(itsDesiredRoll, (float)itsCurrentRoll);
00353       float depthCorrection = itsDepthPID.update((float)itsDesiredDepth, (float)itsCurrentDepth);
00354 
00355       if (depthCorrection > 75) depthCorrection = 75;
00356       if (depthCorrection < -75) depthCorrection = -75;
00357 
00358       // LINFO("Heading | Desired: %d, Current %d,  Correction %f", normDes, normHead, headingCorrection);
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) //hack to set the depth manually
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 //           itsCurrentThruster_Up_Back = 0;
00394 //           itsCurrentThruster_Up_Left = 0;
00395 //           itsCurrentThruster_Up_Right = 0;
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   //LINFO("Set Thruster %i %i", thruster, val);
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   //itsBeeStem->setThrusters(0, 0, 0, 0, 0);
00453 }
00454 
00455 
00456 void SubController::paramChanged(ModelParamBase* const param, const bool valueChanged, ParamClient::ChangeStatus* status)
00457 {
00458 
00459   //////// Pitch PID constants/gain change ////////
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   //////// Roll PID constants/gain change ///////
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   //////// Heading PID constants/gain change ////
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   /////// Depth PID constants/gain change ////
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   //Thruster_Settings
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 ////////////////// GUI Related ////////////////////////////////////
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))); //shift by 90 so that 0 is up
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))); //shift by 90 so that 0 is up
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 /* So things look consistent in everyone's emacs... */
00645 /* Local Variables: */
00646 /* indent-tabs-mode: nil */
00647 /* End: */
Generated on Sun May 8 08:40:20 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3