sinyuu masahito / Mbed 2 deprecated yozakura_arm

Arm control program for Yozakura

Dependencies:   mbed

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?

UserRevisionLine numberNew 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