IRobotI.cpp

Go to the documentation of this file.
00001 /*!@file IRobotI.cpp IRobot service implimantation  */
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 <lelazary@yahoo.com>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/IRobot/irobotService/IRobotI.cpp $
00035 // $Id: IRobotI.cpp 10794 2009-02-08 06:21:09Z itti $
00036 //
00037 
00038 #include <stdlib.h>
00039 #include <time.h>
00040 #include <iostream>
00041 #include <unistd.h>
00042 #include <fcntl.h>
00043 #include <sys/types.h>
00044 #include <sys/stat.h>
00045 #include <signal.h>
00046 #include <IRobotI.h>
00047 #include "capture.h"
00048 #include "serial.h"
00049 
00050 
00051 #include <IceE/IceE.h>
00052 
00053 IRobotI::IRobotI(int debug) :
00054   itsCurrentSpeed(0),
00055   itsCurrentSteering(0),
00056   itsDebug(debug),
00057   itsVideoInit(-1)
00058 {
00059   //start the serial device
00060   itsSerialFd = openPort("/dev/ttyS2");
00061 
00062 //#ifdef USEMMJPEG
00063   printf("Using MJpeg\n");
00064   //Requested Camera Resolution
00065   int width = 320;
00066   int height = 240;
00067   //Requested framerate
00068   int fps = 10;
00069   //Video device file
00070   char* videodevice = "/dev/video0";
00071 
00072   int format = V4L2_PIX_FMT_MJPEG;
00073   int grabmethod = 1;
00074   char *avifilename = NULL;
00075 
00076   colorspace_init();
00077   //Allocate our video input structure
00078   itsVideoIn = (struct vdIn *) calloc(1, sizeof(struct vdIn));
00079   //Initialize our color lookup tables
00080   initLut();
00081   ////Initialize the video input data structure
00082   init_videoIn(itsVideoIn,
00083       (char *) videodevice,
00084       width, height, fps, format,
00085       grabmethod, avifilename);
00086   itsVideoInit = 1;
00087 //#else
00088 //  open_device ();
00089 //  init_device (0);
00090 //  start_capturing();
00091 //  itsVideoInit = 1;
00092 //#endif
00093 
00094   if (itsDebug)
00095     printf("IRobot initalized\n");
00096 }
00097 
00098 IRobotI::~IRobotI() {
00099   closePort(itsSerialFd);
00100 }
00101 
00102 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00103 float IRobotI::getSpeed(const Ice::Current&){
00104   return itsCurrentSpeed;
00105 }
00106 
00107 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00108 short IRobotI::setSpeed(const float speed, const Ice::Current&){
00109   if (itsDebug)
00110     printf("Setting speed to %f\n", speed);
00111   itsCurrentSpeed = speed;
00112 
00113 
00114   sendDirectDriveCommand();
00115 
00116   return 0;
00117 }
00118 
00119 
00120 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00121 float IRobotI::getSteering(const Ice::Current&){
00122   return itsCurrentSteering;
00123 }
00124 
00125 
00126 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00127 short IRobotI::setSteering(const float steeringPos, const Ice::Current&){
00128   if (itsDebug)
00129     printf("Setting steering to %f\n", steeringPos);
00130   itsCurrentSteering = steeringPos;
00131   sendDirectDriveCommand();
00132   return 0;
00133 }
00134 
00135 
00136 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00137 ImageIceMod::ImageIce IRobotI::getImageSensor(const short indx, const bool useColor, const Ice::Current&){
00138 
00139   //frame* f = get_frame(false);
00140   ImageIceMod::ImageIce imgRet;
00141 
00142   if (itsVideoInit == -1)
00143     return imgRet;
00144 
00145 //#ifdef USEMJEPEG
00146   int jpeg_decode_stat;
00147   uvcGrab(itsVideoIn);
00148   imgRet.width = itsVideoIn->width;
00149   imgRet.height = itsVideoIn->height;
00150 
00151   int size = itsVideoIn->width*itsVideoIn->height;
00152   unsigned char frameData[size*3];
00153   Pyuv422torgb24(itsVideoIn->framebuffer,
00154       frameData,
00155       itsVideoIn->width,
00156       itsVideoIn->height);
00157 
00158 
00159   if (useColor)
00160   {
00161     imgRet.pixSize = 3;
00162     imgRet.data.resize(size*imgRet.pixSize);
00163     std::copy(frameData, frameData + (size*imgRet.pixSize), imgRet.data.begin());
00164 
00165   } else {
00166     imgRet.pixSize = 1;
00167     const unsigned char*sPtr = frameData;
00168     imgRet.data.resize(size*imgRet.pixSize);
00169 
00170     for(int i=0; i<size; i++)
00171     {
00172       imgRet.data[i] = (*sPtr + *(sPtr+1) + *(sPtr+2))/3;
00173       sPtr += 3;
00174     }
00175 
00176   }
00177 
00178 
00179 
00180 //#else
00181 //
00182 //  frame* f = get_frame(useColor);
00183 //
00184 //  if (useColor)
00185 //    imgRet.pixSize = 3;
00186 //  else
00187 //    imgRet.pixSize = 1;
00188 //
00189 //  int size = f->width*f->height*imgRet.pixSize;
00190 //  imgRet.width = f->width;
00191 //  imgRet.height = f->height;
00192 //
00193 //  imgRet.data.resize(size);
00194 //  if (useColor)
00195 //    std::copy(f->data, f->data + size, imgRet.data.begin());
00196 //  else
00197 //    std::copy(f->lumData, f->lumData + size, imgRet.data.begin());
00198 //
00199 //#endif
00200   return imgRet;
00201 }
00202 
00203 
00204 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00205 void IRobotI::sendDriveCommand()
00206 {
00207   unsigned char cmd[5];
00208   cmd[0] = 137; //drive command
00209   cmd[1] = ((short int)itsCurrentSpeed&0xFFFF)>>8; //velocity high byte
00210   cmd[2] = ((short int)itsCurrentSpeed&0xFF); //velocity low byte
00211 
00212   if (itsCurrentSteering == 0) //drive striaght
00213   {
00214     cmd[3] = 0x7F; //Radius high byte
00215     cmd[4] = 0xFF; //Radius low byte
00216   } else {
00217     cmd[3] = ((short int)itsCurrentSteering&0xFFFF)>>8; //Radius high byte
00218     cmd[4] = ((short int)itsCurrentSteering&0xFF); //Radius low byte
00219   }
00220 
00221 
00222   if (itsDebug)
00223   {
00224     printf("Sending: ");
00225     for(int i=0; i<5; i++)
00226       printf("%i ", cmd[i]);
00227     printf("\n");
00228   }
00229   sendData(itsSerialFd, cmd, 5);
00230 
00231 }
00232 
00233 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00234 void IRobotI::sendDirectDriveCommand()
00235 {
00236 
00237   int rightWheel = (int)((itsCurrentSpeed*500) + (itsCurrentSteering*500));
00238   int leftWheel = (int)((itsCurrentSpeed*500) - (itsCurrentSteering*500));
00239 
00240   if (rightWheel > 500) rightWheel = 500;
00241   if (rightWheel < -500) rightWheel = -500;
00242 
00243   if (leftWheel > 500) leftWheel = 500;
00244   if (leftWheel < -500) leftWheel = -500;
00245 
00246   unsigned char cmd[5];
00247   cmd[0] = 145; //Direct drive command
00248   cmd[1] = ((short int)rightWheel&0xFFFF)>>8; //Right Wheel high byte
00249   cmd[2] = ((short int)rightWheel&0xFF); //Right Wheel low byte
00250 
00251   cmd[3] = ((short int)leftWheel&0xFFFF)>>8; //Left Wheel high byte
00252   cmd[4] = ((short int)leftWheel&0xFF); //Left Wheel low byte
00253 
00254 
00255   if (itsDebug)
00256   {
00257     printf("Sending: ");
00258     for(int i=0; i<5; i++)
00259       printf("%i ", cmd[i]);
00260     printf("\n");
00261   }
00262   sendData(itsSerialFd, cmd, 5);
00263 
00264 
00265 //  unsigned char data[255];
00266 //  int len = read(itsSerialFd, data, 255);
00267 //  printf("Got %i\n", len);
00268 }
00269 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00270 void IRobotI::sendStart(const Ice::Current&)
00271 {
00272   unsigned char cmd[1];
00273   cmd[0] = 128;
00274   sendData(itsSerialFd, cmd, 1);
00275 }
00276 
00277 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00278 void IRobotI::setMode(const Robots::IRobotModes mode, const Ice::Current&)
00279 {
00280   unsigned char cmd[1];
00281   cmd[0] = 0;
00282 
00283   switch(mode)
00284   {
00285     case Robots::SafeMode: cmd[0] = 131; break;
00286     case Robots::FullMode: cmd[0] = 132; break;
00287     case Robots::SpotMode: cmd[0] = 134; break;
00288     case Robots::CoverMode: cmd[0] = 135; break;
00289     case Robots::CoverAndDockMode: cmd[0] = 143; break;
00290   };
00291 
00292   sendData(itsSerialFd, cmd, 1);
00293 
00294 }
00295 
00296 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00297 void IRobotI::setDemo(const short demo, const Ice::Current&)
00298 {
00299   unsigned char cmd[2];
00300   cmd[0] = 136;
00301   cmd[1] = demo;
00302 
00303   if (itsDebug)
00304   {
00305     printf("Sending: ");
00306     for(int i=0; i<2; i++)
00307       printf("%i ", cmd[i]);
00308     printf("\n");
00309   }
00310   sendData(itsSerialFd, cmd, 2);
00311 
00312 }
00313 
00314 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00315 void IRobotI::setLED(const short led, const short color, const short intensity, const Ice::Current&)
00316 {
00317   unsigned char cmd[4];
00318   cmd[0] = 139;
00319 
00320   switch(led)
00321   {
00322     case 1: cmd[1] = 8; break; //Advance LED
00323     case 2: cmd[1] = 2; break; //Play Led
00324     case 3: cmd[1] = 10; break; //Both Led
00325     default: cmd[1] = 0; //default to turn off
00326   };
00327 
00328   cmd[2] = color;
00329   cmd[3] = intensity;
00330 
00331   if (itsDebug)
00332   {
00333     printf("Sending: ");
00334     for(int i=0; i<2; i++)
00335       printf("%i ", cmd[i]);
00336     printf("\n");
00337   }
00338   sendData(itsSerialFd, cmd, 4);
00339 
00340 }
00341 
00342 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00343 void IRobotI::playSong(const short song, const Ice::Current&)
00344 {
00345 
00346   if (itsDebug)
00347     printf("Play song %i\n", song);
00348   // The imperial March Song
00349   // G  G  G  E  Bb  G  E  Bb  G  2D  2D  2D  2Eb  Bb  F#  E  Bb  G
00350   // 55 55 55 52 58 55 52 58  55  62  62  62  63   58  54  51 58  55
00351   unsigned char s1[21] = {140, 0, 9, 55, 30, 55, 30, 55, 30, 51, 30, 58, 12, 55, 30, 51, 30, 58, 12, 55, 30};
00352   //unsigned char s2[21] = {140, 0, 9, 55, 30, 55, 30, 55, 30, 51, 30, 58, 12, 55, 30, 51, 30, 58, 12, 55, 30};
00353   unsigned char s2[21] = {140, 1, 9, 62, 30, 62, 30, 62, 30, 63, 30, 58, 12, 54, 30, 51, 30, 58, 12, 55, 30};
00354 
00355   unsigned char cmd[2] ;
00356 
00357   sendData(itsSerialFd, s1, 21);
00358   sendData(itsSerialFd, s2, 21);
00359 
00360   cmd[0] = 141; cmd[1] = song;
00361   sendData(itsSerialFd, cmd, 2);
00362   //sleep(4);
00363   //cmd[0] = 141; cmd[1] = 1;
00364   //sendData(itsSerialFd, cmd, 2);
00365 }
00366 
00367 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00368 ImageIceMod::DimsIce IRobotI::getImageSensorDims(const short i, const Ice::Current&) {
00369   ImageIceMod::DimsIce dims;
00370   dims.w = -1;
00371   dims.h = -1;
00372 
00373   return dims;
00374 }
00375 
00376 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00377 float IRobotI::getSensorValue(const short i, const Ice::Current&) {
00378 
00379   //Query the distance and angle sensors
00380   unsigned char cmd[4];
00381   unsigned char data[255];
00382   int len;
00383   cmd[0] = 149; //Query list opcode
00384   cmd[1] = 1; //we need two sensors
00385   cmd[2] = i; //distance
00386 
00387   sendData(itsSerialFd, cmd, 3);
00388 
00389   //get the data
00390 
00391   len = read(itsSerialFd, data, 4);
00392   if (len == 2)
00393   {
00394     int16_t val = (int16_t)((data[0]<<8) | data[1]);
00395     return (float)val;
00396   } else {
00397     return (float)(data[0]);
00398   }
00399 
00400   return -1;
00401 }
00402 
00403 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00404 bool IRobotI::getDistanceAngle(float& dist, float& ang, const Ice::Current&) {
00405 
00406   //Query the distance and angle sensors
00407   unsigned char cmd[4];
00408   unsigned char data[255];
00409   int len;
00410   cmd[0] = 149; //Query list opcode
00411   cmd[1] = 2; //we need two sensors
00412   cmd[2] = 19; //distance
00413   cmd[3] = 20; //angle
00414 
00415   sendData(itsSerialFd, cmd, 4);
00416 
00417   //get the data
00418 
00419   len = read(itsSerialFd, data, 4);
00420   //printf("Read %i\n", len);
00421   short int rDist = 0, rAng = 0;
00422   //if (len == 4)
00423   {
00424    // printf("%i %i %i %i\n\n",
00425    //     data[0], data[1], data[2], data[3]);
00426 
00427     rDist = (int16_t)((data[0]<<8) | data[1]);
00428     rAng = (int16_t)((data[2]<<8) | data[3]);
00429 
00430   }
00431 
00432   dist = rDist;
00433   ang = rAng;
00434 
00435 
00436   return true;
00437 }
00438 
00439 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00440 void IRobotI::motorsOff(const short i, const Ice::Current&)
00441 {
00442   itsCurrentSteering = 0;
00443   itsCurrentSpeed = 0;
00444   sendDirectDriveCommand();
00445 }
00446 
00447 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00448 void IRobotI::setMotor(const short i, const float val, const Ice::Current&)
00449 {
00450 
00451 }
00452 
00453 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00454 short IRobotI::sendRawCmd(const std::string& s, const Ice::Current&)
00455 {
00456   return -1;
00457 }
00458 
00459 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00460 void IRobotI::shutdown(const Ice::Current& c)
00461 {
00462   motorsOff(0, c);
00463   if (itsDebug)
00464     printf("Shutting down...\n");
00465   c.adapter->getCommunicator()->shutdown();
00466 }
00467 
Generated on Sun May 8 08:41:22 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3