Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Dynamixel.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_Dynamixel_h_
00003 #define INCLUDED_Dynamixel_h_
00004 
00005 #include "local/DeviceDriver.h"
00006 #include "local/MotionHook.h"
00007 #include "local/DataSource.h"
00008 #include "local/CommPort.h"
00009 #include "Shared/plist.h"
00010 #include "Shared/plistSpecialty.h"
00011 #include "Shared/get_time.h"
00012 #include "IPC/Thread.h"
00013 #include "IPC/FailsafeThread.h"
00014 #include "IPC/DriverMessaging.h"
00015 #include "DynamixelProtocol.h"
00016 #include <iostream>
00017 #include <sstream>
00018 #include <iomanip>
00019 
00020 //! description of Dynamixel
00021 class DynamixelDriver : public virtual DeviceDriver, public virtual DriverMessaging::Listener, public MotionHook, public DataSource, public virtual plist::PrimitiveListener {
00022 public:
00023   static const int START_SERVO_ID = 1; //!< bioloid kits start the id count at 1
00024   static const unsigned int UNUSED = plist::OutputSelector::UNUSED;
00025   
00026   explicit DynamixelDriver(const std::string& name)
00027     : DeviceDriver(autoRegisterDynamixelDriver,name), DriverMessaging::Listener(), MotionHook(), DataSource(),
00028     servos(), commName(), loadCompensation(28.f), commLatency(16), numPoll(3), responseTime(4000), servoIDSync(), 
00029     commThread(servos,commName,*this), motionActive(false), sensorsActive(false), lastSensor()
00030   {
00031     // redo these lookups, they aren't set reliably otherwise due to static initialization order issues
00032     VOLTAGE_SENSOR_OFFSET = capabilities.findSensorOffset("PowerVoltage");
00033     TEMP_SENSOR_OFFSET = capabilities.findSensorOffset("PowerThermo");
00034     
00035     addEntry("Servos",servos,"Maps servo IDs to Tekkotsu output offsets, use command line new/delete commands to add/remove mappings.");
00036     addEntry("CommPort",commName,"Name of the CommPort to use, generally a SerialCommPort with direct binary TTL communication with the servos");
00037     addEntry("LoadCompensation",loadCompensation,"Ratio of load to deflection, if non-zero will use LoadPrediction messages and attempt to counter anticipated loads");
00038     addEntry("CommLatency",commLatency,"The delay (in MILLIseconds, ms) to get a response from the servos due to buffering in the USB-to-TTL interface.");
00039     addEntry("NumPoll",numPoll,"Number of sensor queries to send before reading responses.  This lets us front-load the queries so we can get multiple responses per read buffer flush period");
00040     addEntry("ResponseTime",responseTime,"The amount of time (in MICROseconds, µs) to wait between sending a sensor query and sending another when front-loading the buffer.\nIf this is too short, the next query will collide with the previous response.");
00041     
00042     for(unsigned int i=0; i<NumPIDJoints; ++i) {
00043       std::stringstream bio_id;
00044       bio_id << std::setw(3) << std::setfill('0') << (i+START_SERVO_ID);
00045       servos[bio_id.str()]=ServoInfo(i+START_SERVO_ID);
00046     }
00047     servos.addCollectionListener(&servoIDSync);
00048     DriverMessaging::addListener(this,DriverMessaging::LoadPrediction::NAME);
00049     DriverMessaging::addListener(this,DriverMessaging::SensorPriority::NAME);
00050   }
00051   virtual ~DynamixelDriver() {
00052     servos.removeCollectionListener(&servoIDSync);
00053     DriverMessaging::removeListener(this,DriverMessaging::SensorPriority::NAME);
00054     DriverMessaging::removeListener(this,DriverMessaging::LoadPrediction::NAME);
00055   }
00056   
00057   virtual std::string getClassName() const { return autoRegisterDynamixelDriver; }
00058   
00059   virtual MotionHook* getMotionSink() { return dynamic_cast<MotionHook*>(this); }
00060   virtual void getSensorSources(std::map<std::string,DataSource*>& sources) {
00061     sources.clear();
00062     sources["Sensors"]=dynamic_cast<DataSource*>(this);
00063   }
00064   
00065   virtual void motionStarting();
00066   virtual bool isConnected();
00067   virtual void motionStopping();
00068   virtual void motionCheck(const float outputs[][NumOutputs]);
00069   virtual void updatePIDs(const std::vector<MotionHook::PIDUpdate>& pids);
00070   virtual void registerSource();
00071   virtual void deregisterSource();
00072   virtual void enteringRealtime(const plist::Primitive<double>& simTimeScale) { DataSource::enteringRealtime(simTimeScale); }
00073   virtual void leavingRealtime(bool isFullSpeed) { DataSource::leavingRealtime(isFullSpeed); }
00074   
00075   virtual unsigned int nextTimestamp();
00076   virtual const std::string& nextName() { return instanceName; }
00077   virtual bool advance();
00078   
00079   virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00080 
00081   virtual void processDriverMessage(const DriverMessaging::Message& d);
00082   
00083   struct ServoInfo : public virtual plist::Dictionary {
00084     ServoInfo(unsigned int i=DynamixelProtocol::INVALID_ID)
00085       : plist::Dictionary(false), detected(false), output(), led(), freeSpinOutput(), /*maxSpeed(0),*/ invertRotation(false), zeroAngle(0), maxTic(1023), maxAngle(300*(float)M_PI/180),
00086       cmdSpeedSlopeP(0.6575f), cmdSpeedOffsetP(0.1557f), cmdSpeedSlopeN(0.6554f), cmdSpeedOffsetN(-0.1256f),
00087       repSpeedSlope(0.111f / 60 * 2 * (float)M_PI), // from Dynamixel documentation, rpm = reported speed * 0.111 (114rpm ≈ 1023) ... rpm / value / 60 seconds per minute * 2π radians = 0.011623892818282 rad/s per value
00088       leftIRDistOffset(), centerIRDistOffset(), rightIRDistOffset(),
00089       leftLuminosityOffset(), centerLuminosityOffset(), rightLuminosityOffset(), 
00090       micVolumeOffset(), micSpikeCountOffset(), 
00091       servoID(i), slope(0), punch(0), margin(0), curRotationMode(UNKNOWN), lastCmd(), recentCmdMotion(0), failures(0), 
00092       predictedLoad(0), sensorPriority(-1), sensorActivation(1), micSpikeFrameNumber(-1U), model(DynamixelProtocol::MODEL_UNKNOWN), modelName()
00093     {
00094       i-=START_SERVO_ID;
00095 #ifdef TGT_HAS_WHEELS
00096       if(WheelOffset<PIDJointOffset || WheelOffset>=PIDJointOffset+NumPIDJoints) {
00097         // wheels are not a subset of PIDJoints, so we'll default map the first NumWheel servos to wheels, and the rest to PIDJoints
00098         if(i<(int)NumWheels) {
00099           output = ( i+WheelOffset );
00100           freeSpinOutput = ( i+WheelOffset );
00101         } else {
00102           output = ( (i-NumWheels<(int)NumPIDJoints) ? i-NumWheels+PIDJointOffset : UNUSED );
00103         }
00104       } else {
00105         // wheels are a subset of PIDJoints, don't subtract NumWheels...
00106         output = ( (i<NumPIDJoints) ? i+PIDJointOffset : UNUSED);
00107         if(i>=(int)WheelOffset && i<(int)(WheelOffset+NumWheels)) {
00108           freeSpinOutput = ( i+PIDJointOffset );
00109         }
00110       }
00111 #else
00112       output = ( (i<NumPIDJoints) ? i+PIDJointOffset : UNUSED);
00113 #endif
00114 #ifdef TGT_HAS_LEDS
00115       led = (i<NumLEDs ? i+LEDOffset : UNUSED);
00116 #endif
00117       
00118       plist::NamedEnumeration<SensorOffset_t>::setNames(sensorNames);
00119       plist::NamedEnumeration<SensorOffset_t>::addNameForVal("UNUSED",static_cast<SensorOffset_t>(-1));
00120       leftIRDistOffset.set(capabilities.findSensorOffset("LeftIRDist"));
00121       centerIRDistOffset.set(capabilities.findSensorOffset("CenterIRDist"));
00122       rightIRDistOffset.set(capabilities.findSensorOffset("RightIRDist"));
00123       leftLuminosityOffset.set(capabilities.findSensorOffset("LeftLuminosity"));
00124       centerLuminosityOffset.set(capabilities.findSensorOffset("CenterLuminosity"));
00125       rightLuminosityOffset.set(capabilities.findSensorOffset("RightLuminosity"));
00126       micVolumeOffset.set(capabilities.findSensorOffset("MicVolume"));
00127       micSpikeCountOffset.set(capabilities.findSensorOffset("MicSpikeCount"));
00128       
00129       setLoadSavePolicy(FIXED,SYNC);
00130       initEntries();
00131     }
00132     
00133     void setModel(DynamixelProtocol::ModelID_t m) { if(model!=m) { model=m; initEntries(); } }
00134     DynamixelProtocol::ModelID_t getModel() const { return model; }
00135     void setModelName(const std::string m) { modelName=m; }
00136     const std::string& getModelName() const { return modelName; }
00137     
00138     //! returns true if any of the AX-S1 sensor offsets are in use
00139     bool hasSensorOffset() const {
00140       const SensorOffset_t US = static_cast<SensorOffset_t>(-1);
00141       return leftIRDistOffset!=US || centerIRDistOffset!=US || rightIRDistOffset!=US
00142         || leftLuminosityOffset!=US || centerLuminosityOffset!=US || rightLuminosityOffset!=US
00143         || micVolumeOffset!=US || micSpikeCountOffset!=US;
00144     }
00145     
00146     plist::Primitive<bool> detected;
00147     
00148     // Servo stuff
00149     plist::OutputSelector output;
00150     plist::OutputSelector led;
00151     plist::OutputSelector freeSpinOutput;
00152     //plist::Primitive<float> maxSpeed;
00153     plist::Primitive<bool> invertRotation;
00154     plist::Angle zeroAngle;
00155     plist::Primitive<unsigned int> maxTic;
00156     plist::Angle maxAngle;
00157     plist::Primitive<float> cmdSpeedSlopeP;
00158     plist::Primitive<float> cmdSpeedOffsetP;
00159     plist::Primitive<float> cmdSpeedSlopeN;
00160     plist::Primitive<float> cmdSpeedOffsetN;
00161     plist::Primitive<float> repSpeedSlope;
00162     
00163     // Sensor (AX-S1) stuff
00164     plist::NamedEnumeration<SensorOffset_t> leftIRDistOffset;
00165     plist::NamedEnumeration<SensorOffset_t> centerIRDistOffset;
00166     plist::NamedEnumeration<SensorOffset_t> rightIRDistOffset;
00167     plist::NamedEnumeration<SensorOffset_t> leftLuminosityOffset;
00168     plist::NamedEnumeration<SensorOffset_t> centerLuminosityOffset;
00169     plist::NamedEnumeration<SensorOffset_t> rightLuminosityOffset;
00170     plist::NamedEnumeration<SensorOffset_t> micVolumeOffset;
00171     plist::NamedEnumeration<SensorOffset_t> micSpikeCountOffset;
00172     
00173     unsigned int servoID;
00174     float slope;
00175     float punch;
00176     float margin;
00177     enum RotationMode { POSITION, CONTINUOUS, UNKNOWN } curRotationMode;
00178     unsigned short lastCmd;
00179     float recentCmdMotion;
00180     unsigned short failures;
00181     float predictedLoad;
00182     float sensorPriority; //!< amount to add to sensor activation each sensor check, negative values indicate "auto" mode based on movement speed with decay
00183     float sensorActivation; //!< the accumulated "energy" for polling this servo, the servos with highest activation are polled as avaiable
00184     unsigned int micSpikeFrameNumber; //!< stores the frame number when the micSpike occurred, so we can tell when the system has received it
00185     
00186   protected:
00187     void initEntries() {
00188       using namespace DynamixelProtocol;
00189       clear();
00190       
00191       addEntry("Detected",detected,"If true, servo is detected and functioning.  If a servo is reconnected, set back to true to attempt to re-enable communication.");
00192       
00193       if(model!=MODEL_AXS1 || model==MODEL_UNKNOWN) {
00194         addEntry("Output",output,"Tekkotsu offset to pull servo positions from; -1 or empty string to mark unused\n"
00195                  "(May still rotate via FreeSpinOutput if position control by Output is unused...)");
00196         addEntry("LED",led,"Tekkotsu offset to pull LED values from; -1 or empty string to mark unused");
00197         addEntry("FreeSpinOutput",freeSpinOutput,"Tekkotsu output to control the 'continuous rotation mode'. If specified output's\n"
00198                  "value is non-zero, the value from Output is ignored, and the servo is rotated at the\n"
00199                  "specified speed.  If Output and FreeSpinOutput are equal, then the servo is permanently\n"
00200                  "in continuous rotation mode.");
00201         //addEntry("MaxSpeed",maxSpeed,"Indicates maximum joint speed in rad/s, 0 for no maximum");
00202         addEntry("InvertRotation",invertRotation,"If true, the servo moves in the reverse direction.  Calibration parameters are always applied relative to the 'native' orientation.");
00203         addEntry("ZeroAngle",zeroAngle,"Specifies the angle to which the zero point of the output will be mapped.\n"
00204                  "For an AX or RX servo, this must be in the range ±2.618 radians (i.e. ±150°), such that\n"
00205                  "a value of zero is centered in the range of motion.");
00206         addEntry("MaxTic",maxTic,"The maximum servo tic value (e.g. 1023 for AX and RX, 4095 for EX-106)");
00207         addEntry("MaxAngle",maxAngle,"The maximum servo angle (e.g. 300° for AX and RX, 280.6° for EX-106)");
00208         addEntry("CmdSpeedSlopePos",cmdSpeedSlopeP,"Calibration parameter for converting desired physical positive speeds to speed command for the servo.");
00209         addEntry("CmdSpeedOffsetPos",cmdSpeedOffsetP,"Calibration parameter for converting desired physical positive speeds to speed command for the servo.");
00210         addEntry("CmdSpeedSlopeNeg",cmdSpeedSlopeN,"Calibration parameter for converting desired physical negative speeds to speed command for the servo.");
00211         addEntry("CmdSpeedOffsetNeg",cmdSpeedOffsetN,"Calibration parameter for converting desired physical negative speeds to speed command for the servo.");
00212         addEntry("ReportedSpeedSlope",repSpeedSlope,"Calibration parameter for converting speed feedback from servo to actual physical speed.");
00213       }
00214       
00215       if(model==MODEL_AXS1 || model==MODEL_UNKNOWN) {
00216         addEntry("LeftIRDistOffset",leftIRDistOffset,"Tekkotsu sensor offset where the left IR distance reading should be stored.");
00217         addEntry("CenterIRDistOffset",centerIRDistOffset,"Tekkotsu sensor offset where the center IR distance reading should be stored.");
00218         addEntry("RightIRDistOffset",rightIRDistOffset,"Tekkotsu sensor offset where the right IR distance reading should be stored.");
00219         addEntry("LeftLuminosityOffset",leftLuminosityOffset,"Tekkotsu sensor offset where the left luminosity reading should be stored.");
00220         addEntry("CenterLuminosityOffset",centerLuminosityOffset,"Tekkotsu sensor offset where the center luminosity reading should be stored.");
00221         addEntry("RightLuminosityOffset",rightLuminosityOffset,"Tekkotsu sensor offset where the right luminosity reading should be stored.");
00222         addEntry("MicVolumeOffset",micVolumeOffset,"Tekkotsu sensor offset where the volume magnitude reading should be stored.");
00223         addEntry("MicSpikeCountOffset",micSpikeCountOffset,"Tekkotsu sensor offset where the volume spike count should be stored.");
00224       }
00225     }
00226     
00227     DynamixelProtocol::ModelID_t model;
00228     std::string modelName; // assigned during pingServos to string name so we can include numeric value of unknown servo model IDs
00229   };
00230   
00231   plist::DictionaryOf< ServoInfo > servos; //!< Maps servo IDs to Tekkotsu output offsets, use command line new/delete commands to add/remove mappings.
00232   typedef plist::DictionaryOf< ServoInfo >::const_iterator servo_iterator;
00233   
00234   plist::Primitive<std::string> commName; //!< Name of the CommPort to use, generally a SerialCommPort with direct binary TTL communication with the servos
00235   plist::Primitive<float> loadCompensation; //!< Ratio of load to deflection, if non-zero will use LoadPrediction messages and attempt to counter anticipated loads
00236   plist::Primitive<unsigned int> commLatency; //!< The delay (in MILLIseconds, ms) to get a response from the servos due to buffering in the USB-to-TTL interface.
00237   plist::Primitive<unsigned int> numPoll; //!< Number of sensor queries to send before reading responses.  This lets us front-load the queries so we can get multiple responses per read buffer flush period
00238   plist::Primitive<unsigned int> responseTime; //!< The amount of time (in MICROseconds, µs) to wait between sending a sensor query and sending another when front-loading the buffer.  If this is too short, the next query will collide with the previous response.
00239   
00240 protected:  
00241   static unsigned int VOLTAGE_SENSOR_OFFSET; //!< index of the voltage sensor
00242   static unsigned int TEMP_SENSOR_OFFSET; //!< index of the temperature sensor
00243   
00244   struct ServoInfoIDSync : plist::CollectionListener {
00245     virtual void plistCollectionEntryAdded(plist::Collection& col, ObjectBase&) { plistCollectionEntriesChanged(col); }
00246     virtual void plistCollectionEntriesChanged(plist::Collection& col) {
00247       plist::DictionaryOf< ServoInfo >& s = dynamic_cast<plist::DictionaryOf<ServoInfo>& > (col);
00248       for(servo_iterator it=s.begin(); it!=s.end(); ++it)
00249         it->second->servoID = atoi(it->first.c_str());
00250     }
00251   } servoIDSync;
00252   
00253   class CommThread : public Thread, public plist::CollectionListener {
00254   public:
00255     CommThread(plist::DictionaryOf< ServoInfo >& servoList, const plist::Primitive<std::string>& comm, DynamixelDriver& parent)
00256       : Thread(), pidLock(), dirtyPIDs(0), commName(comm), servos(servoList), driver(parent), servoPollQueue(),
00257       failsafe(*this,FrameTime*NumFrames*3/2000.0,false), continuousUpdates(), updated(false), responsePending(false), lastSensorTime(0), 
00258       servoDeflection(0), isFirstCheck(true), timestampBufA(0), timestampBufB(0), timestampBufC(0), curBuf(NULL)
00259     {
00260       failsafe.restartFlag=true;
00261       for(unsigned int i=0; i<NumLEDs; ++i)
00262         lastLEDState[i]=LED_UNKNOWN;
00263       servos.addCollectionListener(this);
00264       plistCollectionEntriesChanged(servos);
00265     }
00266     ~CommThread() { servos.removeCollectionListener(this); }
00267     
00268     virtual void plistCollectionEntryAdded(Collection& /*col*/, ObjectBase& primitive) {
00269       if(ServoInfo* si = dynamic_cast<ServoInfo*>(&primitive)) {
00270         servoPollQueue.push_back(si);
00271         si->detected=true;
00272         if(isStarted())
00273           driver.pingServos(true);
00274       }
00275     }
00276     virtual void plistCollectionEntryRemoved(Collection& /*col*/, ObjectBase& primitive) {
00277       ServoInfo* si = dynamic_cast<ServoInfo*>(&primitive);
00278       std::vector<ServoInfo*>::iterator it = std::find(servoPollQueue.begin(),servoPollQueue.end(),si);
00279       if(it!=servoPollQueue.end())
00280         servoPollQueue.erase(it);
00281     }
00282     virtual void plistCollectionEntriesChanged(Collection& /*col*/) {
00283       servoPollQueue.resize(servos.size());
00284       unsigned int i=0;
00285       for(plist::DictionaryOf< ServoInfo >::const_iterator it=servos.begin(); it!=servos.end(); ++it) {
00286         servoPollQueue[i++] = it->second;
00287         if(isStarted())
00288           it->second->detected=true;
00289       }
00290       if(isStarted())
00291         driver.pingServos(true);
00292     }
00293     
00294     virtual void start() { updated=false; continuousUpdates=true; Thread::start(); }
00295     virtual void startOneUpdate() { updated=false; continuousUpdates=false; Thread::start(); }
00296     virtual Thread& stop();
00297     virtual void waitForUpdate();
00298     virtual bool isStarted() const { return Thread::isStarted() || failsafe.isStarted(); }
00299     bool takeUpdate() { if(!updated) { return false; } else { updated=false; return true; } }
00300     
00301     //! find the oldest of the output buffers that isn't currently in use
00302     float* getWriteBuffer();
00303     //! sets the timestamp on the indicated buffer (indicates you're done writing)
00304     void setWriteBufferTimestamp(float * buf);
00305     
00306     unsigned int nextTimestamp() const { return lastSensorTime + driver.commLatency; }
00307     
00308     Thread::Lock pidLock;
00309     PIDUpdate pidValues[NumOutputs];
00310     unsigned int dirtyPIDs;
00311     
00312   protected:
00313     virtual bool launched();
00314     virtual void cancelled();
00315     void clearBuffer();
00316     virtual unsigned int runloop();
00317     template<class T> void readResponse(T& response, std::istream& is, ServoInfo& module);
00318     virtual void updateCommands(std::istream& is, std::ostream& os);
00319     
00320     //! converts the value @a v from radians into the specified servo's range, with expected speed (rad/s)
00321     DynamixelProtocol::SyncWritePosSpeedEntry setServo(const servo_iterator& servo, ServoInfo::RotationMode rm, float v, float speed);
00322     
00323     const plist::Primitive<std::string>& commName;
00324     plist::DictionaryOf< ServoInfo >& servos;
00325     DynamixelDriver& driver;
00326     std::vector<ServoInfo*> servoPollQueue; //!< priority queue of servos waiting to be polled for sensor information
00327     FailsafeThread failsafe;
00328     bool continuousUpdates;
00329     bool updated;
00330     bool responsePending;
00331     unsigned int lastSensorTime;
00332     float servoDeflection; //!< 'tics' of servo deflection from target per newton·meter of torque applied
00333     
00334     bool isFirstCheck;
00335     float outputBufA[NumOutputs];
00336     unsigned int timestampBufA;
00337     float outputBufB[NumOutputs];
00338     unsigned int timestampBufB;
00339     float outputBufC[NumOutputs];
00340     unsigned int timestampBufC;
00341     float lastOutputs[NumOutputs];
00342     float * curBuf;
00343     
00344     //! allows LEDs to flicker at various frequencies to emulate having linear brightness control instead of boolean control
00345     inline bool calcLEDValue(unsigned int i,float x) {
00346       if(x<=0.0) {
00347         ledActivation[i]*=.9f; //decay activation... resets to keeps LEDs in sync, looks a little better
00348         return false;
00349       } else if(x>=1.0) {
00350         return true;
00351       } else {
00352         float x3 = x*x*x;
00353         x = .25f*x3*x3 + .75f*x; // fancy but unscientific equation to "gamma correct" - we can see a single pulse better than a single flicker - after image and all that
00354         ledActivation[i]+=x;
00355         if(ledActivation[i]>=1) {
00356           ledActivation[i]-=1;
00357           return true;
00358         } else {
00359           return false;
00360         }
00361       }
00362     }
00363     float ledActivation[NumLEDs]; //!< used to track partial LED activation (see calcLEDValue())
00364     enum LedState { LED_OFF=0, LED_ON, LED_UNKNOWN } lastLEDState[NumLEDs]; //!< keeps track of the last full activation value sent to servo
00365     
00366   private:
00367     CommThread(const CommThread&); //!< don't copy
00368     CommThread& operator=(const CommThread&); //!< don't assign
00369   } commThread;
00370   
00371   void doFreeze();
00372   void doUnfreeze();
00373   
00374   //! sends servo feedback values into the framework
00375   static void provideValues(const ServoInfo& info, const DynamixelProtocol::ServoSensorsResponse& response);
00376   //! sends AX-S1 sensor values into the framework
00377   static void provideValues(ServoInfo& info, const DynamixelProtocol::AXS1SensorsResponse& response);
00378   
00379   //! forwards call to DataSource::providingOutput() if the index is valid
00380   void provideOutput(unsigned int idx) { if(idx<NumOutputs) providingOutput(idx); }
00381   //! forwards call to DataSource::ignoringOutput() if the index is valid
00382   void ignoreOutput(unsigned int idx) { if(idx<NumOutputs) ignoringOutput(idx); }
00383   
00384   //! tests each servo to see if it is connected
00385   void pingServos(bool detectedOnly=false);
00386   
00387   //! broadcasts a "relax" command to all servos
00388   void sendZeroTorqueCmd(CommPort& comm);
00389   
00390   //! sends a series of sync write entries into a stream (adding appropriate header and checksum)
00391   template<class T> static void writeSyncEntries(std::ostream& os, const std::vector<T>& entries) {
00392     if(entries.size()==0)
00393       return;
00394     unsigned char checksum=0;
00395     DynamixelProtocol::write(os, DynamixelProtocol::SyncWriteHeader<T>(entries.size()), checksum);
00396     os.write(entries[0],entries.size()*sizeof(T));
00397     checksum+=DynamixelProtocol::nchecksum(entries[0],entries.size()*sizeof(T));
00398     os.put(~checksum);
00399   }
00400   
00401   bool motionActive;
00402   bool sensorsActive;
00403   std::string lastSensor;
00404 
00405 private:
00406   //! holds the class name, set via registration with the DeviceDriver registry
00407   static const std::string autoRegisterDynamixelDriver;
00408 };
00409 
00410 template<class T> void DynamixelDriver::CommThread::readResponse(T& response, std::istream& is, ServoInfo& module) {
00411   do {
00412     testCancel();
00413     while(!DynamixelProtocol::readResponse(is,response,module.output)) {
00414       /*std::cerr << "DynamixelDriver got an invalid response from " << (int)response.servoid << " to sensor query of " << module.servoID << ", re-reading." << std::endl;
00415        if(response.servoid!=module.servoID)
00416          std::cerr << "DynamixelDriver got an unexpected response from " << (int)response.servoid << " to sensor query of " << module.servoID << ", re-reading." << std::endl;*/
00417       failsafe.progressFlag=true;
00418       testCancel();
00419     }
00420   } while(response.servoid!=module.servoID);
00421     
00422   module.failures=0;
00423   module.sensorActivation=0;
00424   // LOW LEVEL LOGGING:
00425   //cout << get_time() << '\t' << response.getPosition() << '\t' << response.getLoad() << '\t' << servoPollQueue[i]->lastCmd << endl;
00426   //cout << servoPollQueue[i]->servoID << ": " << get_time() << /*" = " <<  TimeET() <<*/ ' ' << validate(response) << ' ' << response.getPosition() << ' ' << response.getSpeed() << ' ' << response.getLoad() << ' ' << (int)response.voltage << ' ' << (int)response.temp << endl;
00427   
00428   provideValues(module, response);
00429 }
00430 
00431 
00432 /*! @file
00433  * @brief 
00434  * @author Ethan Tira-Thompson (ejt) (Creator)
00435  */
00436 
00437 #endif

Tekkotsu Hardware Abstraction Layer 5.1CVS
Generated Tue Jan 31 04:35:31 2012 by Doxygen 1.6.3