00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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,
00048 80),
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 }