Main Page | Class Hierarchy | Alphabetical List | Class List | File List | Class Members | Related Pages

ArIRs.cpp

00001 /*
00002 ActivMedia Robotics Interface for Applications (ARIA)
00003 Copyright (C) 2004,2005 ActivMedia Robotics, LLC
00004 
00005 
00006      This program is free software; you can redistribute it and/or modify
00007      it under the terms of the GNU General Public License as published by
00008      the Free Software Foundation; either version 2 of the License, or
00009      (at your option) any later version.
00010 
00011      This program is distributed in the hope that it will be useful,
00012      but WITHOUT ANY WARRANTY; without even the implied warranty of
00013      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014      GNU General Public License for more details.
00015 
00016      You should have received a copy of the GNU General Public License
00017      along with this program; if not, write to the Free Software
00018      Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019 
00020 If you wish to redistribute ARIA under different terms, contact 
00021 ActivMedia Robotics for information about a commercial version of ARIA at 
00022 robots@activmedia.com or 
00023 ActivMedia Robotics, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
00024 
00025 */
00026 
00027 #include "ArExport.h"
00028 #include "ariaOSDef.h"
00029 #include "ArRobot.h"
00030 #include "ariaUtil.h"
00031 #include "ArIRs.h"
00032 
00041 ArIRs::ArIRs(size_t currentBufferSize, size_t cumulativeBufferSize, 
00042                       const char *name, int maxSecondsToKeepCurrent) :
00043   ArRangeDevice(currentBufferSize, cumulativeBufferSize, name, 5000, maxSecondsToKeepCurrent), 
00044   myProcessCB(this, &ArIRs::processReadings)
00045 {
00046   setCurrentDrawingData(new ArDrawingData("polyArrows", ArColor(255, 255, 0),
00047                                           120, // mm diameter of dots
00048                                           80), // layer above sick and sonar below bumpers
00049                         true);
00050 }
00051 
00052 ArIRs::~ArIRs()
00053 {
00054   if (myRobot != NULL)
00055     {
00056       myRobot->remSensorInterpTask(&myProcessCB);
00057       myRobot->remRangeDevice(this);
00058     }
00059 }
00060 
00061 void ArIRs::setRobot(ArRobot *robot)
00062 {
00063   myRobot = robot;
00064   if (myRobot != NULL)
00065     myRobot->addSensorInterpTask(myName.c_str(), 10, &myProcessCB);
00066   ArRangeDevice::setRobot(robot);
00067 
00068   const ArRobotParams *params;
00069   params = myRobot->getRobotParams();
00070   myParams = *params;
00071 
00072   for(int i = 0; i < myParams.getNumIR(); i++)
00073     cycleCounters.push_back(1);
00074 }
00075 
00079 void ArIRs::processReadings(void)
00080 {
00081   ArUtil::BITS bit;
00082   if(myParams.haveTableSensingIR())
00083     {
00084       for (int i = 0; i < myParams.getNumIR(); ++i)
00085         {
00086           switch(i)
00087             {
00088             case 0:
00089               bit = ArUtil::BIT0;
00090               break;
00091             case 1:
00092               bit = ArUtil::BIT1;
00093               break;
00094             case 2:
00095               bit = ArUtil::BIT2;
00096               break;
00097             case 3:
00098               bit = ArUtil::BIT3;
00099               break;
00100             case 4:
00101               bit = ArUtil::BIT4;
00102               break;
00103             case 5:
00104               bit = ArUtil::BIT5;
00105               break;
00106             case 6:
00107               bit = ArUtil::BIT6;
00108               break;
00109             case 7:
00110               bit = ArUtil::BIT7;
00111               break;
00112             }
00113 
00114           if(myParams.haveNewTableSensingIR() && myRobot->getIODigInSize() > 3)
00115             {
00116               if((myParams.getIRType(i) && !(myRobot->getIODigIn(3) & bit)) ||
00117                   (!myParams.getIRType(i) && (myRobot->getIODigIn(3) & bit)))
00118                 {
00119                   if(cycleCounters[i] < myParams.getIRCycles(i))
00120                     {
00121                       cycleCounters[i] = cycleCounters[i] + 1;                
00122                     }
00123                   else
00124                     {
00125                       cycleCounters[i] = 1;
00126                       ArPose pose;
00127                       pose.setX(myParams.getIRX(i));
00128                       pose.setY(myParams.getIRY(i));
00129                       
00130                       ArTransform global = myRobot->getToGlobalTransform();
00131                       pose = global.doTransform(pose);
00132                       
00133                       myCurrentBuffer.addReading(pose.getX(), pose.getY());
00134                     }
00135                 }
00136               else
00137                 {
00138                   cycleCounters[i] = 1;
00139                 }
00140             }
00141           else
00142             {
00143               if(!(myRobot->getDigIn() & bit))
00144                 {
00145                   if(cycleCounters[i] < myParams.getIRCycles(i))
00146                     {
00147                       cycleCounters[i] = cycleCounters[i] + 1;                
00148                     }
00149                   else
00150                     {
00151                       cycleCounters[i] = 1;
00152                       
00153                       ArPose pose;
00154                       pose.setX(myParams.getIRX(i));
00155                       pose.setY(myParams.getIRY(i));
00156                       
00157                       ArTransform global = myRobot->getToGlobalTransform();
00158                       pose = global.doTransform(pose);
00159                       
00160                       myCurrentBuffer.addReading(pose.getX(), pose.getY());
00161                     }
00162                 }
00163               else
00164                 {
00165                   cycleCounters[i] = 1;
00166                 }
00167             }
00168         }
00169     }
00170 }

Generated on Wed Oct 19 12:56:35 2005 for Aria by  doxygen 1.4.0