BeoSubTwoBal.C

Go to the documentation of this file.
00001 /*!@file BeoSub/BeoSubTwoBal.C An autonomous submarine with two ballasts*/
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: Laurent Itti <itti@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/BeoSub/BeoSubTwoBal.C $
00035 // $Id: BeoSubTwoBal.C 7880 2007-02-09 02:34:07Z itti $
00036 //
00037 
00038 #include "BeoSub/BeoSubTwoBal.H"
00039 #include "Devices/HMR3300.H"
00040 #include "BeoSub/BeoSubBallast.H"
00041 #include "BeoSub/BeoSubIMU.H"
00042 #include "Util/MathFunctions.H"
00043 #include "Devices/IEEE1394grabber.H"
00044 #include "rutz/compat_snprintf.h"
00045 
00046 // ######################################################################
00047 //! Class definition for BeoSubListener
00048 /*! This is the listener class that is attached to each BeoChip in the
00049   left and right ballast tubes of the submarins. This class is just a
00050   pass-through to the function dispatchBeoChipEvent() of class
00051   BeoSub. */
00052 class BeoSubListener : public BeoChipListener
00053 {
00054 public:
00055   //! Constructor
00056   BeoSubListener(const BeoSubSide side, BeoSub *sub) :
00057     itsSide(side), itsBeoSub(sub)
00058   { }
00059 
00060   //! destructor
00061   virtual ~BeoSubListener()
00062   { }
00063 
00064   //! get an event
00065   virtual void event(const BeoChipEventType t, const int valint,
00066                      const float valfloat)
00067   {
00068     LDEBUG("Event: %d val = %d, fval = %f", int(t), valint, valfloat);
00069     itsBeoSub->dispatchBeoChipEvent(itsSide, t, valint, valfloat);
00070   }
00071 
00072 private:
00073   const BeoSubSide itsSide; //!< which side of the sub are we on?
00074   BeoSubTwoBal *itsBeoSub;        //!< pointer to our master
00075 };
00076 
00077 
00078 
00079 // ######################################################################
00080 BeoSubTwoBal::BeoSubTwoBal(OptionManager& mgr) :
00081   BeoSub(mgr),
00082   ModelComponent(mgr, descrName, tagName),
00083   itsLeftThrusterServoNum("BeoSubLeftThrusterServoNum", this, 2),
00084   itsRightThrusterServoNum("BeoSubRightThrusterServoNum", this, 2),
00085   itsHMR3300(new HMR3300(mgr)),
00086   itsBeoLeft(new BeoChip(mgr, "BeoChipLeft", "BeoChipLeft")),
00087   itsBeoRight(new BeoChip(mgr, "BeoChipRight", "BeoChipRight")),
00088   itsBeoLisLeft(new BeoSubListener(BEOSUBLEFT, this)),
00089   itsBeoLisRight(new BeoSubListener(BEOSUBRIGHT, this)),
00090   itsDepthSensor(),
00091   itsLFballast(new BeoSubBallast(mgr, "LF Ballast", "LFballast")),
00092   itsLRballast(new BeoSubBallast(mgr, "LR Ballast", "LRballast")),
00093   itsRFballast(new BeoSubBallast(mgr, "RF Ballast", "RFballast")),
00094   itsRRballast(new BeoSubBallast(mgr, "RR Ballast", "RRballast")),
00095   itsIMU(new BeoSubIMU(mgr)),
00096   itsCameraFront(new IEEE1394grabber(mgr, "Front Camera", "FrontCamera")),
00097   itsCameraDown(new IEEE1394grabber(mgr, "Down Camera", "DownCamera")),
00098   itsCameraUp(new IEEE1394grabber(mgr, "Up Camera", "UpCamera"))
00099 {
00100   // register our babies as subcomponents:
00101   addSubComponent(itsHMR3300);
00102   addSubComponent(itsIMU);
00103   addSubComponent(itsBeoLeft);
00104   addSubComponent(itsBeoRight);
00105   addSubComponent(itsCameraDown);
00106   addSubComponent(itsCameraFront);
00107   addSubComponent(itsCameraUp);
00108   addSubComponent(itsLFballast);
00109   addSubComponent(itsLRballast);
00110   addSubComponent(itsRFballast);
00111   addSubComponent(itsRRballast);
00112 
00113   // connect our listeners to our beochips:
00114   rutz::shared_ptr<BeoChipListener> bl = itsBeoLisLeft, br = itsBeoLisRight;
00115   itsBeoLeft->setListener(bl);
00116   itsBeoRight->setListener(br);
00117 
00118   // hook up BeoChips to Ballasts:
00119   itsLFballast->setBeoChip(itsBeoLeft);
00120   itsLRballast->setBeoChip(itsBeoLeft);
00121   itsRFballast->setBeoChip(itsBeoRight);
00122   itsRRballast->setBeoChip(itsBeoRight);
00123 
00124   // disable RTS/CTS flow control on our BeoChips:
00125   itsBeoLeft->setModelParamVal("BeoChipUseRTSCTS", false);
00126   itsBeoRight->setModelParamVal("BeoChipUseRTSCTS", false);
00127 
00128   // select default serial ports for everybody:
00129   itsHMR3300->setModelParamString("HMR3300SerialPortDevName",
00130                                   "/dev/ttyS0", MC_RECURSE);
00131   itsBeoLeft->setModelParamString("BeoChipDeviceName", "/dev/ttyS2");
00132   itsBeoRight->setModelParamString("BeoChipDeviceName", "/dev/ttyS1");
00133   itsIMU->setModelParamString("IMUSerialPortDevName", "/dev/ttyS3",
00134                               MC_RECURSE);
00135 
00136   // set the I/O for our ballasts:
00137   itsLRballast->setModelParamVal("LRballastOutRed", 1);
00138   itsLRballast->setModelParamVal("LRballastOutWhite", 0);
00139   itsLRballast->setModelParamVal("LRballastInYellow", 1);
00140   itsLRballast->setModelParamVal("LRballastInWhite", 0);
00141   itsLFballast->setModelParamVal("LFballastOutRed", 3);
00142   itsLFballast->setModelParamVal("LFballastOutWhite", 2);
00143   itsLFballast->setModelParamVal("LFballastInYellow", 3);
00144   itsLFballast->setModelParamVal("LFballastInWhite", 2);
00145   itsRRballast->setModelParamVal("RRballastOutRed", 1);
00146   itsRRballast->setModelParamVal("RRballastOutWhite", 0);
00147   itsRRballast->setModelParamVal("RRballastInYellow", 1);
00148   itsRRballast->setModelParamVal("RRballastInWhite", 0);
00149   itsRFballast->setModelParamVal("RFballastOutRed", 3);
00150   itsRFballast->setModelParamVal("RFballastOutWhite", 2);
00151   itsRFballast->setModelParamVal("RFballastInYellow", 3);
00152   itsRFballast->setModelParamVal("RFballastInWhite", 2);
00153 }
00154 
00155 // ######################################################################
00156 BeoSubTwoBal::~BeoSubTwoBal()
00157 {  }
00158 
00159 // ######################################################################
00160 void BeoSubTwoBal::start1()
00161 {
00162   // select firewire subchannels for our cameras:
00163   itsCameraDown->setModelParamVal("FrameGrabberSubChan", int(BEOSUBCAMDOWN));
00164   itsCameraFront->setModelParamVal("FrameGrabberSubChan", int(BEOSUBCAMFRONT));
00165   itsCameraUp->setModelParamVal("FrameGrabberSubChan", int(BEOSUBCAMUP));
00166 }
00167 
00168 // ######################################################################
00169 void BeoSubTwoBal::start2()
00170 {
00171   // turn on the analog port capture for the pressure sensor:
00172   itsBeoLeft -> captureAnalog(0,true);
00173 
00174   // turn on KBD capture for our ballasts:
00175   itsBeoLeft->captureKeyboard(true);
00176   itsBeoRight->captureKeyboard(true);
00177 
00178   // initialize all 4 ballasts at once:
00179   CLINFO("filling ballasts...");
00180   setBallasts(1.0F, 1.0F, 1.0F, 1.0F, false); sleep(4);
00181   CLINFO("emptying ballasts...");
00182   setBallasts(0.0F, 0.0F, 0.0F, 0.0F, true);
00183 }
00184 
00185 // ######################################################################
00186 void BeoSubTwoBal::thrust(const float leftval, const float rightval, const bool blocking)
00187 {
00188   // note: if the value is out of range, the BeoChip will clamp it.
00189   // That's why we read it back from the BeoChip before displaying it:
00190   itsBeoLeft->setServo(itsLeftThrusterServoNum.getVal(), leftval);
00191   itsThrustLeft = itsBeoLeft->getServo(itsLeftThrusterServoNum.getVal());
00192   itsBeoLeft->lcdPrintf(0, 0, "Th=%-1.2f", itsThrustLeft);
00193 
00194   itsBeoRight->setServo(itsRightThrusterServoNum.getVal(), rightval);
00195   itsThrustRight = itsBeoRight->getServo(itsRightThrusterServoNum.getVal());
00196   itsBeoRight->lcdPrintf(0, 0, "Th=%-1.2f", itsThrustRight);
00197 }
00198 
00199 // ######################################################################
00200 void BeoSubTwoBal::getThrusters(float& leftval, float& rightval)
00201 { leftval = itsThrustLeft; rightval = itsThrustRight; }
00202 
00203 // ######################################################################
00204 void BeoSubTwoBal::setLFballast(const float val, const bool blocking)
00205 { itsLFballast->set(val, blocking); }
00206 
00207 // ######################################################################
00208 void BeoSubTwoBal::setLRballast(const float val, const bool blocking)
00209 { itsLRballast->set(val, blocking); }
00210 
00211 // ######################################################################
00212 void BeoSubTwoBal::setRFballast(const float val, const bool blocking)
00213 { itsRFballast->set(val, blocking); }
00214 
00215 // ######################################################################
00216 void BeoSubTwoBal::setRRballast(const float val, const bool blocking)
00217 { itsRRballast->set(val, blocking); }
00218 
00219 // ######################################################################
00220 void BeoSubTwoBal::setBallasts(const float lf, const float lr,
00221                          const float rf, const float rr, const bool blocking)
00222 {
00223   // move all four ballasts simultaneously rather than sequentially:
00224   itsLFballast->set(lf, false);
00225   itsLRballast->set(lr, false);
00226   itsRFballast->set(rf, false);
00227   itsRRballast->set(rr, false);
00228 
00229   if (blocking)
00230     {
00231       while(true)
00232         {
00233           if (itsLFballast->moving() == false &&
00234               itsLRballast->moving() == false &&
00235               itsRFballast->moving() == false &&
00236               itsRRballast->moving() == false) break;
00237           usleep(100000);
00238         }
00239     }
00240 }
00241 
00242 // ######################################################################
00243 float BeoSub::getLFballast()
00244 { return itsLFballast->get(); }
00245 
00246 // ######################################################################
00247 float BeoSub::getLRballast()
00248 { return itsLRballast->get(); }
00249 
00250 // ######################################################################
00251 float BeoSub::getRFballast()
00252 { return itsRFballast->get(); }
00253 
00254 // ######################################################################
00255 float BeoSub::getRRballast()
00256 { return itsRRballast->get(); }
00257 
00258 // ######################################################################
00259 void BeoSub::getBallasts(float& lf, float& lr, float& rf, float& rr)
00260 {
00261   lf = itsLFballast->get();
00262   lr = itsLRballast->get();
00263   rf = itsRFballast->get();
00264   rr = itsRRballast->get();
00265 }
00266 
00267 // ######################################################################
00268 void BeoSub::dropMarker(const bool blocking)
00269 {
00270   LFATAL("unimplemented!");
00271 }
00272 
00273 /*
00274 // ######################################################################
00275 void BeoSub::checkpoint(const char *fmt, ...)
00276 {
00277   va_list a; va_start(a, fmt); char buf[2048];
00278   vsnprintf(buf, 2047, fmt, a); va_end(a);
00279   LDEBUG("===== START CHECKPOINT %d =====", itsCkPt);
00280   LDEBUG(buf);
00281   LDEBUG("depth = %f", getDepth());
00282   LDEBUG("compass = [ %f, %f, %f ]", itsHMR3300->getHeading().getVal(),
00283          itsHMR3300->getPitch().getVal(), itsHMR3300->getRoll().getVal());
00284   LDEBUG("===== END CHECKPOINT %d =====", itsCkPt);
00285   itsCkPt ++;
00286 }
00287 */
00288 
00289 // ######################################################################
00290 void BeoSub::dispatchBeoChipEvent(const BeoSubSide side,
00291                                   const BeoChipEventType t,
00292                                   const int valint,
00293                                   const float valfloat)
00294 {
00295   // what is this event about and whom is it for?
00296   switch(t)
00297     {
00298     case NONE: // ##############################
00299       break;
00300 
00301     case PWM0: // ##############################
00302       /* nobody cares */
00303       break;
00304 
00305     case PWM1: // ##############################
00306       /* nobody cares */
00307       break;
00308 
00309     case KBD: // ##############################
00310       LDEBUG("%s Keyboard: new value %d", beoSubSideName(side), valint);
00311       // print keyboard values:
00312       char kb[6]; kb[5] = '\0';
00313       for (int i = 0; i < 5; i ++) kb[i] = (valint>>(4-i))&1 ? '1':'0';
00314 
00315       switch(side)
00316         {
00317         case BEOSUBLEFT:
00318           // current diagram is:
00319           // #0 = front full/empty switch
00320           // #1 = front gear encoder
00321           // #2 = rear full/empty switch
00322           // #3 = rear gear encoder
00323           // #4 = kill switch
00324 
00325           // let both left ballasts know:
00326           itsLFballast->input(valint);
00327           itsLRballast->input(valint);
00328 
00329           itsBeoLeft->lcdPrintf(15, 3, kb);
00330           break;
00331         case BEOSUBRIGHT:
00332           // current diagram is:
00333           // #0 = front full/empty switch
00334           // #1 = front gear encoder
00335           // #2 = rear full/empty switch
00336           // #3 = rear gear encoder
00337           // #4 = some other switch
00338 
00339           // let both right ballasts know:
00340           itsRFballast->input(valint);
00341           itsRRballast->input(valint);
00342 
00343           itsBeoRight->lcdPrintf(15, 3, kb);
00344           break;
00345         }
00346       break;
00347 
00348     case ADC0: // ##############################
00349       LDEBUG("%s ADC0 new value = %d", beoSubSideName(side), valint);
00350       // the left ADC0 is connected to the pressure sensor:
00351       if (side == BEOSUBLEFT)
00352         {
00353           itsDepthSensor.newMeasurement(valfloat);
00354           itsBeoLeft->lcdPrintf(8, 0, "Pr=%03d", valint);
00355         }
00356       else
00357         LERROR("Getting values from right ADC0 -- turn it off!");
00358       break;
00359 
00360     case ADC1: // ##############################
00361       /* anybody? */
00362       break;
00363 
00364     case RESET: // ##############################
00365       LERROR("%s BeoChip RESET!", beoSubSideName(side));
00366       break;
00367 
00368     case ECHOREP: // ##############################
00369       LINFO("%s BeoChip Echo reply.", beoSubSideName(side));
00370       break;
00371 
00372     case INOVERFLOW: // ##############################
00373       LERROR("%s BeoChip input overflow!", beoSubSideName(side));
00374       break;
00375 
00376     case SERIALERROR: // ##############################
00377       LERROR("%s BeoChip serial error!", beoSubSideName(side));
00378       break;
00379 
00380     case OUTOVERFLOW: // ##############################
00381       LERROR("%s BeoChip output overflow!", beoSubSideName(side));
00382       break;
00383 
00384     default: // ##############################
00385       LERROR("%s BeoChip unknown event %d!", beoSubSideName(side), int(t));
00386       break;
00387     }
00388 }
00389 
00390 // ######################################################################
00391 Image< PixRGB<byte> > BeoSub::grabImage(const enum BeoSubCamera cam)
00392 {
00393   if (cam == BEOSUBCAMFRONT) return itsCameraFront->readRGB();
00394   if (cam == BEOSUBCAMDOWN) return itsCameraDown->readRGB();
00395   if (cam == BEOSUBCAMUP) return itsCameraUp->readRGB();
00396   LERROR("Wrong camera %d -- RETURNING EMPTY IMAGE", int(cam));
00397   return Image< PixRGB<byte> >();
00398 }
00399 
00400 // ######################################################################
00401 const char* beoSubSideName(const BeoSubSide s)
00402 {
00403   if (s == BEOSUBLEFT) return "Left";
00404   if (s == BEOSUBRIGHT) return "Right";
00405   LERROR("Unknown BeoSubSide value %d", int(s));
00406   return "Unknown";
00407 }
00408 
00409 // ######################################################################
00410 const char* beoSubCameraName(const BeoSubCamera cam)
00411 {
00412   if (cam == BEOSUBCAMFRONT) return "Front";
00413   if (cam == BEOSUBCAMDOWN) return "Down";
00414   if (cam == BEOSUBCAMUP) return "Up";
00415   LERROR("Unknown BeoSubCamera value %d", int(cam));
00416   return "Unknown";
00417 }
00418 
00419 
00420 
00421 // ######################################################################
00422 /* So things look consistent in everyone's emacs... */
00423 /* Local Variables: */
00424 /* indent-tabs-mode: nil */
00425 /* End: */
Generated on Sun May 8 08:40:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3