BotControl.C

Go to the documentation of this file.
00001 /*!@file RCBot/BotControl.C  abstract robot control (can use corba)  */
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/RCBot/BotControl.C $
00035 // $Id: BotControl.C 13993 2010-09-20 04:54:23Z itti $
00036 //
00037 // ######################################################################
00038 
00039 #include "RCBot/BotControl.H"
00040 #include "Component/OptionManager.H"
00041 #include "Beobot/BeobotConfig.H"
00042 
00043 BeobotConfig bbc;
00044 
00045 // ######################################################################
00046 BotControl::BotControl(OptionManager& mgr, const std::string& descrName,
00047                        const std::string& tagName, RobotType inBotType) :
00048   ModelComponent(mgr, descrName, tagName),
00049   botType(inBotType)
00050 {
00051   // switch for the the actuator system
00052   switch(botType)
00053     {
00054     case RCBOT_JEEP:
00055     case RCBOT_TRUCK:
00056       sc8000 = nub::ref<SC8000>(new SC8000(mgr));
00057       addSubComponent(sc8000);
00058       break;
00059 
00060     case WIREBOT:
00061       ssc = nub::soft_ref<SSC>(new SSC(mgr));
00062       addSubComponent(ssc);
00063       break;
00064 
00065     case BEOBOT:
00066       bc = nub::soft_ref<BeoChip>(new BeoChip(mgr));
00067       addSubComponent(bc);
00068 
00069       // configure the BeoChip
00070       bc->setModelParamVal("BeoChipDeviceName", std::string("/dev/ttyS0"));
00071       break;
00072 
00073     case SIMULATION:
00074       break;
00075     }
00076 
00077   // for frame grabber:
00078   // --in=ieee1394 --framegrabber-mode=YUV44 --framegrabber-dims=160x120
00079   //
00080   // for mpg input:
00081   // --in=fname.mpg --rescale-input=160x120 --crop-input=x1,y1,x2,y2
00082   //
00083   // for raster series input:
00084   // --in=prefix#.ppm --rescale-input=160x120 --crop-input=x1,y1,x2,y2
00085   ifs =  nub::ref<InputFrameSeries>(new InputFrameSeries(mgr));
00086   addSubComponent(ifs);
00087 
00088   // default is we do not display the image
00089   dispImg = true;
00090 }
00091 
00092 // ######################################################################
00093 void BotControl::setDisplay(bool sd)
00094 {
00095   if(dispImg != sd)
00096     {
00097       dispImg = sd;
00098       if(dispImg)
00099         xwin = new XWinManaged(Dims(width,height), -1, -1, "Bot Control");
00100       else
00101         delete(xwin);
00102     }
00103 }
00104 
00105 // ######################################################################
00106 BotControl::~BotControl()
00107 {  }
00108 
00109 // ######################################################################
00110 void BotControl::init()
00111 {
00112   LINFO("initializing BotControl");
00113   switch(botType)
00114     {
00115     case RCBOT_JEEP:
00116       //set the servos
00117       speedServo = 3;
00118       steerServo = 1;
00119 
00120       //calibrate the servos
00121       sc8000->calibrate(steerServo, 13650, 10800, 16000); //for steering
00122       sc8000->calibrate(speedServo, 14000, 12000, 16000); //for speed
00123       break;
00124 
00125     case RCBOT_TRUCK:
00126       LINFO("RC TRUCK");
00127       //set the servos
00128       speedServo = 3;
00129       steerServo = 4;
00130 
00131       //calibrate the servos
00132       sc8000->calibrate(steerServo, 10000, 15000, 20000); //for steering
00133       sc8000->calibrate(speedServo, 10000, 15000, 19000); //for speed
00134 
00135       sc8000->calibrate(1, 13000, 16000, 20000); //for speed
00136       sc8000->calibrate(2, 10000, 17000, 19000); //for speed
00137       sc8000->move(1, 0);
00138       sc8000->move(2, 0);
00139       break;
00140 
00141     case WIREBOT:
00142       LINFO("Wire Bot");
00143       //set the servos
00144       driveServoRight = 0;
00145       driveServoLeft = 1;
00146 
00147       //calibrate the servos
00148       //FIXME ssc->calibrate(driveServoRight, 1157, 757, 1557); //for steering
00149       //FIXME ssc->calibrate(driveServoLeft, 1161, 761, 1561); //for speed
00150 
00151       ssc->move(driveServoRight, 0);
00152       ssc->move(driveServoLeft, 0);
00153       break;
00154 
00155     case BEOBOT:
00156       LINFO("BeoBot");
00157 
00158       // reset the beochip:
00159       LINFO("Resetting BeoChip...");
00160       bc->resetChip(); sleep(1);
00161 
00162       //set the servos
00163       speedServo = bbc.speedServoNum;
00164       steerServo = bbc.steerServoNum;
00165 
00166       // keep the gear at the lowest speed/highest torque
00167       bc->setServoRaw(bbc.gearServoNum, bbc.gearMinVal);
00168 
00169       // turn on the keyboard
00170       bc->debounceKeyboard(true);
00171       bc->captureKeyboard(true);
00172 
00173       // calibrate the PWMs:
00174       bc->calibratePulse(0,
00175                          bbc.pwm0NeutralVal,
00176                          bbc.pwm0MinVal,
00177                          bbc.pwm0MaxVal);
00178       bc->calibratePulse(1,
00179                          bbc.pwm1NeutralVal,
00180                          bbc.pwm1MinVal,
00181                          bbc.pwm1MaxVal);
00182       bc->capturePulse(0, true);
00183       bc->capturePulse(1, true);
00184       break;
00185 
00186     case SIMULATION:
00187       break;
00188     }
00189 
00190   // reset fps
00191   avgtime = 0; avgn = 0; fps = 0.0F;
00192   timer.reset();
00193 
00194   width = ifs->getWidth();
00195   height = ifs->getHeight();
00196   xwin = new XWinManaged(Dims(width,height), -1, -1, "Bot Control");
00197 }
00198 
00199 // ######################################################################
00200 float BotControl::getSpeed()
00201 {
00202   switch (botType)
00203     {
00204     case WIREBOT:
00205       return 0.0;
00206     case BEOBOT:
00207       return (bc->getServo(speedServo));
00208 
00209     case RCBOT_JEEP:
00210     case RCBOT_TRUCK:
00211       return sc8000->getPosition(speedServo);
00212 
00213     case SIMULATION:
00214       // nothing to do
00215       return 0.0;
00216     }
00217   return 0.0;
00218 }
00219 
00220 // ######################################################################
00221 bool BotControl::setSpeed(const float speedPos)
00222 {
00223   speed = speedPos;
00224   switch (botType)
00225     {
00226     case WIREBOT:
00227       return (ssc->move(driveServoRight, -1*speed)
00228               && ssc->move(driveServoLeft, speed) );
00229     case BEOBOT:
00230       return (bc->setServo(speedServo, speed));
00231 
00232     case RCBOT_JEEP:
00233     case RCBOT_TRUCK:
00234       return sc8000->move(speedServo, speed);
00235 
00236     case SIMULATION:
00237       // nothing to do
00238       return false;
00239     }
00240 
00241   return false;
00242 }
00243 
00244 // ######################################################################
00245 float BotControl::getSteering()
00246 {
00247   switch (botType)
00248     {
00249     case WIREBOT:
00250       return 0.0;
00251     case BEOBOT:
00252       return (bc->getServo(steerServo));
00253 
00254     case RCBOT_JEEP:
00255     case RCBOT_TRUCK:
00256 
00257       return sc8000->getPosition(steerServo);
00258 
00259     case SIMULATION:
00260       // nothing to do
00261       return 0.0;
00262     }
00263   return 0.0;
00264 }
00265 
00266 // ######################################################################
00267 bool BotControl::setSteering(const float steeringPos)
00268 {
00269   steering = steeringPos;
00270 
00271   switch (botType)
00272     {
00273     case WIREBOT:
00274       return (ssc->move(driveServoRight, steeringPos) &&
00275               ssc->move(driveServoLeft, steeringPos) );
00276 
00277     case BEOBOT:
00278       return (bc->setServo(steerServo, steering));
00279 
00280     case RCBOT_JEEP:
00281     case RCBOT_TRUCK:
00282       return sc8000->move(steerServo, steeringPos);
00283 
00284     case SIMULATION:
00285       // nothing to do
00286       return false;
00287     }
00288   return false;
00289 }
00290 
00291 // ######################################################################
00292 Image<PixRGB<byte> > BotControl::getImageSensor(int i)
00293 {
00294   ifs->updateNext();
00295   Image<PixRGB<byte> > img = ifs->readRGB();
00296 
00297   // compute frame rate
00298   avgtime += timer.getReset(); avgn ++;
00299   if (avgn == NAVG)
00300     {
00301       fps = 1000.0F / float(avgtime) * float(avgn);
00302       avgtime = 0; avgn = 0;
00303     }
00304 
00305   if (img.initialized() && dispImg) showImg(img);
00306   return img;
00307 }
00308 
00309 // ######################################################################
00310 void BotControl::getImageSensorDims(short &w, short &h, int i)
00311 {
00312   w = width;
00313   h = height;
00314 }
00315 
00316 // ######################################################################
00317 void BotControl::setInfo(const char *info, Point2D<int> inTrackLoc,
00318                          Point2D<int> inRecLoc)
00319 {
00320   extraInfo = strdup(info);
00321   trackLoc = inTrackLoc;
00322   recLoc = inRecLoc;
00323 }
00324 
00325 // ######################################################################
00326 void BotControl::showImg(Image<PixRGB<byte> > &img)
00327 {
00328   Image<PixRGB<byte> > disp = img;
00329   char info[255];
00330 
00331   if (recLoc.isValid())
00332     drawDisk(disp, Point2D<int>(recLoc.i, recLoc.j),
00333              10, PixRGB<byte>(255, 255, 20));
00334   if (trackLoc.isValid())
00335     drawDisk(disp, Point2D<int>(trackLoc.i, trackLoc.j),
00336              6, PixRGB<byte>(20, 50, 255));
00337 
00338   sprintf(info, " %.1ffps %s", fps, extraInfo);
00339 
00340   writeText(disp, Point2D<int>(0,220), info,
00341             PixRGB<byte>(255), PixRGB<byte>(127));
00342 
00343   xwin->drawImage(disp);
00344 }
00345 
00346 // ######################################################################
00347 int BotControl::getUserInput(Point2D<int> &loc)
00348 {
00349   loc = xwin->getLastMouseClick();
00350   return  xwin->getLastKeyPress();
00351 }
00352 
00353 // ######################################################################
00354 /* So things look consistent in everyone's emacs... */
00355 /* Local Variables: */
00356 /* indent-tabs-mode: nil */
00357 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3