PingSonar.C

00001 /*!@file Devices/PingSonar.H Interface to a parallax ping sonar */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: farhan baluch fbaluch@usc.edu
00033 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Devices/PingSonar.C $
00034 // $Id: PingSonar.C 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 #include "Devices/PingSonar.H"
00038 #include "Component/OptionManager.H"
00039 #include "Devices/Serial.H"
00040 
00041 // ######################################################################
00042 PingSonar::PingSonar(OptionManager& mgr, const std::string& descrName,
00043                      const std::string& tagName, const char *defdev,
00044                      const int ns) :
00045   ModelComponent(mgr, descrName, tagName),
00046   itsPort(new Serial(mgr))
00047 {
00048   // set a default config for our serial port:
00049   itsPort->configure(defdev, 115200, "8N1", false, false, 1);
00050   // attach our port as a subcomponent:
00051   addSubComponent(itsPort);
00052   numSonars = ns;
00053 }
00054 
00055 // ######################################################################
00056 PingSonar::~PingSonar()
00057 { }
00058 
00059 // ######################################################################
00060 void PingSonar::setNumSonars(int n)
00061 {
00062     numSonars = n;
00063 }
00064 
00065 
00066 // ######################################################################
00067 std::vector<int> PingSonar::getDists()
00068 {
00069     unsigned char start ={255};
00070     unsigned char end = {255};
00071 
00072     unsigned int numBytes = numSonars*4;
00073     int cnt = 0;
00074     currentDists.clear();
00075 
00076 
00077     std::vector<unsigned char> frame = itsPort->readFrame(start,end,
00078                                                           numBytes,1);
00079 
00080     LINFO("got %"ZU" bytes",frame.size());
00081     if(frame.size() == numBytes)
00082     {
00083         for(int i=0;i<numSonars;i++)
00084         {
00085             int tempDist = ((0x0FF & frame[cnt])  << 24) |
00086             ((0x0FF & frame[cnt + 1])  << 16) |
00087             ((0x0FF & frame[cnt + 2])  << 8)  |
00088             ((0x0FF & frame[cnt + 3])  << 0);
00089             currentDists.push_back(tempDist);
00090             cnt = cnt+4;
00091         }
00092 
00093     }
00094     else
00095     {
00096         LFATAL("bad packets");
00097     }
00098 
00099     usleep(10000);
00100     return currentDists;
00101 
00102 
00103 }
00104 // ######################################################################
00105 /* So things look consistent in everyone's emacs... */
00106 /* Local Variables: */
00107 /* indent-tabs-mode: nil */
00108 /* End: */
Generated on Sun May 8 08:40:37 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3