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 <iostream>
00011 #include <sstream>
00012
00013
00014 class DynamixelDriver : public virtual DeviceDriver, public MotionHook, public virtual plist::PrimitiveListener {
00015 public:
00016 explicit DynamixelDriver(const std::string& name)
00017 : DeviceDriver(autoRegisterDynamixelDriver,name),
00018 protocol(CM5,protocolNames), servos(), commName(), queryServos(false),
00019 motionActive(false), sensorsActive(false), lastSensor(), frameNumber(0)
00020 {
00021 addEntry("Protocol",protocol,"Indicates whether to send CM-5 controller commands or direct binary commands\n"
00022 "(latter can only be used if you have a direct connection to the Dynamixel TTL bus)\n"+protocol.getDescription());
00023 addEntry("Servos",servos,"Maps servo IDs to Tekkotsu output offsets, use command line new/delete commands to add/remove mappings.");
00024 addEntry("CommPort",commName,"The name of the comm port where output will be sent");
00025 addEntry("QueryServos",queryServos,"If set to true, will attempt to query the servo positions with each sensor update.\nThis may decrease the sampling frequency");
00026 for(unsigned int i=0; i<NumPIDJoints; ++i) {
00027 std::stringstream bio_id; bio_id<<i+1;
00028 servos[bio_id.str()]=ServoInfo(i);
00029 }
00030 }
00031 virtual ~DynamixelDriver() {}
00032
00033 virtual std::string getClassName() const { return autoRegisterDynamixelDriver; }
00034
00035 virtual MotionHook* getMotionSink() { return dynamic_cast<MotionHook*>(this); }
00036 virtual void getSensorSources(std::map<std::string,DataSource*>& sources) {
00037 sources.clear();
00038 sources["Sensors"]=dynamic_cast<DataSource*>(this);
00039 }
00040
00041 virtual void motionStarting();
00042 virtual void motionStopping();
00043 virtual void motionCheck(const float outputs[][NumOutputs]);
00044
00045
00046
00047
00048
00049
00050 virtual void plistValueChanged(const plist::PrimitiveBase& pl);
00051
00052 static const char * protocolNames[];
00053 enum protocol_t {
00054 BINARY,
00055 CM5
00056 };
00057 plist::NamedEnumeration<protocol_t> protocol;
00058
00059 class ServoInfo : public virtual plist::Dictionary {
00060 public:
00061 static const int UNUSED=-1;
00062 ServoInfo(int i=UNUSED)
00063 : plist::Dictionary(false), output(i<(int)NumPIDJoints?i:UNUSED), led(i<(int)NumLEDs?i:UNUSED), freeSpin(false)
00064 {
00065 setLoadSavePolicy(FIXED,SYNC);
00066 addEntry("Output",output,"Tekkotsu offset to pull servo positions from, relative to PIDJointOffset");
00067 addEntry("LED",led,"Tekkotsu offset to pull LED values from, relative to LEDOffset");
00068 addEntry("FreeSpin",freeSpin,"If true, servo will spin freely, interpreting output value as rotation speed");
00069 }
00070 plist::Primitive<int> output;
00071 plist::Primitive<int> led;
00072 plist::Primitive<bool> freeSpin;
00073 };
00074
00075 plist::DictionaryOf< ServoInfo > servos;
00076 typedef plist::DictionaryOf< ServoInfo >::const_iterator servo_iterator;
00077
00078 plist::Primitive<std::string> commName;
00079 plist::Primitive<bool> queryServos;
00080
00081 protected:
00082
00083 void provideOutput(unsigned int idx) {}
00084
00085 void ignoreOutput(unsigned int idx) {}
00086
00087
00088 virtual void setServo(struct SyncWritePosSpeedEntry& packet, const servo_iterator& servo, float v);
00089
00090
00091 virtual float getServo(unsigned int servoIdx, unsigned int pw);
00092
00093 virtual float getAnalog(unsigned int inputIdx, unsigned char s);
00094
00095 virtual float getDigital(unsigned int inputIdx, unsigned char cur, unsigned char latch);
00096 */
00097 bool motionActive;
00098 bool sensorsActive;
00099 std::string lastSensor;
00100 unsigned int frameNumber;
00101
00102 private:
00103
00104 static const std::string autoRegisterDynamixelDriver;
00105 };
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 #endif