Aria: ArRobotParams.cpp Source File

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 
00029 
00030 #include "ariaOSDef.h"
00031 #include "ArRobotParams.h"
00032 #include "ariaInternal.h"
00033 
00034 ArRobotParams::ArRobotParams() :
00035   ArConfig(NULL, true),
00036   myIRUnitGetFunctor(this, &ArRobotParams::getIRUnits),
00037   myIRUnitSetFunctor(this, &ArRobotParams::parseIRUnit),
00038   mySonarUnitGetFunctor(this, &ArRobotParams::getSonarUnits),
00039   mySonarUnitSetFunctor(this, &ArRobotParams::parseSonarUnit)
00040 {
00041   sprintf(myClass, "Pioneer");
00042   mySubClass[0] = '\0';
00043   myRobotRadius = 250;
00044   myRobotDiagonal = 120;
00045   myRobotWidth = 400;
00046   myRobotLength = 500; 
00047   myHolonomic = true;
00048   myAbsoluteMaxVelocity = 0;
00049   myAbsoluteMaxRVelocity = 0;
00050   myHaveMoveCommand = true;
00051   myAngleConvFactor = 0.001534;
00052   myDistConvFactor = 0;
00053   myVelConvFactor = 1.0;
00054   myRangeConvFactor = 0;
00055   myVel2Divisor = 20;
00056   myGyroScaler = 1.626;
00057   myNumSonar = 0;
00058   myTableSensingIR = false;
00059   myNewTableSensingIR = false;
00060   myFrontBumpers = false;
00061   myNumFrontBumpers = 5;
00062   myRearBumpers = false;
00063   myNumRearBumpers = 5;
00064   myNumSonar = 0;
00065   myNumIR = 0;
00066   mySonarMap.clear();
00067   myIRMap.clear();
00068   myLaserPossessed = false;
00069   sprintf(myLaserPort, "COM3");
00070   myLaserFlipped = false;
00071   myLaserPowerControlled = true;
00072   myLaserX = 0;
00073   myLaserY = 0;
00074   myLaserTh = 0.0;
00075   myLaserIgnore[0] = '\0';
00076   
00077   myRequestIOPackets = false;
00078   myRequestEncoderPackets = false;
00079   mySwitchToBaudRate = 38400;
00080 
00081   mySettableVelMaxes = true;
00082   myTransVelMax = 0;
00083   myRotVelMax = 0;
00084 
00085   mySettableAccsDecs = true;
00086   myTransAccel = 0;
00087   myTransDecel = 0;
00088   myRotAccel = 0;
00089   myRotDecel = 0;
00090 
00091   addComment("Robot parameter file");
00092 //  addComment("");
00093   //addComment("General settings");
00094   std::string section;
00095   section = "General settings";
00096   addParam(ArConfigArg("Class", myClass, "general type of robot", 
00097                  sizeof(myClass)), section.c_str(), ArPriority::TRIVIAL);
00098   addParam(ArConfigArg("Subclass", mySubClass, "specific type of robot", 
00099                        sizeof(mySubClass)), section.c_str(), 
00100            ArPriority::TRIVIAL);
00101   addParam(ArConfigArg("RobotRadius", &myRobotRadius, "radius in mm"), 
00102            section.c_str(), ArPriority::NORMAL);
00103   addParam(ArConfigArg("RobotDiagonal", &myRobotDiagonal, 
00104                  "half-height to diagonal of octagon"), "General settings",
00105            ArPriority::TRIVIAL);
00106   addParam(ArConfigArg("RobotWidth", &myRobotWidth, "width in mm"), 
00107            section.c_str(), ArPriority::NORMAL);
00108   addParam(ArConfigArg("RobotLength", &myRobotLength, "length in mm"), 
00109            section.c_str(), ArPriority::NORMAL);
00110   addParam(ArConfigArg("Holonomic", &myHolonomic, "turns in own radius"), 
00111            section.c_str(), ArPriority::TRIVIAL);
00112   addParam(ArConfigArg("MaxRVelocity", &myAbsoluteMaxRVelocity, 
00113                        "absolute maximum degrees / sec"), section.c_str(),
00114            ArPriority::TRIVIAL);
00115   addParam(ArConfigArg("MaxVelocity", &myAbsoluteMaxVelocity, 
00116                  "absolute maximum mm / sec"), section.c_str(), 
00117            ArPriority::TRIVIAL);
00118   addParam(ArConfigArg("HasMoveCommand", &myHaveMoveCommand, 
00119                  "has built in move command"), section.c_str(),
00120            ArPriority::TRIVIAL);
00121   addParam(ArConfigArg("RequestIOPackets", &myRequestIOPackets,
00122                  "automatically request IO packets"), section.c_str(),
00123            ArPriority::NORMAL);
00124   addParam(ArConfigArg("RequestEncoderPackets", &myRequestEncoderPackets,
00125                        "automatically request encoder packets"), 
00126            section.c_str(), ArPriority::NORMAL);
00127   addParam(ArConfigArg("SwitchToBaudRate", &mySwitchToBaudRate, 
00128                  "switch to this baud if non-0 and supported on robot"), 
00129            section.c_str(), ArPriority::IMPORTANT);
00130   
00131   section = "Conversion factors";
00132   addParam(ArConfigArg("AngleConvFactor", &myAngleConvFactor,
00133                      "radians per angular unit (2PI/4096)"), section.c_str(),
00134            ArPriority::TRIVIAL);
00135   addParam(ArConfigArg("DistConvFactor", &myDistConvFactor,
00136                        "multiplier to mm from robot units"), section.c_str(),
00137            ArPriority::IMPORTANT);
00138   addParam(ArConfigArg("VelConvFactor", &myVelConvFactor,
00139                      "multiplier to mm/sec from robot units"), 
00140            section.c_str(),
00141            ArPriority::NORMAL);
00142   addParam(ArConfigArg("RangeConvFactor", &myRangeConvFactor, 
00143                        "multiplier to mm from sonar units"), section.c_str(),
00144            ArPriority::TRIVIAL);
00145   addParam(ArConfigArg("DiffConvFactor", &myDiffConvFactor, 
00146                      "ratio of angular velocity to wheel velocity"), 
00147            section.c_str(),
00148            ArPriority::TRIVIAL);
00149   addParam(ArConfigArg("Vel2Divisor", &myVel2Divisor, 
00150                        "divisor for VEL2 commands"), section.c_str(),
00151            ArPriority::TRIVIAL);
00152   addParam(ArConfigArg("GyroScaler", &myGyroScaler, 
00153                      "Scaling factor for gyro readings"), section.c_str(),
00154            ArPriority::IMPORTANT);
00155 
00156   section = "Accessories the robot has";
00157   addParam(ArConfigArg("TableSensingIR", &myTableSensingIR,
00158                        "if robot has upwards facing table sensing IR"), 
00159            section.c_str(),
00160            ArPriority::TRIVIAL);
00161   addParam(ArConfigArg("NewTableSensingIR", &myNewTableSensingIR,
00162                  "if table sensing IR are sent in IO packet"), 
00163            section.c_str(),
00164            ArPriority::TRIVIAL);
00165   addParam(ArConfigArg("FrontBumpers", &myFrontBumpers, 
00166                  "if robot has a front bump ring"), section.c_str(),
00167            ArPriority::IMPORTANT);
00168   addParam(ArConfigArg("NumFrontBumpers", &myNumFrontBumpers,
00169                      "number of front bumpers on the robot"), 
00170            section.c_str(),
00171            ArPriority::TRIVIAL);
00172   addParam(ArConfigArg("RearBumpers", &myRearBumpers,
00173                        "if the robot has a rear bump ring"), section.c_str(),
00174            ArPriority::IMPORTANT);
00175   addParam(ArConfigArg("NumRearBumpers", &myNumRearBumpers,
00176                        "number of rear bumpers on the robot"), section.c_str(),
00177            ArPriority::TRIVIAL);
00178 
00179   section = "IR parameters";
00180   addParam(ArConfigArg("IRNum", &myNumIR, "number of IRs on the robot"), section.c_str(), ArPriority::NORMAL);
00181    addParam(ArConfigArg("IRUnit", &myIRUnitSetFunctor, &myIRUnitGetFunctor,
00182                         "IRUnit <IR Number> <IR Type> <Persistance, cycles> <x position, mm> <y position, mm>"), 
00183             section.c_str(), ArPriority::TRIVIAL);
00184 
00185 
00186   section = "Sonar parameters";
00187   addParam(ArConfigArg("SonarNum", &myNumSonar, 
00188                      "number of sonar on the robot"), section.c_str(),
00189            ArPriority::NORMAL);
00190   addParam(ArConfigArg("SonarUnit", &mySonarUnitSetFunctor, 
00191                      &mySonarUnitGetFunctor,
00192                      "SonarUnit <sonarNumber> <x position, mm> <y position, mm> <heading of disc, degrees>"), section.c_str(), ArPriority::TRIVIAL);
00193 
00194 
00195   section = "Laser parameters";
00196   addParam(ArConfigArg("LaserPossessed", &myLaserPossessed, 
00197                      "if there is a laser on the robot"), section.c_str(),
00198            ArPriority::IMPORTANT);
00199   addParam(ArConfigArg("LaserPort", myLaserPort, "port the laser is on", 
00200                      sizeof(myLaserPort)), section.c_str(),
00201            ArPriority::NORMAL);
00202   addParam(ArConfigArg("LaserFlipped", &myLaserFlipped,
00203                      "if the laser is upside-down or not"), section.c_str(),
00204            ArPriority::NORMAL);
00205   addParam(ArConfigArg("LaserPowerControlled", &myLaserPowerControlled,
00206                      "if the power to the laser is controlled by serial"), 
00207            section.c_str(),
00208            ArPriority::TRIVIAL);
00209   addParam(ArConfigArg("LaserX", &myLaserX, "x location of laser, mm"), 
00210            section.c_str(),
00211            ArPriority::NORMAL);
00212   addParam(ArConfigArg("LaserY", &myLaserY, "y location of laser, mm"), 
00213            section.c_str(),
00214            ArPriority::NORMAL);
00215   addParam(ArConfigArg("LaserTh", &myLaserTh, "rotation of laser, deg"), 
00216            section.c_str(),
00217            ArPriority::NORMAL);
00218   addParam(ArConfigArg("LaserIgnore", myLaserIgnore, "Readings within a degree of the listed degrees (seperated by a space) will be ignored", sizeof(myLaserIgnore)), 
00219            section.c_str(),
00220            ArPriority::NORMAL);
00221 
00222   section = "Movement control parameters";
00223   setSectionComment(section.c_str(), "if these are 0 the parameters from robot flash will be used, otherwise these values will be used");
00224   addParam(ArConfigArg("SettableVelMaxes", &mySettableVelMaxes, "if TransVelMax and RotVelMax can be set"), section.c_str(),
00225            ArPriority::TRIVIAL);
00226   addParam(ArConfigArg("TransVelMax", &myTransVelMax, "maximum desired translational velocity for the robot"), section.c_str(), 
00227            ArPriority::IMPORTANT);
00228   addParam(ArConfigArg("RotVelMax", &myRotVelMax, "maximum desired rotational velocity for the robot"), section.c_str(),
00229            ArPriority::IMPORTANT);
00230   addParam(ArConfigArg("SettableAccsDecs", &mySettableAccsDecs, "if the accel and decel parameters can be set"), section.c_str(), ArPriority::TRIVIAL);
00231   addParam(ArConfigArg("TransAccel", &myTransAccel, "translational acceleration"), 
00232            section.c_str(), ArPriority::IMPORTANT);
00233   addParam(ArConfigArg("TransDecel", &myTransDecel, "translational deceleration"), 
00234 
00235            section.c_str(), ArPriority::IMPORTANT);
00236   addParam(ArConfigArg("RotAccel", &myRotAccel, "rotational acceleration"), 
00237            section.c_str());
00238   addParam(ArConfigArg("RotDecel", &myRotDecel, "rotational deceleration"),
00239            section.c_str(), ArPriority::IMPORTANT);
00240 
00241 }
00242 
00243 ArRobotParams::~ArRobotParams()
00244 {
00245 
00246 }
00247 
00248 
00249 bool ArRobotParams::parseIRUnit(ArArgumentBuilder *builder)
00250 {
00251   if (builder->getArgc() != 5 || !builder->isArgInt(0) || 
00252       !builder->isArgInt(1) || !builder->isArgInt(2) || 
00253       !builder->isArgInt(3) || !builder->isArgInt(4))
00254   {
00255     ArLog::log(ArLog::Terse, "ArRobotParams: IRUnit parameters invalid");
00256     return false;
00257   }
00258   myIRMap[builder->getArgInt(0)][IR_TYPE] = builder->getArgInt(1);
00259   myIRMap[builder->getArgInt(0)][IR_CYCLES] = builder->getArgInt(2);
00260   myIRMap[builder->getArgInt(0)][IR_X] = builder->getArgInt(3);
00261   myIRMap[builder->getArgInt(0)][IR_Y] = builder->getArgInt(4);
00262   return true;
00263 }
00264 
00265 const std::list<ArArgumentBuilder *> *ArRobotParams::getIRUnits(void)
00266 {
00267   std::map<int, std::map<int, int> >::iterator it;
00268   int num, type, cycles,  x, y;
00269   ArArgumentBuilder *builder;
00270 
00271   for (it = myIRMap.begin(); it != myIRMap.end(); it++)
00272   {
00273     num = (*it).first;
00274     type = (*it).second[IR_TYPE];
00275     cycles = (*it).second[IR_CYCLES];
00276     x = (*it).second[IR_X];
00277     y = (*it).second[IR_Y];
00278     builder = new ArArgumentBuilder;
00279     builder->add("%d %d %d %d %d", num, type, cycles, x, y);
00280     myGetIRUnitList.push_back(builder);
00281   }
00282   return &myGetIRUnitList;
00283 }
00284 
00285 void ArRobotParams::internalSetIR(int num, int type, int cycles, int x, int y)
00286 {
00287   myIRMap[num][IR_TYPE] = type;
00288   myIRMap[num][IR_CYCLES] = cycles;
00289   myIRMap[num][IR_X] = x;
00290   myIRMap[num][IR_Y] = y;
00291 }
00292 
00293 bool ArRobotParams::parseSonarUnit(ArArgumentBuilder *builder)
00294 {
00295   if (builder->getArgc() != 4 || !builder->isArgInt(0) || 
00296       !builder->isArgInt(1) || !builder->isArgInt(2) ||
00297       !builder->isArgInt(3))
00298   {
00299     ArLog::log(ArLog::Terse, "ArRobotParams: SonarUnit parameters invalid");
00300     return false;
00301   }
00302   mySonarMap[builder->getArgInt(0)][SONAR_X] = builder->getArgInt(1);
00303   mySonarMap[builder->getArgInt(0)][SONAR_Y] = builder->getArgInt(2);
00304   mySonarMap[builder->getArgInt(0)][SONAR_TH] = builder->getArgInt(3);
00305   return true;
00306 }
00307 
00308 
00309 const std::list<ArArgumentBuilder *> *ArRobotParams::getSonarUnits(void)
00310 {
00311   std::map<int, std::map<int, int> >::iterator it;
00312   int num, x, y, th;
00313   ArArgumentBuilder *builder;
00314 
00315   for (it = mySonarMap.begin(); it != mySonarMap.end(); it++)
00316   {
00317     num = (*it).first;
00318     x = (*it).second[SONAR_X];
00319     y = (*it).second[SONAR_Y];
00320     th = (*it).second[SONAR_TH];
00321     builder = new ArArgumentBuilder;
00322     builder->add("%d %d %d %d", num, x, y, th);
00323     myGetSonarUnitList.push_back(builder);
00324   }
00325   return &myGetSonarUnitList;
00326 }
00327 
00328 void ArRobotParams::internalSetSonar(int num, int x, 
00329                                               int y, int th)
00330 {
00331   mySonarMap[num][SONAR_X] = x;
00332   mySonarMap[num][SONAR_Y] = y;
00333   mySonarMap[num][SONAR_TH] = th;
00334 }
00335 
00336 bool ArRobotParams::save(void)
00337 {
00338   char buf[10000];
00339   sprintf(buf, "%sparams/", Aria::getDirectory());
00340   setBaseDirectory(buf);
00341   sprintf(buf, "%s.p", getSubClassName());
00342   return writeFile(buf, false, NULL, false);
00343 }