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
00027 enum ServoParam_t {
00028 DYNAMIXEL_P = 0,
00029 DYNAMIXEL_I,
00030 DYNAMIXEL_D
00031 };
00032
00033
00034 enum OldServoParam_t {
00035 DYNAMIXEL_SLOPE=0,
00036 DYNAMIXEL_PUNCH,
00037 DYNAMIXEL_MARGIN
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
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(), 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),
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
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
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
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
00163 plist::OutputSelector output;
00164 plist::OutputSelector led;
00165 plist::OutputSelector freeSpinOutput;
00166
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
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;
00197 float sensorActivation;
00198 unsigned int micSpikeFrameNumber;
00199
00200 DynamixelProtocol::ModelID_t model;
00201 plist::Primitive<std::string> modelName;
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
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;
00249 typedef plist::DictionaryOf< ServoInfo >::const_iterator servo_iterator;
00250
00251 plist::Primitive<std::string> commName;
00252 plist::Primitive<float> loadCompensation;
00253 plist::Primitive<unsigned int> commLatency;
00254 plist::Primitive<unsigned int> numPoll;
00255 plist::Primitive<unsigned int> responseTime;
00256
00257 protected:
00258 static unsigned int VOLTAGE_SENSOR_OFFSET;
00259 static unsigned int TEMP_SENSOR_OFFSET;
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& , 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& , 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& ) {
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
00319 float* getWriteBuffer();
00320
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
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;
00344 FailsafeThread failsafe;
00345 bool continuousUpdates;
00346 bool updated;
00347 bool responsePending;
00348 unsigned int lastSensorTime;
00349 float servoDeflection;
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
00362 inline bool calcLEDValue(unsigned int i,float x) {
00363 if(x<=0.0) {
00364 ledActivation[i]*=.9f;
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;
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];
00381 enum LedState { LED_OFF=0, LED_ON, LED_UNKNOWN } lastLEDState[NumLEDs];
00382
00383 private:
00384 CommThread(const CommThread&);
00385 CommThread& operator=(const CommThread&);
00386 } commThread;
00387
00388 void doFreeze();
00389 void doUnfreeze();
00390
00391
00392 static void provideValues(const ServoInfo& info, const DynamixelProtocol::ServoSensorsResponse& response);
00393
00394 static void provideValues(ServoInfo& info, const DynamixelProtocol::AXS1SensorsResponse& response);
00395
00396
00397 void provideOutput(unsigned int idx) { if(idx<NumOutputs) providingOutput(idx); }
00398
00399 void ignoreOutput(unsigned int idx) { if(idx<NumOutputs) ignoringOutput(idx); }
00400
00401
00402 void pingServos(bool detectedOnly=false);
00403
00404
00405 void sendZeroTorqueCmd(CommPort& comm);
00406
00407
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
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
00432
00433
00434 failsafe.progressFlag=true;
00435 testCancel();
00436 }
00437 } while(response.servoid!=module.servoID);
00438
00439 module.failures=0;
00440 module.sensorActivation=0;
00441
00442
00443
00444
00445 provideValues(module, response);
00446 }
00447
00448
00449
00450
00451
00452
00453
00454 #endif