Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

Dynamixel.cc

Go to the documentation of this file.
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     //debuget::charhexout(*p); cout << ' '; debuget::charhexout(c+*p); cout << endl;
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         //printf("\nHEADER: ");
00079         //debuget::hexout(BroadcastTorqueCmd(true),sizeof(BroadcastTorqueCmd));
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) // listener count is not recursive, so only remove if we're the last one
00091     commName.removePrimitiveListener(this);
00092   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00093   if(comm!=NULL)
00094     comm->close(); // this *is* recursive, so we always close it to match our open() in motionStarting()
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; // invalid/unused servo
00113     }
00114     if(isFirstCheck || lastOutputs[idx]!=outputs[NumFrames-1][idx])
00115       setServo(packets[packetsUsed++], it, outputs[NumFrames-1][idx]);
00116   }
00117   if(packetsUsed>0) { // if no changes, skip update altogether
00118     ThreadNS::Lock& l = comm->getLock();
00119     unsigned int t=get_time();
00120     // keep trying to get the lock, sleeping 1 ms each time, until 3/4 the frame time is gone (then give up)
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(); //transfer lock to MarkScope
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); // updates lastOutputs and isFirstCheck, we ignore its motionUpdated() call
00151 }
00152 
00153 void DynamixelDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00154   if(&pl==&commName) {
00155     // if here, then motionStarted or setDataSourceThread has been called, thus when commName changes,
00156     // need to close old one and reopen new one
00157     CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00158     if(comm!=NULL) {
00159       // close each of our old references
00160       if(sensorsActive)
00161         comm->close();
00162       if(motionActive)
00163         comm->close();
00164     }
00165     comm = CommPort::getRegistry().getInstance(commName);
00166     if(comm!=NULL) {
00167       // open each of our new references
00168       if(sensorsActive)
00169         comm->open();
00170       if(motionActive)
00171         comm->open();
00172     }
00173   } else if(&pl==&queryServos) {
00174     // if here, LoadDataThread has been assigned, need to update providing/ignoring outputs
00175     // (and maintain listeners for individual servos while providing)
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     // check if it's one of the individual servos... if it is, means we're providing servo feedback,
00189     // need to call providingOutput/ignoringOutput as appropriate
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; // found it, DON'T fall through to error message below...
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   // get output's range in radians
00207   float outRange = outputRanges[outputIdx][MaxRange]-outputRanges[outputIdx][MinRange];
00208   // get servo's range in pulse width (ms)
00209   unsigned int servoRange = 1023;
00210   // get commanded position as percent of range of motion
00211   float cmd = (v-outputRanges[outputIdx][MinRange])/outRange;
00212   // do conversion from radians (output domain) to pulse width (servo domain)
00213   float pw = cmd*servoRange;
00214   // round to int
00215   int bpw = static_cast<int>(pw+0.5);
00216   // check bounds
00217   if(bpw<0)
00218     bpw=0;
00219   if(bpw>1023)
00220     bpw=1023;
00221   packet = SyncWritePosSpeedEntry(servoIdx,bpw,0);
00222 }
00223 
00224 /*! @file
00225  * @brief 
00226  * @author Ethan Tira-Thompson (ejt) (Creator)
00227  *
00228  * $Author: ejt $
00229  * $Name: tekkotsu-4_0 $
00230  * $Revision: 1.2 $
00231  * $State: Exp $
00232  * $Date: 2007/11/05 00:20:11 $
00233  */

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4