remoteBeoChip.C

Go to the documentation of this file.
00001 /*!@file RCBot/remoteBeoChip.C */
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: Christian Siagian <siagian@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/RCBot/remoteBeoChip.C $
00035 // $Id: remoteBeoChip.C 7063 2006-08-29 18:26:55Z rjpeters $
00036 //
00037 
00038 #include "Component/ModelManager.H"
00039 #include "Devices/BeoChip.H"
00040 #include "Devices/DeviceOpts.H"
00041 #include "Util/MathFunctions.H"
00042 #include "RCBot/BotControl.H"
00043 
00044 #include <cstdlib>
00045 
00046 #include "Beowulf/Beowulf.H"
00047 #include "Util/Types.H"
00048 #include "Util/log.H"
00049 
00050 #include <cstring>
00051 #include <signal.h>
00052 #include <time.h>
00053 #include <unistd.h>
00054 
00055 static bool goforever = true;
00056 void terminate(int s) { LERROR("*** INTERRUPT ***"); goforever = false; exit(1);}
00057 
00058 // ######################################################################
00059 //! Our own little BeoChipListener
00060 class MyBeoChipListener : public BeoChipListener
00061 {
00062 public:
00063   MyBeoChipListener(nub::soft_ref<BeoChip> bc) :
00064     itsBeoChip(bc), minp0(9999), maxp0(0), minp1(9999), maxp1(0),
00065     counter0(0), counter1(0), kbd(0x1f) { }
00066 
00067   virtual ~MyBeoChipListener() { }
00068 
00069   virtual void event(const BeoChipEventType t, const int valint,
00070                      const float valfloat)
00071   {
00072     LDEBUG("Event: %d val = %d, fval = %f", int(t), valint, valfloat);
00073     switch(t)
00074       {
00075       case PWM0:
00076         if (valint < minp0) minp0 = valint;
00077         else if (valint > maxp0) maxp0 = valint;
00078         itsBeoChip->setServo(0, valfloat);
00079         if (++counter0 >= 10)
00080           {
00081             itsBeoChip->lcdPrintf(5, 2, "%04d  %04d-%04d",
00082                                   valint, minp0, maxp0);
00083             itsBeoChip->lcdPrintf(6, 1, "%03d",
00084                                   itsBeoChip->getServoRaw(0));
00085             counter0 = 0;
00086           }
00087         break;
00088       case PWM1:
00089         if (valint < minp1) minp1 = valint;
00090         else if (valint > maxp1) maxp1 = valint;
00091         itsBeoChip->setServo(1, valfloat);
00092         if (++counter1 >= 10)
00093           {
00094             itsBeoChip->lcdPrintf(5, 3, "%04d  %04d-%04d",
00095                                   valint, minp1, maxp1);
00096             itsBeoChip->lcdPrintf(17, 1, "%03d",
00097                                   itsBeoChip->getServoRaw(1));
00098             counter1 = 0;
00099           }
00100         break;
00101       case KBD: kbd = valint;  break;
00102       case RESET: LERROR("BeoChip RESET occurred!"); break;
00103       case ECHOREP: LINFO("BeoChip Echo reply received."); break;
00104       case INOVERFLOW: LERROR("BeoChip input overflow!"); break;
00105       case SERIALERROR: LERROR("BeoChip serial error!"); break;
00106       case OUTOVERFLOW: LERROR("BeoChip output overflow!"); break;
00107       default: LERROR("Unknown event %d received!", int(t)); break;
00108       }
00109   }
00110 
00111   nub::soft_ref<BeoChip> itsBeoChip;
00112   int minp0, maxp0, minp1, maxp1;
00113   int counter0, counter1;
00114   int kbd;
00115 };
00116 
00117 // ######################################################################
00118 //! Drive the Beobot under remote control
00119 /*! This simple test program demonstrate how to capture PWM signals
00120   from the Beochip, which on the Beobot correspond to steering and
00121   speed inputs from the remote control. The captured signals are then
00122   directly fed into the Beobot's steering and speed servos controlled
00123   by the BeoChip. */
00124 int main(const int argc, const char* argv[])
00125 {
00126   MYLOGVERB = LOG_INFO;
00127 
00128   // instantiate a model manager:
00129   ModelManager manager("Remote BeoChip");
00130 
00131   // Instantiate our various ModelComponents:
00132   nub::soft_ref<BeoChip> b(new BeoChip(manager));
00133   manager.addSubComponent(b);
00134 
00135   nub::soft_ref<Beowulf>
00136   beo(new Beowulf(manager, "Beowulf Slave", "BeowulfSlave", false));
00137   manager.addSubComponent(beo);
00138 
00139   // Parse command-line:
00140   if (manager.parseCommandLine(argc, argv, "<serdev>", 1, 1) == false)
00141     return(1);
00142 
00143   // let's configure our serial device:
00144   b->setModelParamVal("BeoChipDeviceName", manager.getExtraArg(0));
00145 
00146   // let's register our listener:
00147   rutz::shared_ptr<MyBeoChipListener> lis(new MyBeoChipListener(b));
00148   rutz::shared_ptr<BeoChipListener> lis2; lis2.dynCastFrom(lis); // cast down
00149   b->setListener(lis2);
00150 
00151   // setup signal handling:
00152   signal(SIGHUP, terminate); signal(SIGINT, terminate);
00153   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00154 
00155   TCPmessage rmsg;            // message being received and to process
00156   TCPmessage smsg;            // message being sent
00157 
00158   // let's get all our ModelComponent instances started:
00159   manager.start();
00160 
00161   // let's play with the LCD:
00162   b->lcdClear();   // 01234567890123456789
00163   b->lcdPrintf(0, 0, "collectFrames: 00000");
00164   b->lcdPrintf(0, 1, "STEER=XXX  SPEED=XXX");
00165   b->lcdPrintf(0, 2, "PWM0=0000  0000-0000");
00166   b->lcdPrintf(0, 3, "PWM1=0000  0000-0000");
00167 
00168   int32 rframe, raction, rnode = -1;  // receive from any node
00169   while(!beo->receive(rnode, rmsg, rframe, raction, 5));
00170   LINFO("size: %d", rmsg.getSize());
00171   rmsg.reset(rframe, raction);
00172 
00173   // wait for keyboard and process it:
00174   while(goforever) {
00175 
00176     // print keyboard values:
00177     char kb[6]; kb[5] = '\0';
00178     for (int i = 0; i < 5; i ++) kb[i] = (lis->kbd>>(4-i))&1 ? '1':'0';
00179 
00180     // quit if both extreme keys pressed simultaneously:
00181     if (kb[0] == '0' && kb[4] == '0') {
00182       b->lcdPrintf(15, 0, "QUIT ");
00183       goforever = false; break;
00184     }
00185 
00186     if (kb[1] == '0' || kb[3] == '0') {
00187 
00188       smsg.reset(0, 1);
00189       if (kb[1] == '0')
00190         {
00191           // start capturing frames
00192           smsg.addInt32(1);
00193           b->lcdPrintf(15, 0, "CAPTR");
00194           LINFO("start capturing");
00195         }
00196       else
00197         {
00198           // stop capturing frames
00199           smsg.addInt32(0);
00200            b->lcdPrintf(15, 0, "STOP ");
00201          LINFO("stop capturing");
00202         }
00203       beo->send(-1, smsg);
00204     }
00205     usleep(50000);
00206 
00207     // receive messages from other nodes
00208     int sNum, zero, min, max; float steering;
00209     if(beo->receive(rnode, rmsg, rframe, raction, 5))
00210       {
00211         switch(raction)
00212           {
00213           case RBEOCHIP_RESET:
00214             // reset the beochip:
00215             LINFO("Resetting BeoChip...");
00216             b->resetChip(); sleep(1);
00217             rmsg.reset(rframe, raction);
00218 
00219             // keep the gear at the lowest speed/highest torque
00220             b->setServoRaw(2, 0);
00221 
00222             // turn on the keyboard
00223             b->debounceKeyboard(true);
00224             b->captureKeyboard(true);
00225             break;
00226           case RBEOCHIP_CAL:
00227             sNum = int(rmsg.getElementInt32());
00228             zero = int(rmsg.getElementInt32());
00229             min = int(rmsg.getElementInt32());
00230             max = int(rmsg.getElementInt32());
00231 
00232             // calibrate the PWMs:
00233             LINFO("Calibrating BeoChip Servo: %d",sNum);
00234             b->calibratePulse(sNum, zero, min, max);
00235             b->capturePulse(sNum, true);
00236             break;
00237 
00238           case RBEOCHIP_SETSERVO:
00239             sNum = int(rmsg.getElementInt32());
00240             steering = float(rmsg.getElementFloat());
00241             LINFO("Setting BeoChip Servo: %d to %f",sNum, steering);
00242             b->setServo(sNum, steering);
00243             break;
00244           }
00245       }
00246   }
00247   manager.stop();
00248   return 0;
00249 }
00250 
00251 // ######################################################################
00252 /* So things look consistent in everyone's emacs... */
00253 /* Local Variables: */
00254 /* indent-tabs-mode: nil */
00255 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3