sinyuu masahito
/
yozakura_arm
Arm control program for Yozakura
Dynamixel/Dynamixel.h@0:6b3497b2f2ec, 2015-04-24 (annotated)
- Committer:
- masasin
- Date:
- Fri Apr 24 01:55:32 2015 +0000
- Revision:
- 0:6b3497b2f2ec
Initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
masasin | 0:6b3497b2f2ec | 1 | /* mbed Dynamixel Servo Library |
masasin | 0:6b3497b2f2ec | 2 | * |
masasin | 0:6b3497b2f2ec | 3 | * Copyright (c) 2015, Jean Nassar |
masasin | 0:6b3497b2f2ec | 4 | * |
masasin | 0:6b3497b2f2ec | 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
masasin | 0:6b3497b2f2ec | 6 | * of this software and associated documentation files (the "Software"), to deal |
masasin | 0:6b3497b2f2ec | 7 | * in the Software without restriction, including without limitation the rights |
masasin | 0:6b3497b2f2ec | 8 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
masasin | 0:6b3497b2f2ec | 9 | * copies of the Software, and to permit persons to whom the Software is |
masasin | 0:6b3497b2f2ec | 10 | * furnished to do so, subject to the following conditions: |
masasin | 0:6b3497b2f2ec | 11 | * |
masasin | 0:6b3497b2f2ec | 12 | * The above copyright notice and this permission notice shall be included in |
masasin | 0:6b3497b2f2ec | 13 | * all copies or substantial portions of the Software. |
masasin | 0:6b3497b2f2ec | 14 | * |
masasin | 0:6b3497b2f2ec | 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
masasin | 0:6b3497b2f2ec | 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
masasin | 0:6b3497b2f2ec | 17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
masasin | 0:6b3497b2f2ec | 18 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
masasin | 0:6b3497b2f2ec | 19 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
masasin | 0:6b3497b2f2ec | 20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
masasin | 0:6b3497b2f2ec | 21 | * THE SOFTWARE. |
masasin | 0:6b3497b2f2ec | 22 | */ |
masasin | 0:6b3497b2f2ec | 23 | #ifndef MBED_DYNAMIXEL_H |
masasin | 0:6b3497b2f2ec | 24 | #define MBED_DYNAMIXEL_H |
masasin | 0:6b3497b2f2ec | 25 | |
masasin | 0:6b3497b2f2ec | 26 | #include "mbed.h" |
masasin | 0:6b3497b2f2ec | 27 | #include "SerialHalfDuplex.h" |
masasin | 0:6b3497b2f2ec | 28 | |
masasin | 0:6b3497b2f2ec | 29 | #define AX12_WRITE_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 30 | #define AX12_READ_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 31 | #define AX12_TRIGGER_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 32 | #define AX12_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 33 | |
masasin | 0:6b3497b2f2ec | 34 | #define AX12_REG_ID 0x3 |
masasin | 0:6b3497b2f2ec | 35 | #define AX12_REG_CW_LIMIT 0x06 |
masasin | 0:6b3497b2f2ec | 36 | #define AX12_REG_CCW_LIMIT 0x08 |
masasin | 0:6b3497b2f2ec | 37 | #define AX12_REG_TORQUE_ENABLE 0x18 |
masasin | 0:6b3497b2f2ec | 38 | #define AX12_REG_GOAL_POSITION 0x1E |
masasin | 0:6b3497b2f2ec | 39 | #define AX12_REG_MOVING_SPEED 0x20 |
masasin | 0:6b3497b2f2ec | 40 | #define AX12_REG_VOLTS 0x2A |
masasin | 0:6b3497b2f2ec | 41 | #define AX12_REG_TEMP 0x2B |
masasin | 0:6b3497b2f2ec | 42 | #define AX12_REG_MOVING 0x2E |
masasin | 0:6b3497b2f2ec | 43 | #define AX12_REG_TORQUE_LIMIT 0x22 |
masasin | 0:6b3497b2f2ec | 44 | #define AX12_REG_POSITION 0x24 |
masasin | 0:6b3497b2f2ec | 45 | |
masasin | 0:6b3497b2f2ec | 46 | #define AX12_MODE_POSITION 0 |
masasin | 0:6b3497b2f2ec | 47 | #define AX12_MODE_ROTATION 1 |
masasin | 0:6b3497b2f2ec | 48 | |
masasin | 0:6b3497b2f2ec | 49 | #define AX12_CW 1 |
masasin | 0:6b3497b2f2ec | 50 | #define AX12_CCW 0 |
masasin | 0:6b3497b2f2ec | 51 | |
masasin | 0:6b3497b2f2ec | 52 | #define MX28_WRITE_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 53 | #define MX28_READ_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 54 | #define MX28_TRIGGER_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 55 | #define MX28_DEBUG 0 |
masasin | 0:6b3497b2f2ec | 56 | |
masasin | 0:6b3497b2f2ec | 57 | #define MX28_REG_ID 0x3 |
masasin | 0:6b3497b2f2ec | 58 | #define MX28_REG_CW_LIMIT 0x06 |
masasin | 0:6b3497b2f2ec | 59 | #define MX28_REG_CCW_LIMIT 0x08 |
masasin | 0:6b3497b2f2ec | 60 | #define MX28_REG_TORQUE_ENABLE 0x18 |
masasin | 0:6b3497b2f2ec | 61 | #define MX28_REG_GOAL_POSITION 0x1E |
masasin | 0:6b3497b2f2ec | 62 | #define MX28_REG_MOVING_SPEED 0x20 |
masasin | 0:6b3497b2f2ec | 63 | #define MX28_REG_VOLTS 0x2A |
masasin | 0:6b3497b2f2ec | 64 | #define MX28_REG_TEMP 0x2B |
masasin | 0:6b3497b2f2ec | 65 | #define MX28_REG_MOVING 0x2E |
masasin | 0:6b3497b2f2ec | 66 | #define MX28_REG_POSITION 0x24 |
masasin | 0:6b3497b2f2ec | 67 | #define MX28_REG_TORQUE_LIMIT 0x22 |
masasin | 0:6b3497b2f2ec | 68 | #define MX28_REG_CURRENT 0x44 |
masasin | 0:6b3497b2f2ec | 69 | |
masasin | 0:6b3497b2f2ec | 70 | #define MX28_MODE_POSITION 0 |
masasin | 0:6b3497b2f2ec | 71 | #define MX28_MODE_ROTATION 1 |
masasin | 0:6b3497b2f2ec | 72 | |
masasin | 0:6b3497b2f2ec | 73 | #define MX28_CW 1 |
masasin | 0:6b3497b2f2ec | 74 | #define MX28_CCW 0 |
masasin | 0:6b3497b2f2ec | 75 | |
masasin | 0:6b3497b2f2ec | 76 | /** Servo control class, based on a PwmOut |
masasin | 0:6b3497b2f2ec | 77 | * |
masasin | 0:6b3497b2f2ec | 78 | * Example: |
masasin | 0:6b3497b2f2ec | 79 | * @code |
masasin | 0:6b3497b2f2ec | 80 | * #include "mbed.h" |
masasin | 0:6b3497b2f2ec | 81 | * #include "AX12.h" |
masasin | 0:6b3497b2f2ec | 82 | * |
masasin | 0:6b3497b2f2ec | 83 | * int main() { |
masasin | 0:6b3497b2f2ec | 84 | * |
masasin | 0:6b3497b2f2ec | 85 | * AX12 myax12 (p9, p10, 1); |
masasin | 0:6b3497b2f2ec | 86 | * |
masasin | 0:6b3497b2f2ec | 87 | * while (1) { |
masasin | 0:6b3497b2f2ec | 88 | * myax12.SetGoal(0); // go to 0 degrees |
masasin | 0:6b3497b2f2ec | 89 | * wait (2.0); |
masasin | 0:6b3497b2f2ec | 90 | * myax12.SetGoal(300); // go to 300 degrees |
masasin | 0:6b3497b2f2ec | 91 | * wait (2.0); |
masasin | 0:6b3497b2f2ec | 92 | * } |
masasin | 0:6b3497b2f2ec | 93 | * } |
masasin | 0:6b3497b2f2ec | 94 | * @endcode |
masasin | 0:6b3497b2f2ec | 95 | */ |
masasin | 0:6b3497b2f2ec | 96 | class Dynamixel { |
masasin | 0:6b3497b2f2ec | 97 | |
masasin | 0:6b3497b2f2ec | 98 | public: |
masasin | 0:6b3497b2f2ec | 99 | |
masasin | 0:6b3497b2f2ec | 100 | |
masasin | 0:6b3497b2f2ec | 101 | |
masasin | 0:6b3497b2f2ec | 102 | /** Set the mode of the servo |
masasin | 0:6b3497b2f2ec | 103 | * @param mode |
masasin | 0:6b3497b2f2ec | 104 | * 0 = Positional, default |
masasin | 0:6b3497b2f2ec | 105 | * 1 = Continuous rotation |
masasin | 0:6b3497b2f2ec | 106 | */ |
masasin | 0:6b3497b2f2ec | 107 | virtual int SetMode(int mode) {return 0;} |
masasin | 0:6b3497b2f2ec | 108 | |
masasin | 0:6b3497b2f2ec | 109 | /** Set goal angle in integer degrees, in positional mode |
masasin | 0:6b3497b2f2ec | 110 | * |
masasin | 0:6b3497b2f2ec | 111 | * @param degrees 0-300 |
masasin | 0:6b3497b2f2ec | 112 | * @param flags, defaults to 0 |
masasin | 0:6b3497b2f2ec | 113 | * flags[0] = blocking, return when goal position reached |
masasin | 0:6b3497b2f2ec | 114 | * flags[1] = register, activate with a broadcast trigger |
masasin | 0:6b3497b2f2ec | 115 | * |
masasin | 0:6b3497b2f2ec | 116 | */ |
masasin | 0:6b3497b2f2ec | 117 | virtual int SetGoal(int degrees, int flags = 0) {return 0;} |
masasin | 0:6b3497b2f2ec | 118 | |
masasin | 0:6b3497b2f2ec | 119 | |
masasin | 0:6b3497b2f2ec | 120 | /** Set the speed of the servo in continuous rotation mode |
masasin | 0:6b3497b2f2ec | 121 | * |
masasin | 0:6b3497b2f2ec | 122 | * @param speed, -1.0 to 1.0 |
masasin | 0:6b3497b2f2ec | 123 | * -1.0 = full speed counter clock wise |
masasin | 0:6b3497b2f2ec | 124 | * 1.0 = full speed clock wise |
masasin | 0:6b3497b2f2ec | 125 | */ |
masasin | 0:6b3497b2f2ec | 126 | virtual int SetCRSpeed(float speed) {return 0;} |
masasin | 0:6b3497b2f2ec | 127 | |
masasin | 0:6b3497b2f2ec | 128 | |
masasin | 0:6b3497b2f2ec | 129 | /** Set the clockwise limit of the servo |
masasin | 0:6b3497b2f2ec | 130 | * |
masasin | 0:6b3497b2f2ec | 131 | * @param degrees, 0-300 |
masasin | 0:6b3497b2f2ec | 132 | */ |
masasin | 0:6b3497b2f2ec | 133 | virtual int SetCWLimit(int degrees) {return 0;} |
masasin | 0:6b3497b2f2ec | 134 | |
masasin | 0:6b3497b2f2ec | 135 | /** Set the counter-clockwise limit of the servo |
masasin | 0:6b3497b2f2ec | 136 | * |
masasin | 0:6b3497b2f2ec | 137 | * @param degrees, 0-300 |
masasin | 0:6b3497b2f2ec | 138 | */ |
masasin | 0:6b3497b2f2ec | 139 | virtual int SetCCWLimit(int degrees) {return 0;} |
masasin | 0:6b3497b2f2ec | 140 | |
masasin | 0:6b3497b2f2ec | 141 | // Change the ID |
masasin | 0:6b3497b2f2ec | 142 | |
masasin | 0:6b3497b2f2ec | 143 | /** Change the ID of a servo |
masasin | 0:6b3497b2f2ec | 144 | * |
masasin | 0:6b3497b2f2ec | 145 | * @param CurentID 1-255 |
masasin | 0:6b3497b2f2ec | 146 | * @param NewID 1-255 |
masasin | 0:6b3497b2f2ec | 147 | * |
masasin | 0:6b3497b2f2ec | 148 | * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID. |
masasin | 0:6b3497b2f2ec | 149 | * In this situation, only one servo should be connected to the bus |
masasin | 0:6b3497b2f2ec | 150 | */ |
masasin | 0:6b3497b2f2ec | 151 | virtual int SetID(int CurrentID, int NewID) {return 0;} |
masasin | 0:6b3497b2f2ec | 152 | |
masasin | 0:6b3497b2f2ec | 153 | |
masasin | 0:6b3497b2f2ec | 154 | /** Poll to see if the servo is moving |
masasin | 0:6b3497b2f2ec | 155 | * |
masasin | 0:6b3497b2f2ec | 156 | * @returns true is the servo is moving |
masasin | 0:6b3497b2f2ec | 157 | */ |
masasin | 0:6b3497b2f2ec | 158 | virtual int isMoving(void) {return 0;} |
masasin | 0:6b3497b2f2ec | 159 | |
masasin | 0:6b3497b2f2ec | 160 | /** Send the broadcast "trigger" command, to activate any outstanding registered commands |
masasin | 0:6b3497b2f2ec | 161 | */ |
masasin | 0:6b3497b2f2ec | 162 | virtual void trigger(void) {} |
masasin | 0:6b3497b2f2ec | 163 | |
masasin | 0:6b3497b2f2ec | 164 | /** Read the current angle of the servo |
masasin | 0:6b3497b2f2ec | 165 | * |
masasin | 0:6b3497b2f2ec | 166 | * @returns float in the range 0.0-300.0 |
masasin | 0:6b3497b2f2ec | 167 | */ |
masasin | 0:6b3497b2f2ec | 168 | virtual float GetPosition() {return 0;} |
masasin | 0:6b3497b2f2ec | 169 | |
masasin | 0:6b3497b2f2ec | 170 | /** Read the temperature of the servo |
masasin | 0:6b3497b2f2ec | 171 | * |
masasin | 0:6b3497b2f2ec | 172 | * @returns float temperature |
masasin | 0:6b3497b2f2ec | 173 | */ |
masasin | 0:6b3497b2f2ec | 174 | virtual float GetTemp(void) {return 0;} |
masasin | 0:6b3497b2f2ec | 175 | |
masasin | 0:6b3497b2f2ec | 176 | /** Read the supply voltage of the servo |
masasin | 0:6b3497b2f2ec | 177 | * |
masasin | 0:6b3497b2f2ec | 178 | * @returns float voltage |
masasin | 0:6b3497b2f2ec | 179 | */ |
masasin | 0:6b3497b2f2ec | 180 | virtual float GetVolts(void) {return 0;} |
masasin | 0:6b3497b2f2ec | 181 | virtual float GetCurrent(void) {return 0;} |
masasin | 0:6b3497b2f2ec | 182 | |
masasin | 0:6b3497b2f2ec | 183 | virtual int TorqueEnable(int mode) {return 0;} |
masasin | 0:6b3497b2f2ec | 184 | |
masasin | 0:6b3497b2f2ec | 185 | virtual int SetTorqueLimit(float torque_limit) {return 0;} |
masasin | 0:6b3497b2f2ec | 186 | |
masasin | 0:6b3497b2f2ec | 187 | protected : |
masasin | 0:6b3497b2f2ec | 188 | int _ID; |
masasin | 0:6b3497b2f2ec | 189 | |
masasin | 0:6b3497b2f2ec | 190 | virtual int read(int ID, int start, int length, char* data) {return 0;} |
masasin | 0:6b3497b2f2ec | 191 | virtual int write(int ID, int start, int length, char* data, int flag=0) {return 0;} |
masasin | 0:6b3497b2f2ec | 192 | |
masasin | 0:6b3497b2f2ec | 193 | }; |
masasin | 0:6b3497b2f2ec | 194 | |
masasin | 0:6b3497b2f2ec | 195 | class AX12 : public Dynamixel { |
masasin | 0:6b3497b2f2ec | 196 | public: |
masasin | 0:6b3497b2f2ec | 197 | /** Create a Dynamixel servo object connected to the specified serial port, with the specified ID |
masasin | 0:6b3497b2f2ec | 198 | * |
masasin | 0:6b3497b2f2ec | 199 | * @param pin tx pin |
masasin | 0:6b3497b2f2ec | 200 | * @param pin rx pin |
masasin | 0:6b3497b2f2ec | 201 | * @param int ID, the Bus ID of the servo 1-255 |
masasin | 0:6b3497b2f2ec | 202 | */ |
masasin | 0:6b3497b2f2ec | 203 | AX12(PinName tx, PinName rx, int ID); |
masasin | 0:6b3497b2f2ec | 204 | |
masasin | 0:6b3497b2f2ec | 205 | virtual int SetMode(int mode); |
masasin | 0:6b3497b2f2ec | 206 | virtual int SetGoal(int degrees, int flags = 0); |
masasin | 0:6b3497b2f2ec | 207 | virtual int SetCRSpeed(float speed); |
masasin | 0:6b3497b2f2ec | 208 | virtual int SetCWLimit(int degrees); |
masasin | 0:6b3497b2f2ec | 209 | virtual int SetCCWLimit(int degrees); |
masasin | 0:6b3497b2f2ec | 210 | virtual int SetID(int CurrentID, int NewID); |
masasin | 0:6b3497b2f2ec | 211 | virtual int isMoving(void); |
masasin | 0:6b3497b2f2ec | 212 | virtual void trigger(void); |
masasin | 0:6b3497b2f2ec | 213 | virtual float GetPosition(); |
masasin | 0:6b3497b2f2ec | 214 | virtual float GetTemp(void); |
masasin | 0:6b3497b2f2ec | 215 | virtual float GetVolts(void); |
masasin | 0:6b3497b2f2ec | 216 | virtual int TorqueEnable(int mode); |
masasin | 0:6b3497b2f2ec | 217 | virtual int SetTorqueLimit(float torque_limit); |
masasin | 0:6b3497b2f2ec | 218 | virtual float GetCurrent(void); |
masasin | 0:6b3497b2f2ec | 219 | |
masasin | 0:6b3497b2f2ec | 220 | protected: |
masasin | 0:6b3497b2f2ec | 221 | virtual int read(int ID, int start, int length, char* data); |
masasin | 0:6b3497b2f2ec | 222 | virtual int write(int ID, int start, int length, char* data, int flag=0); |
masasin | 0:6b3497b2f2ec | 223 | |
masasin | 0:6b3497b2f2ec | 224 | private: |
masasin | 0:6b3497b2f2ec | 225 | SerialHalfDuplex _ax12; |
masasin | 0:6b3497b2f2ec | 226 | }; |
masasin | 0:6b3497b2f2ec | 227 | |
masasin | 0:6b3497b2f2ec | 228 | class MX28 : public Dynamixel { |
masasin | 0:6b3497b2f2ec | 229 | public: |
masasin | 0:6b3497b2f2ec | 230 | /** Read the supply current of the servo |
masasin | 0:6b3497b2f2ec | 231 | * |
masasin | 0:6b3497b2f2ec | 232 | * @returns float current |
masasin | 0:6b3497b2f2ec | 233 | */ |
masasin | 0:6b3497b2f2ec | 234 | MX28(PinName tx, PinName rx, int ID); |
masasin | 0:6b3497b2f2ec | 235 | |
masasin | 0:6b3497b2f2ec | 236 | virtual int SetMode(int mode); |
masasin | 0:6b3497b2f2ec | 237 | virtual int SetGoal(int degrees, int flags = 0); |
masasin | 0:6b3497b2f2ec | 238 | virtual int SetCRSpeed(float speed); |
masasin | 0:6b3497b2f2ec | 239 | virtual int SetCWLimit(int degrees); |
masasin | 0:6b3497b2f2ec | 240 | virtual int SetCCWLimit(int degrees); |
masasin | 0:6b3497b2f2ec | 241 | virtual int SetID(int CurrentID, int NewID); |
masasin | 0:6b3497b2f2ec | 242 | virtual int isMoving(void); |
masasin | 0:6b3497b2f2ec | 243 | virtual void trigger(void); |
masasin | 0:6b3497b2f2ec | 244 | virtual float GetPosition(); |
masasin | 0:6b3497b2f2ec | 245 | virtual float GetTemp(void); |
masasin | 0:6b3497b2f2ec | 246 | virtual float GetVolts(void); |
masasin | 0:6b3497b2f2ec | 247 | virtual int TorqueEnable(int mode); |
masasin | 0:6b3497b2f2ec | 248 | virtual int SetTorqueLimit(float torque_limit); |
masasin | 0:6b3497b2f2ec | 249 | |
masasin | 0:6b3497b2f2ec | 250 | /** Create a Dynamixel servo object connected to the specified serial port, with the specified ID |
masasin | 0:6b3497b2f2ec | 251 | * |
masasin | 0:6b3497b2f2ec | 252 | * @param pin tx pin |
masasin | 0:6b3497b2f2ec | 253 | * @param pin rx pin |
masasin | 0:6b3497b2f2ec | 254 | * @param int ID, the Bus ID of the servo 1-255 |
masasin | 0:6b3497b2f2ec | 255 | */ |
masasin | 0:6b3497b2f2ec | 256 | virtual float GetCurrent(void); |
masasin | 0:6b3497b2f2ec | 257 | |
masasin | 0:6b3497b2f2ec | 258 | protected: |
masasin | 0:6b3497b2f2ec | 259 | virtual int read(int ID, int start, int length, char* data); |
masasin | 0:6b3497b2f2ec | 260 | virtual int write(int ID, int start, int length, char* data, int flag=0); |
masasin | 0:6b3497b2f2ec | 261 | |
masasin | 0:6b3497b2f2ec | 262 | private: |
masasin | 0:6b3497b2f2ec | 263 | SerialHalfDuplex _mx28; |
masasin | 0:6b3497b2f2ec | 264 | }; |
masasin | 0:6b3497b2f2ec | 265 | |
masasin | 0:6b3497b2f2ec | 266 | #endif |