SubController.H

Go to the documentation of this file.
00001 /*!@file SeaBee/SubController.H  Control motors and pid */
00002 
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2000-2003   //
00005 // by the 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
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/SeaBee/SubController.H $
00035 // $Id: SubController.H 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #ifndef SubController_H_DEFINED
00039 #define SubController_H_DEFINED
00040 
00041 #include "Component/ModelComponent.H"
00042 #include "Component/ModelOptionDef.H"
00043 #include "Component/ModelParam.H"
00044 #include "Component/OptionManager.H"
00045 #include "Devices/V4L2grabber.H"
00046 #include "rutz/shared_ptr.h"
00047 #include "Util/Types.H"
00048 #include "Util/WorkThreadServer.H"
00049 #include "Util/JobWithSemaphore.H"
00050 #include "Controllers/PID.H"
00051 #include "Devices/BeeStemTiny.H"
00052 #include "Image/Image.H"
00053 #include "Image/Pixels.H"
00054 #include "Image/ShapeOps.H"
00055 #include "Util/Angle.H"
00056 #include "Transport/FrameIstream.H"
00057 #include "Transport/FrameInfo.H"
00058 #include "Raster/GenericFrame.H"
00059 #include "Media/FrameSeries.H"
00060 #include "SeaBee/BeeStemSim.H"
00061 #include <list>
00062 
00063 
00064 //NOTE: Do not change these thruster mappings! Just plug in the cables correctly, please.
00065 //      If for some reason these mappings must be changed, then you need to also change the order
00066 //      in which SubController::setThruster and SubController::updatePID sendd their thruster packets
00067 #define THRUSTER_UP_LEFT 0
00068 #define THRUSTER_UP_RIGHT 4
00069 #define THRUSTER_UP_BACK 2
00070 #define THRUSTER_FWD_RIGHT 3
00071 #define THRUSTER_FWD_LEFT 1
00072 
00073 #define THRUSTER_UP_LEFT_THRESH 0
00074 #define THRUSTER_UP_RIGHT_THRESH 0
00075 #define THRUSTER_UP_BACK_THRESH 0
00076 #define THRUSTER_FWD_RIGHT_THRESH 0
00077 #define THRUSTER_FWD_LEFT_THRESH 0
00078 
00079 // ######################################################################
00080 class SubController : public ModelComponent
00081 {
00082 public:
00083 
00084   enum SensorType  {DEPTH, PITCH, ROLL, HEADING};
00085   // ######################################################################
00086   /*! @name Constructors and Destructors */
00087   //@{
00088 
00089   //! Constructor
00090   SubController(OptionManager& mgr,
00091              const std::string& descrName = "SubController",
00092              const std::string& tagName = "SubController",
00093              const bool simulation = false);
00094 
00095   //! Destructor
00096   ~SubController();
00097 
00098   void runSimLoop();
00099   bool setLevel(); //maintain sub at current pitch/roll level
00100 
00101   bool setHeading(int heading); //maintain a heading
00102   bool setDepth(int depth);  //maintain a depth
00103   bool setSpeed(int speed);  //maintain a speed
00104   bool setTurningSpeed(int speed);  //maintain a turning speed
00105 
00106   // althought we will most likely not use these. It's good to have them anyway
00107   // maybe it will be part of our victory dance
00108   bool setRoll(int roll); //maintain roll
00109   bool setPitch(int pitch); //maintain pitch
00110 
00111   void start1();
00112   void start2();
00113 
00114   int getHeading() { return itsCurrentHeading; }
00115   int getHeadingErr();
00116   int getDepthErr();
00117   int getPitch() { return itsCurrentPitch; }
00118   int getRoll() { return itsCurrentRoll; }
00119   int getDepth() { return itsCurrentDepth; }
00120   int getIntPressure() { return (itsCurrentIntPressure); }
00121 
00122   bool getKillSwitch() { return false; }
00123 
00124   void setThruster(int thruster, int val);
00125 
00126   const int* getIntPressurePtr() { return &itsCurrentIntPressure; }
00127   const int* getHeadingPtr() { return &itsCurrentHeading; }
00128   const int* getPitchPtr() { return &itsCurrentPitch; }
00129   const int* getRollPtr() { return &itsCurrentRoll; }
00130   const int* getDepthPtr() { return &itsCurrentDepth; }
00131 
00132   const int* getThruster_Up_Left_Ptr() { return &itsCurrentThruster_Up_Left; }
00133   const int* getThruster_Up_Right_Ptr() { return &itsCurrentThruster_Up_Right; }
00134   const int* getThruster_Up_Back_Ptr() { return &itsCurrentThruster_Up_Back; }
00135   const int* getThruster_Fwd_Right_Ptr() { return &itsCurrentThruster_Fwd_Right; }
00136   const int* getThruster_Fwd_Left_Ptr() { return &itsCurrentThruster_Fwd_Left; }
00137 
00138   void setMotorsOn(bool val) { motorsOn.setVal(val);}
00139   void setPidOn(bool val) { pidOn.setVal(val); }
00140 
00141   void updateHeading(unsigned int currentHeading);
00142   void updatePitch(int currentPitch);
00143   void updateRoll(int currentRoll);
00144   void updateDepth(unsigned int currentDepth);
00145   void updatePID();
00146   void sendHeartBeat();
00147 
00148   void setIntPressure(unsigned int pressure);
00149 
00150   void killMotors();
00151 
00152   void genPIDImage();
00153   void genSubImage();
00154 
00155   const Image<PixRGB<byte> >* getPIDImagePtr() { return &itsPIDImage; }
00156   const Image<PixRGB<byte> >* getSubImagePtr() { return &itsSubImage; }
00157 
00158   const Image<PixRGB<byte> > getImage(int camera);
00159   const Dims peekDims();
00160 
00161   bool isSimMode();
00162 
00163 
00164 
00165   //@}
00166 
00167   // ######################################################################
00168   /*! @name member functions */
00169   //@{
00170 
00171 
00172 private:
00173   OModelParam<bool> itsSimulation;
00174   int itsDesiredPitch;
00175   int itsDesiredRoll;
00176   int itsDesiredHeading;
00177   int itsDesiredDepth;
00178   int itsDesiredSpeed;
00179   int itsDesiredTurningSpeed;
00180 
00181 
00182   int itsCurrentPitch;
00183   int itsCurrentRoll;
00184   int itsCurrentHeading;
00185   int itsCurrentDepth;
00186   int itsCurrentSpeed;
00187   int itsCurrentIntPressure;
00188 
00189   NModelParam<float> speedScale;
00190   float itsSpeedScale;
00191 
00192   NModelParam<float> depthRatio;
00193   float itsDepthRatio;
00194 
00195   nub::soft_ref<BeeStemTiny> itsBeeStemTiny;
00196   nub::soft_ref<BeeStemSim> itsBeeStemSim;
00197 
00198   PID<float> itsPitchPID;
00199   PID<float> itsRollPID;
00200 
00201   NModelParam<float> headingP;
00202   NModelParam<float> headingI;
00203   NModelParam<float> headingD;
00204 
00205   PID<float> itsHeadingPID;
00206 
00207   NModelParam<float> depthP;
00208   NModelParam<float> depthI;
00209   NModelParam<float> depthD;
00210 
00211   PID<float> itsDepthPID;
00212 
00213   int itsCurrentThruster_Up_Left;
00214   int itsCurrentThruster_Up_Right;
00215   int itsCurrentThruster_Up_Back;
00216   int itsCurrentThruster_Fwd_Right;
00217   int itsCurrentThruster_Fwd_Left;
00218 
00219 
00220   void paramChanged(ModelParamBase* const param, const bool valueChanged, ParamClient::ChangeStatus* status);
00221 
00222   NModelParam<int> setCurrentThruster_Up_Left;
00223   NModelParam<int> setCurrentThruster_Up_Right;
00224   NModelParam<int> setCurrentThruster_Up_Back;
00225   NModelParam<int> setCurrentThruster_Fwd_Right;
00226   NModelParam<int> setCurrentThruster_Fwd_Left;
00227 
00228   NModelParam<float> pitchP;
00229   NModelParam<float> pitchI;
00230   NModelParam<float> pitchD;
00231 
00232   NModelParam<float> rollP;
00233   NModelParam<float> rollI;
00234   NModelParam<float> rollD;
00235 
00236 
00237   NModelParam<bool>  motorsOn;
00238   NModelParam<bool>  pidOn;
00239   NModelParam<bool>  guiOn;
00240   NModelParam<bool>  depthPIDDisplay;
00241   NModelParam<bool>  pitchPIDDisplay;
00242   NModelParam<bool>  headingPIDDisplay;
00243   NModelParam<bool>  rollPIDDisplay;
00244 
00245 
00246   NModelParam<int>  setDepthValue;
00247   NModelParam<int>  setPitchValue;
00248   NModelParam<int>  setRollValue;
00249   NModelParam<int>  setHeadingValue;
00250 
00251 
00252 
00253   /// GUI Related
00254   Image<PixRGB<byte> > itsPIDImage;
00255   Image<PixRGB<byte> > itsSubImage;
00256 
00257         int itsPrevDepth;
00258         int itsPrevPrevDepth;
00259         int itsDepthCount;
00260         std::list<int> itsAvgDepth;
00261 
00262   rutz::shared_ptr<WorkThreadServer> itsThreadServer;
00263 
00264   nub::soft_ref<V4L2grabber> itsForwardCam;
00265   nub::soft_ref<V4L2grabber> itsBottomCam;
00266 
00267   void initSensorVals();
00268 
00269 };
00270 
00271 #endif
00272 
00273 // ######################################################################
00274 /* So things look consistent in everyone's emacs... */
00275 /* Local Variables: */
00276 /* indent-tabs-mode: nil */
00277 /* End: */
Generated on Sun May 8 08:42:15 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3