00001 #include "Dynamixel.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/MarkScope.h"
00004 #include "Shared/debuget.h"
00005
00006 using namespace std;
00007
00008 const char * DynamixelDriver::protocolNames[] = { "BINARY", "CM-5", "" };
00009 INSTANTIATE_NAMEDENUMERATION_STATICS(DynamixelDriver::protocol_t);
00010
00011 const std::string DynamixelDriver::autoRegisterDynamixelDriver = DeviceDriver::getRegistry().registerType<DynamixelDriver>("Dynamixel");
00012
00013 static char nchecksum(const char* p, size_t len, size_t off=0) {
00014 const char* end=p+len;
00015 p+=off;
00016 char c=0;
00017 while(p!=end) {
00018
00019 c+=*p++;
00020 }
00021 return c;
00022 }
00023
00024 struct GenericCmdHeader {
00025 GenericCmdHeader(unsigned char bytelen, unsigned char instruction)
00026 : markerA(0xFF), markerB(0xFF), servoid(0xFE), cmdlen(bytelen), cmdid(instruction) {}
00027 operator const char*() const { return reinterpret_cast<const char*>(&markerA); }
00028 unsigned char markerA;
00029 unsigned char markerB;
00030 unsigned char servoid;
00031 unsigned char cmdlen;
00032 unsigned char cmdid;
00033 };
00034 static char nchecksum(const struct GenericCmdHeader& p, size_t len) { return nchecksum(p,len,2); }
00035
00036 template<class T>
00037 struct SyncWriteHeader : public GenericCmdHeader {
00038 SyncWriteHeader(unsigned char len)
00039 : GenericCmdHeader(sizeof(T)*len+4,0x83), addr(T::ADDRESS), writelen(sizeof(T)-1) {}
00040 unsigned char addr;
00041 unsigned char writelen;
00042 };
00043 struct WriteHeader : public GenericCmdHeader {
00044 WriteHeader(unsigned char address, unsigned char len)
00045 : GenericCmdHeader(len+3,0x3), addr(address) {}
00046 unsigned char addr;
00047 };
00048 struct SyncWritePosSpeedEntry {
00049 SyncWritePosSpeedEntry() : servoid(), posl(), posh(), speedl(), speedh() {}
00050 SyncWritePosSpeedEntry(unsigned char sid, unsigned short pos, unsigned short speed)
00051 : servoid(sid), posl(pos), posh(pos>>8), speedl(speed), speedh(speed>>8) {}
00052 static const unsigned char ADDRESS=0x1E;
00053 operator const char*() const { return reinterpret_cast<const char*>(&servoid); }
00054 unsigned char servoid;
00055 unsigned char posl;
00056 unsigned char posh;
00057 unsigned char speedl;
00058 unsigned char speedh;
00059 };
00060 struct BroadcastTorqueCmd : public WriteHeader {
00061 BroadcastTorqueCmd(bool enable)
00062 : WriteHeader(0x18,1), torqueEnable(enable?1:0), checksum(0) { updateChecksum(); }
00063 void updateChecksum() { checksum=~nchecksum(*this,sizeof(*this)-1); }
00064 unsigned char torqueEnable;
00065 char checksum;
00066 };
00067
00068 void DynamixelDriver::motionStarting() {
00069 MotionHook::motionStarting();
00070 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00071 if(comm!=NULL) {
00072 if(comm->open()) {
00073 if(!comm->isWriteable()) {
00074 std::cerr << "Dynamixel unable to send initialization" << std::endl;
00075 } else {
00076 MarkScope autolock(*comm);
00077 std::ostream os(&comm->getWriteStreambuf());
00078
00079
00080 os.write(BroadcastTorqueCmd(true),sizeof(BroadcastTorqueCmd)).flush();
00081 }
00082 }
00083 }
00084 motionActive=true;
00085 commName.addPrimitiveListener(this);
00086 }
00087
00088 void DynamixelDriver::motionStopping() {
00089 motionActive=false;
00090 if(!sensorsActive)
00091 commName.removePrimitiveListener(this);
00092 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00093 if(comm!=NULL)
00094 comm->close();
00095 MotionHook::motionStopping();
00096 }
00097
00098 void DynamixelDriver::motionCheck(const float outputs[][NumOutputs]) {
00099 CommPort * comm = CommPort::getRegistry().getInstance(commName);
00100 if(comm==NULL || !comm->isWriteable())
00101 std::cerr << "Bad comm!" << std::endl;
00102 if(comm==NULL || !comm->isWriteable())
00103 return;
00104
00105 SyncWritePosSpeedEntry packets[servos.size()];
00106 unsigned int packetsUsed=0;
00107 for(servo_iterator it=servos.begin(); it!=servos.end(); ++it) {
00108 int idx=it->second->output;
00109 if(idx<0 || static_cast<unsigned int>(idx)>=NumOutputs) {
00110 if(idx!=ServoInfo::UNUSED)
00111 std::cerr << "Warning: Dynamixel driver mapping servo " << it->first << " to invalid output index " << idx << std::endl;
00112 continue;
00113 }
00114 if(isFirstCheck || lastOutputs[idx]!=outputs[NumFrames-1][idx])
00115 setServo(packets[packetsUsed++], it, outputs[NumFrames-1][idx]);
00116 }
00117 if(packetsUsed>0) {
00118 ThreadNS::Lock& l = comm->getLock();
00119 unsigned int t=get_time();
00120
00121 unsigned int dt = static_cast<unsigned int>(NumFrames*FrameTime/((getTimeScale()>0)?getTimeScale():1.f));
00122 unsigned int giveup = t+dt*3/4;
00123 while(!l.trylock()) {
00124 if(get_time()>=giveup) {
00125 if(MotionHook::verbose>0)
00126 cerr << "Dropping dynamixel motion update: couldn't get lock on comm port" << endl;
00127 return;
00128 }
00129 usleep(1000);
00130 }
00131 MarkScope autolock(l); l.unlock();
00132 std::ostream os(&comm->getWriteStreambuf());
00133 SyncWriteHeader<SyncWritePosSpeedEntry> header(packetsUsed);
00134 char checksum=nchecksum(header,sizeof(header));
00135 checksum+=nchecksum(packets[0],packetsUsed*sizeof(SyncWritePosSpeedEntry));
00136 checksum=~checksum;
00137 os.write(header,sizeof(header));
00138 os.write(packets[0],packetsUsed*sizeof(SyncWritePosSpeedEntry));
00139 os.put(checksum);
00140 os.flush();
00141 printf("\nHEADER: ");
00142 debuget::hexout(reinterpret_cast<char*>(&header),sizeof(header));
00143 printf("\nBODY: ");
00144 debuget::hexout(packets[0],packetsUsed*sizeof(SyncWritePosSpeedEntry));
00145 printf("\nCHECKSUM: ");
00146 debuget::hexout(&checksum,1);
00147 printf("\n");
00148 }
00149
00150 MotionHook::motionCheck(outputs);
00151 }
00152
00153 void DynamixelDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00154 if(&pl==&commName) {
00155
00156
00157 CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00158 if(comm!=NULL) {
00159
00160 if(sensorsActive)
00161 comm->close();
00162 if(motionActive)
00163 comm->close();
00164 }
00165 comm = CommPort::getRegistry().getInstance(commName);
00166 if(comm!=NULL) {
00167
00168 if(sensorsActive)
00169 comm->open();
00170 if(motionActive)
00171 comm->open();
00172 }
00173 } else if(&pl==&queryServos) {
00174
00175
00176 if(queryServos) {
00177 for(servo_iterator it=servos.begin(); it!=servos.end(); ++it) {
00178 provideOutput(it->second->output);
00179 it->second->output.addPrimitiveListener(this);
00180 }
00181 } else {
00182 for(servo_iterator it=servos.begin(); it!=servos.end(); ++it) {
00183 it->second->output.removePrimitiveListener(this);
00184 ignoreOutput(it->second->output);
00185 }
00186 }
00187 } else {
00188
00189
00190 for(servo_iterator it=servos.begin(); it!=servos.end(); ++it) {
00191 if(&pl==&it->second->output) {
00192 ignoreOutput(it->second->output.getPreviousValue());
00193 provideOutput(it->second->output);
00194 return;
00195 }
00196 }
00197 std::cerr << "Unhandled value change in " << getClassName() << ": " << pl.get() << std::endl;
00198 }
00199 }
00200
00201 void DynamixelDriver::setServo(SyncWritePosSpeedEntry& packet, const servo_iterator& servo, float v) {
00202 unsigned int servoIdx = atoi(servo->first.c_str());
00203 if(servoIdx>255)
00204 return;
00205 unsigned int outputIdx = servo->second->output;
00206
00207 float outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00208
00209 unsigned int servoRange = 1023;
00210
00211 float cmd = (v-outputRanges[outputIdx][MinRange])/outRange;
00212
00213 float pw = cmd*servoRange;
00214
00215 int bpw = static_cast<int>(pw+0.5);
00216
00217 if(bpw<0)
00218 bpw=0;
00219 if(bpw>1023)
00220 bpw=1023;
00221 packet = SyncWritePosSpeedEntry(servoIdx,bpw,0);
00222 }
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233