00001
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
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;
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
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(), 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),
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
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
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
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
00149 plist::OutputSelector output;
00150 plist::OutputSelector led;
00151 plist::OutputSelector freeSpinOutput;
00152
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
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;
00183 float sensorActivation;
00184 unsigned int micSpikeFrameNumber;
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
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;
00229 };
00230
00231 plist::DictionaryOf< ServoInfo > servos;
00232 typedef plist::DictionaryOf< ServoInfo >::const_iterator servo_iterator;
00233
00234 plist::Primitive<std::string> commName;
00235 plist::Primitive<float> loadCompensation;
00236 plist::Primitive<unsigned int> commLatency;
00237 plist::Primitive<unsigned int> numPoll;
00238 plist::Primitive<unsigned int> responseTime;
00239
00240 protected:
00241 static unsigned int VOLTAGE_SENSOR_OFFSET;
00242 static unsigned int TEMP_SENSOR_OFFSET;
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& , 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& , 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& ) {
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
00302 float* getWriteBuffer();
00303
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
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;
00327 FailsafeThread failsafe;
00328 bool continuousUpdates;
00329 bool updated;
00330 bool responsePending;
00331 unsigned int lastSensorTime;
00332 float servoDeflection;
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
00345 inline bool calcLEDValue(unsigned int i,float x) {
00346 if(x<=0.0) {
00347 ledActivation[i]*=.9f;
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;
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];
00364 enum LedState { LED_OFF=0, LED_ON, LED_UNKNOWN } lastLEDState[NumLEDs];
00365
00366 private:
00367 CommThread(const CommThread&);
00368 CommThread& operator=(const CommThread&);
00369 } commThread;
00370
00371 void doFreeze();
00372 void doUnfreeze();
00373
00374
00375 static void provideValues(const ServoInfo& info, const DynamixelProtocol::ServoSensorsResponse& response);
00376
00377 static void provideValues(ServoInfo& info, const DynamixelProtocol::AXS1SensorsResponse& response);
00378
00379
00380 void provideOutput(unsigned int idx) { if(idx<NumOutputs) providingOutput(idx); }
00381
00382 void ignoreOutput(unsigned int idx) { if(idx<NumOutputs) ignoringOutput(idx); }
00383
00384
00385 void pingServos(bool detectedOnly=false);
00386
00387
00388 void sendZeroTorqueCmd(CommPort& comm);
00389
00390
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
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
00415
00416
00417 failsafe.progressFlag=true;
00418 testCancel();
00419 }
00420 } while(response.servoid!=module.servoID);
00421
00422 module.failures=0;
00423 module.sensorActivation=0;
00424
00425
00426
00427
00428 provideValues(module, response);
00429 }
00430
00431
00432
00433
00434
00435
00436
00437 #endif