BeoSubBallast.C

Go to the documentation of this file.
00001 /*!@file BeoSub/BeoSubBallast.C A Ballast for the beosub */
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/BeoSubBallast.C $
00035 // $Id: BeoSubBallast.C 5132 2005-07-30 07:51:13Z itti $
00036 //
00037 
00038 #include "BeoSub/BeoSubBallast.H"
00039 #include "Devices/BeoChip.H"
00040 
00041 // ######################################################################
00042 BeoSubBallast::BeoSubBallast(OptionManager& mgr ,
00043                              const std::string& descrName,
00044                              const std::string& tagName) :
00045   ModelComponent(mgr, descrName, tagName),
00046   itsPulsesPerFill(tagName+"PulsesPerFill", this, 435),
00047   itsOutRed(tagName+"OutRed", this, 0),
00048   itsOutWhite(tagName+"OutWhite", this, 1),
00049   itsInYellow(tagName+"InYellow", this, 0),
00050   itsInWhite(tagName+"InWhite", this, 1),
00051   itsBeoChip(), itsDesiredPulses(0), itsCurrentPulses(10),
00052   itsDirection(Idle), itsPreviousDirection(Idle),
00053   itsIsFull(false), itsIsEmpty(false), itsInitialized(false),
00054   itsPreviousInputs(0), itsGotEndstop(false)
00055 {
00056   pthread_mutex_init(&itsLock, NULL);
00057 }
00058 
00059 // ######################################################################
00060 void BeoSubBallast::setBeoChip(nub::soft_ref<BeoChip>& bc)
00061 { itsBeoChip = bc; }
00062 
00063 // ######################################################################
00064 BeoSubBallast::~BeoSubBallast()
00065 {
00066   pthread_mutex_destroy(&itsLock);
00067 }
00068 
00069 // ######################################################################
00070 void  BeoSubBallast::set(const float val, const bool blocking)
00071 {
00072   if (itsBeoChip.get() == NULL) LFATAL("I need a BeoChip!");
00073   bool waitdone = true;
00074 
00075   pthread_mutex_lock(&itsLock);
00076 
00077   // keep track of desired value:
00078   float v = val;
00079   if (v < 0.0F) { LERROR("CAUTION: Value %f clamped to 0.0", v); v = 0.0F; }
00080   if (v > 1.0F) { LERROR("CAUTION: Value %f clamped to 1.0", v); v = 1.0F; }
00081   itsDesiredPulses = int(val * itsPulsesPerFill.getVal() + 0.5F);
00082 
00083   // actuate the motor:
00084   if (itsDesiredPulses > itsCurrentPulses) move(Filling);
00085   else if (itsDesiredPulses < itsCurrentPulses) move(Emptying);
00086   else move(Idle);
00087 
00088   pthread_mutex_unlock(&itsLock);
00089 
00090   if (blocking)
00091     {
00092       while(waitdone)
00093         {
00094           // check whether we have reached our target:
00095           bool done = false;
00096           pthread_mutex_lock(&itsLock);
00097           if (itsDirection == Idle) done = true;
00098           pthread_mutex_unlock(&itsLock);
00099 
00100           if (done) break; else usleep(50000);
00101         }
00102     }
00103 }
00104 
00105 // ######################################################################
00106 void BeoSubBallast::mechanicalInitialize()
00107 {
00108   // attempt an initialization. We don't know in which initial
00109   // position we may be. The following could happen:
00110   //
00111   // - we start somewhere between full and empty; then actualing the
00112   //   ballast in any direction should yield pulses and we may bump
00113   //   into an endpoint
00114   //
00115   // - we start at one endpoint; then attempting to move past it will
00116   //   yield no response from the BeoChip at all; moving in the other
00117   //   direction should yield pulses at least after a brief instant.
00118 
00119   CLINFO("filling...");
00120   set(1.0F, false); sleep(4);
00121   CLINFO("emptying...");
00122   set(0.0F, true);
00123 }
00124 
00125 // ######################################################################
00126 float BeoSubBallast::get() const
00127 {
00128   pthread_mutex_lock(const_cast<pthread_mutex_t *>(&itsLock));
00129   const float ret = float(itsCurrentPulses) / itsPulsesPerFill.getVal();
00130   pthread_mutex_unlock(const_cast<pthread_mutex_t *>(&itsLock));
00131   return ret;
00132 }
00133 
00134 // ######################################################################
00135 int BeoSubBallast::getPulses() const
00136 {
00137   pthread_mutex_lock(const_cast<pthread_mutex_t *>(&itsLock));
00138   int ret = itsCurrentPulses;
00139   pthread_mutex_unlock(const_cast<pthread_mutex_t *>(&itsLock));
00140   return ret;
00141 }
00142 
00143 // ######################################################################
00144 void BeoSubBallast::input(const int val)
00145 {
00146   // use alternate input() function during init:
00147   if (itsInitialized == false) { inputDuringInit(val); return; }
00148 
00149   pthread_mutex_lock(&itsLock);
00150 
00151   // otherwise let's decode the input:
00152   int changed = itsPreviousInputs ^ val;
00153   bool opto = ((changed & (1 << itsInYellow.getVal())) != 0);
00154   bool endstop = ((val & (1 << itsInWhite.getVal())) != 0);
00155   itsPreviousInputs = val;
00156 
00157   //CLINFO("Input received: opto=%d, endstop=%d, dir=%d, pdir=%d",
00158   //       int(opto), int(endstop), int(itsDirection),
00159   //       int(itsPreviousDirection));
00160 
00161   // did we just receive a pulse from the opto encoder? if so,
00162   // increment/decrement our pulse counter according to our current
00163   // direction of motion. If we are not moving, probably we just
00164   // decided to stop the motors but they are still spinning in
00165   // whatever previous motion direction:
00166   if (opto)
00167     {
00168       switch (itsDirection)
00169         {
00170         case Filling:
00171           ++ itsCurrentPulses;
00172           if (itsCurrentPulses >= itsDesiredPulses) move(Idle);
00173           break;
00174 
00175         case Emptying:
00176           -- itsCurrentPulses;
00177           if (itsCurrentPulses <= itsDesiredPulses) move(Idle);
00178           break;
00179 
00180         case Idle:
00181           {
00182             switch (itsPreviousDirection)
00183               {
00184               case Filling:
00185                 if (itsCurrentPulses < itsPulsesPerFill.getVal())
00186                   ++ itsCurrentPulses;
00187                 break;
00188 
00189               case Emptying:
00190                 if (itsCurrentPulses > 0)
00191                   -- itsCurrentPulses;
00192                 break;
00193 
00194               case Idle:
00195                 CLERROR("Received pulse while Idle! -- DISCARDED");
00196               }
00197           }
00198         }
00199     }
00200 
00201   // clear our endstop history if appropriate:
00202   if (endstop == false) itsGotEndstop = false;
00203 
00204   // did we just hit an endpoint? If so, we can reset our pulse counter:
00205   // NOTE: endstop signals only work when we apply current to the motor...
00206   if (itsDirection != Idle)
00207     {
00208       if (endstop == false)
00209         {
00210           itsIsFull = false;
00211           itsIsEmpty = false;
00212         }
00213       else
00214         {
00215           // we just hit an endpoint; first, were we in travel (this
00216           // is to implement hysteresis at the endpoints - we wait
00217           // until we get out of an endpoint before we consider
00218           // hitting it again)?
00219           if (itsIsFull == false && itsIsEmpty == false)
00220             {
00221               // if we already got a first endstop pulse, this is now
00222               // the second one, and we will accept it no matter
00223               // what. To know which endstop we hit, we just pick the
00224               // one we are closest to. Note that we used to take our
00225               // current motion direction into account (Filling or
00226               // Emptying) but that may be bogus if we are flipping
00227               // directions quickly (the inertia of the motor may
00228               // trick us). Now, could this be a spurious endstop
00229               // pulse (if it's the first one)? If so, we will only
00230               // get one, while we usually get two when we truly hit
00231               // the endpoint (but this is not guaranteed, so we will
00232               // still accept a single pulse if we are reasonably
00233               // close to an endpoint according to our pulse counter):
00234               if (itsGotEndstop ||
00235                   itsCurrentPulses < itsPulsesPerFill.getVal() / 10 ||
00236                   itsCurrentPulses > (itsPulsesPerFill.getVal()*9) / 10)
00237                 {
00238                   // which side of the middle point are we?
00239                   if (itsCurrentPulses > itsPulsesPerFill.getVal() / 2)
00240                     {
00241                       // ok so we must now be full:
00242                       itsCurrentPulses = itsPulsesPerFill.getVal();
00243                       move(Idle); itsIsFull = true;
00244                       CLINFO("Endstop reached -> full");
00245                     }
00246                   else
00247                     {
00248                       // ok so we must now be empty:
00249                       itsCurrentPulses = 0;
00250                       move(Idle); itsIsEmpty = true;
00251                       CLINFO("Endstop reached -> empty");
00252                     }
00253                 }
00254               else
00255                 itsGotEndstop = true; // fishy; wait for second one
00256             }
00257         }
00258     }
00259 
00260   //CLINFO("desired=%d current=%d", itsDesiredPulses, itsCurrentPulses);
00261 
00262   pthread_mutex_unlock(&itsLock);
00263 }
00264 
00265 // ######################################################################
00266 void BeoSubBallast::inputDuringInit(const int val)
00267 {
00268   pthread_mutex_lock(&itsLock);
00269 
00270   bool endstop = ((val & (1 << itsInWhite.getVal())) != 0);
00271   itsPreviousInputs = val;
00272 
00273   //CLINFO("Input received during init: endstop=%d", int(endstop));
00274 
00275   // boot sequence should be to fill for a while, then empty until we
00276   // hit the endpoint. Once we are completely empty, we will turn
00277   // itsInitialized to true.
00278 
00279   // did we just hit an endpoint? If so, we can reset our pulse counter:
00280   // NOTE: endstop signals only work when we apply current to the motor...
00281   if (itsDirection != Idle)
00282     {
00283       if (endstop == false)
00284         {
00285           itsIsFull = false;
00286           itsIsEmpty = false;
00287         }
00288       else
00289         {
00290           // we just hit an endpoint:
00291           switch (itsDirection)
00292             {
00293             case Filling:
00294               // were we in travel?
00295               if (itsIsFull == false && itsIsEmpty == false)
00296                 {
00297                   // ok so we must now be full:
00298                   itsCurrentPulses = itsPulsesPerFill.getVal();
00299                   move(Idle); itsIsFull = true;
00300                 }
00301               break;
00302 
00303             case Emptying:
00304               // were we in travel?
00305               if (itsIsFull == false && itsIsEmpty == false)
00306                 {
00307                   // ok so we must now be empty:
00308                   itsCurrentPulses = 0;
00309                   move(Idle); itsIsEmpty = true;
00310 
00311 
00312                   // ok, we are now initialized:
00313                   CLINFO("Mechanical initialization complete.");
00314                   itsInitialized = true;
00315                 }
00316               break;
00317 
00318             default:
00319               break;
00320             }
00321         }
00322     }
00323 
00324   //CLINFO("desired=%d current=%d", itsDesiredPulses, itsCurrentPulses);
00325   pthread_mutex_unlock(&itsLock);
00326 }
00327 
00328 // ######################################################################
00329 bool BeoSubBallast::moving() const
00330 {
00331   pthread_mutex_lock(const_cast<pthread_mutex_t *>(&itsLock));
00332   bool ret = (itsDirection != Idle);
00333   pthread_mutex_unlock(const_cast<pthread_mutex_t *>(&itsLock));
00334   return ret;
00335 }
00336 
00337 // ######################################################################
00338 void BeoSubBallast::move(const MotorDirection dir)
00339 {
00340   // CAUTION: Should only be called under protection of our itsLock!
00341   switch(dir)
00342     {
00343     case Filling:
00344       if (itsDirection != Filling) itsPreviousDirection = itsDirection;
00345       itsDirection = Filling;
00346       itsBeoChip->setDigitalOut(itsOutRed.getVal(), true);
00347       itsBeoChip->setDigitalOut(itsOutWhite.getVal(), false);
00348       break;
00349     case Emptying:
00350       if (itsDirection != Emptying) itsPreviousDirection = itsDirection;
00351       itsDirection = Emptying;
00352       itsBeoChip->setDigitalOut(itsOutRed.getVal(), false);
00353       itsBeoChip->setDigitalOut(itsOutWhite.getVal(), true);
00354       break;
00355     case Idle:
00356       if (itsDirection != Idle) itsPreviousDirection = itsDirection;
00357       itsDirection = Idle;
00358       itsBeoChip->setDigitalOut(itsOutRed.getVal(), false);
00359       itsBeoChip->setDigitalOut(itsOutWhite.getVal(), false);
00360       break;
00361     default:
00362       CLERROR("Unknown move command ignored");
00363     }
00364 }
00365 
00366 // ######################################################################
00367 /* So things look consistent in everyone's emacs... */
00368 /* Local Variables: */
00369 /* indent-tabs-mode: nil */
00370 /* End: */
Generated on Sun May 8 08:40:19 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3