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

Tekkotsu Hardware Abstraction Layer 5.1CVS
Generated Mon May 9 05:01:38 2016 by Doxygen 1.6.3