James Hilder / Mbed 2 deprecated PR_RobotArmSetup

Code to set up an AX-12 or MX-28 (and to scan for unidentified servos) ready for use in the robot arm.

Dependencies:   mbed

servo.h@0:03f84c95f73b, 2017-02-16 (annotated)

Committer:
jah128
Date:
Thu Feb 16 20:45:33 2017 +0000
Revision:
0:03f84c95f73b
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 0:03f84c95f73b 1 /* University of York Robotics Laboratory Robot Arm Controller Board
jah128 0:03f84c95f73b 2 *
jah128 0:03f84c95f73b 3 * Dynamixel Servo Library for AX-12 and MX-28
jah128 0:03f84c95f73b 4 *
jah128 0:03f84c95f73b 5 * Based on library by Chris Styles (see copyright notice at end of file)
jah128 0:03f84c95f73b 6 *
jah128 0:03f84c95f73b 7 * File: servo.cpp
jah128 0:03f84c95f73b 8 *
jah128 0:03f84c95f73b 9 * (C) Dept. Electronics & Computer Science, University of York
jah128 0:03f84c95f73b 10 * Chris Styles, James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis
jah128 0:03f84c95f73b 11 *
jah128 0:03f84c95f73b 12 * February 2017, Version 1.0
jah128 0:03f84c95f73b 13 */
jah128 0:03f84c95f73b 14
jah128 0:03f84c95f73b 15 // NOTE:
jah128 0:03f84c95f73b 16 // When communicating with 'new' servos the are defaulted to ID 1
jah128 0:03f84c95f73b 17 // Make sure only one such servo is connected at one time
jah128 0:03f84c95f73b 18 // Also note that this library defaults to 57600 serial comms
jah128 0:03f84c95f73b 19 // The MX-28s are defaulted to communicate at this speed but the AX-12s are not (1Mbaud)
jah128 0:03f84c95f73b 20 // Use SetInitBaud(57600) to override the default and then change baud rate when setting up MX-28s
jah128 0:03f84c95f73b 21
jah128 0:03f84c95f73b 22 #ifndef SERVO_H
jah128 0:03f84c95f73b 23 #define SERVO_H
jah128 0:03f84c95f73b 24
jah128 0:03f84c95f73b 25 #include "mbed.h"
jah128 0:03f84c95f73b 26 #include "SerialHalfDuplex.h"
jah128 0:03f84c95f73b 27
jah128 0:03f84c95f73b 28 #define WRITE_DEBUG 0
jah128 0:03f84c95f73b 29 #define READ_DEBUG 0
jah128 0:03f84c95f73b 30 #define TRIGGER_DEBUG 0
jah128 0:03f84c95f73b 31 #define DEBUG 0
jah128 0:03f84c95f73b 32
jah128 0:03f84c95f73b 33 // Number of times to retry a read\write operation if read times out
jah128 0:03f84c95f73b 34 #define READ_TIMEOUT_LIMIT 3
jah128 0:03f84c95f73b 35
jah128 0:03f84c95f73b 36 #define LOW_VOLTAGE_LIMIT 90
jah128 0:03f84c95f73b 37 #define HIGH_VOLTAGE_LIMIT 140
jah128 0:03f84c95f73b 38 #define HIGH_TEMPERATURE_LIMIT 70
jah128 0:03f84c95f73b 39
jah128 0:03f84c95f73b 40 #define AX12_MODEL 0x0C
jah128 0:03f84c95f73b 41 #define MX28_MODEL 0x1D
jah128 0:03f84c95f73b 42
jah128 0:03f84c95f73b 43 #define RETURN_DELAY 250
jah128 0:03f84c95f73b 44
jah128 0:03f84c95f73b 45 #define REG_MODEL_NUMBER 0x00
jah128 0:03f84c95f73b 46 #define REG_FIRMWARE_VERSION 0x02
jah128 0:03f84c95f73b 47 #define REG_ID 0x03
jah128 0:03f84c95f73b 48 #define REG_BAUDRATE 0x04
jah128 0:03f84c95f73b 49 #define REG_RETURN_DELAY 0x05
jah128 0:03f84c95f73b 50 #define REG_CW_LIMIT 0x06
jah128 0:03f84c95f73b 51 #define REG_CCW_LIMIT 0x08
jah128 0:03f84c95f73b 52 #define REG_HIGHTEMP_LIMIT 0x0B
jah128 0:03f84c95f73b 53 #define REG_LOW_VOLTAGE_LIMIT 0x0C
jah128 0:03f84c95f73b 54 #define REG_HIGH_VOLTAGE_LIMIT 0x0D
jah128 0:03f84c95f73b 55 #define REG_MAX_TORQUE 0x0E
jah128 0:03f84c95f73b 56 #define REG_TORQUE_ENABLE 0x18
jah128 0:03f84c95f73b 57 #define REG_GOAL_POSITION 0x1E
jah128 0:03f84c95f73b 58 #define REG_MOVING_SPEED 0x20
jah128 0:03f84c95f73b 59 #define REG_VOLTS 0x2A
jah128 0:03f84c95f73b 60 #define REG_TEMP 0x2B
jah128 0:03f84c95f73b 61 #define REG_SPEED 0x26
jah128 0:03f84c95f73b 62 #define REG_LOAD 0x28
jah128 0:03f84c95f73b 63 #define REG_MOVING 0x2E
jah128 0:03f84c95f73b 64 #define REG_POSITION 0x24
jah128 0:03f84c95f73b 65 #define REG_EEPROM_LOCK 0x2F
jah128 0:03f84c95f73b 66
jah128 0:03f84c95f73b 67 #define MODE_POSITION 0
jah128 0:03f84c95f73b 68 #define MODE_ROTATION 1
jah128 0:03f84c95f73b 69
jah128 0:03f84c95f73b 70 #define CW 1
jah128 0:03f84c95f73b 71 #define CCW 0
jah128 0:03f84c95f73b 72
jah128 0:03f84c95f73b 73 extern Serial data;
jah128 0:03f84c95f73b 74
jah128 0:03f84c95f73b 75 class Servo {
jah128 0:03f84c95f73b 76
jah128 0:03f84c95f73b 77 public:
jah128 0:03f84c95f73b 78
jah128 0:03f84c95f73b 79 /** Create a Dynamixel servo controller object connected to the specified serial port, with the specified ID
jah128 0:03f84c95f73b 80 *
jah128 0:03f84c95f73b 81 * @param pin tx pin
jah128 0:03f84c95f73b 82 * @param pin rx pin
jah128 0:03f84c95f73b 83 */
jah128 0:03f84c95f73b 84 Servo(PinName tx, PinName rx);
jah128 0:03f84c95f73b 85
jah128 0:03f84c95f73b 86 void ClearBuffer(void);
jah128 0:03f84c95f73b 87
jah128 0:03f84c95f73b 88 void ScanForServos(void);
jah128 0:03f84c95f73b 89
jah128 0:03f84c95f73b 90
jah128 0:03f84c95f73b 91 /** Prints (over PC serial) a detailed set of data for the servo
jah128 0:03f84c95f73b 92 *
jah128 0:03f84c95f73b 93 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 94 */
jah128 0:03f84c95f73b 95 void DebugData(int ID);
jah128 0:03f84c95f73b 96
jah128 0:03f84c95f73b 97 /** Set the mode of the servo
jah128 0:03f84c95f73b 98 *
jah128 0:03f84c95f73b 99 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 100 * @param mode
jah128 0:03f84c95f73b 101 * 0 = Positional, default
jah128 0:03f84c95f73b 102 * 1 = Continuous rotation
jah128 0:03f84c95f73b 103 */
jah128 0:03f84c95f73b 104 int SetMode(int ID, int mode);
jah128 0:03f84c95f73b 105
jah128 0:03f84c95f73b 106 /** Set goal angle, in positional mode
jah128 0:03f84c95f73b 107 *
jah128 0:03f84c95f73b 108 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 109 * @param goal 0-4095
jah128 0:03f84c95f73b 110 * @param flags, defaults to 0
jah128 0:03f84c95f73b 111 * flags[0] = blocking, return when goal position reached
jah128 0:03f84c95f73b 112 * flags[1] = register, activate with a broadcast trigger
jah128 0:03f84c95f73b 113 *
jah128 0:03f84c95f73b 114 */
jah128 0:03f84c95f73b 115 int SetGoal(int ID, short goal, int flags = 0);
jah128 0:03f84c95f73b 116
jah128 0:03f84c95f73b 117 /** Set goal angle in integer degrees, in positional mode
jah128 0:03f84c95f73b 118 *
jah128 0:03f84c95f73b 119 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 120 * @param degrees 0-300
jah128 0:03f84c95f73b 121 * @param flags, defaults to 0
jah128 0:03f84c95f73b 122 * flags[0] = blocking, return when goal position reached
jah128 0:03f84c95f73b 123 * flags[1] = register, activate with a broadcast trigger
jah128 0:03f84c95f73b 124 *
jah128 0:03f84c95f73b 125 */
jah128 0:03f84c95f73b 126 int SetGoalDegrees(int ID, int degrees, int flags = 0);
jah128 0:03f84c95f73b 127
jah128 0:03f84c95f73b 128
jah128 0:03f84c95f73b 129 /** Set the speed of the servo in continuous rotation mode
jah128 0:03f84c95f73b 130 *
jah128 0:03f84c95f73b 131 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 132 * @param speed, -1.0 to 1.0
jah128 0:03f84c95f73b 133 * -1.0 = full speed counter clock wise
jah128 0:03f84c95f73b 134 * 1.0 = full speed clock wise
jah128 0:03f84c95f73b 135 */
jah128 0:03f84c95f73b 136 int SetCRSpeed(int ID, float speed);
jah128 0:03f84c95f73b 137
jah128 0:03f84c95f73b 138
jah128 0:03f84c95f73b 139 /** Set the clockwise limit of the servo
jah128 0:03f84c95f73b 140 *
jah128 0:03f84c95f73b 141 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 142 * @param degrees, 0-300
jah128 0:03f84c95f73b 143 */
jah128 0:03f84c95f73b 144 int SetCWLimit(int ID, int degrees);
jah128 0:03f84c95f73b 145
jah128 0:03f84c95f73b 146 /** Set the counter-clockwise limit of the servo
jah128 0:03f84c95f73b 147 *
jah128 0:03f84c95f73b 148 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 149 * @param degrees, 0-300
jah128 0:03f84c95f73b 150 */
jah128 0:03f84c95f73b 151 int SetCCWLimit(int ID, int degrees);
jah128 0:03f84c95f73b 152
jah128 0:03f84c95f73b 153 /** Enable or disable the torque hold of the servo
jah128 0:03f84c95f73b 154 *
jah128 0:03f84c95f73b 155 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 156 * @param enable, 0=disable (floppy) 1=enable (hold)
jah128 0:03f84c95f73b 157 */
jah128 0:03f84c95f73b 158 int SetTorqueEnable (int ID, int enable);
jah128 0:03f84c95f73b 159
jah128 0:03f84c95f73b 160
jah128 0:03f84c95f73b 161 /** Change the low voltage limit of a servo
jah128 0:03f84c95f73b 162 *
jah128 0:03f84c95f73b 163 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 164 * @param lv_limit, 0-254
jah128 0:03f84c95f73b 165 *
jah128 0:03f84c95f73b 166 * voltage = lv_limit / 10
jah128 0:03f84c95f73b 167 */
jah128 0:03f84c95f73b 168 int SetLowVoltageLimit (int ID, char lv_limit);
jah128 0:03f84c95f73b 169
jah128 0:03f84c95f73b 170 /** Lock the EEPROM area of a servo
jah128 0:03f84c95f73b 171 *
jah128 0:03f84c95f73b 172 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 173 */
jah128 0:03f84c95f73b 174 int LockEeprom(int ID);
jah128 0:03f84c95f73b 175
jah128 0:03f84c95f73b 176 /** Change the high voltage limit of a servo
jah128 0:03f84c95f73b 177 *
jah128 0:03f84c95f73b 178 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 179 * @param hv_limit, 0-254
jah128 0:03f84c95f73b 180 *
jah128 0:03f84c95f73b 181 * voltage = hv_limit / 10
jah128 0:03f84c95f73b 182 */
jah128 0:03f84c95f73b 183 int SetHighVoltageLimit (int ID, char hv_limit);
jah128 0:03f84c95f73b 184
jah128 0:03f84c95f73b 185 /** Change the delay time of a servo
jah128 0:03f84c95f73b 186 *
jah128 0:03f84c95f73b 187 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 188 * @param delay, 0-254 C
jah128 0:03f84c95f73b 189 *
jah128 0:03f84c95f73b 190 */
jah128 0:03f84c95f73b 191 int SetDelayTime (int ID, char delay);
jah128 0:03f84c95f73b 192
jah128 0:03f84c95f73b 193 /** Change the high temperature limit of a servo
jah128 0:03f84c95f73b 194 *
jah128 0:03f84c95f73b 195 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 196 * @param temp_limit, 0-254 C
jah128 0:03f84c95f73b 197 *
jah128 0:03f84c95f73b 198 */
jah128 0:03f84c95f73b 199 int SetTemperatureLimit (int ID, char temp_limit);
jah128 0:03f84c95f73b 200
jah128 0:03f84c95f73b 201 /** Change the ID of a servo
jah128 0:03f84c95f73b 202 *
jah128 0:03f84c95f73b 203 * @param CurentID 1-255
jah128 0:03f84c95f73b 204 * @param NewID 1-255
jah128 0:03f84c95f73b 205 *
jah128 0:03f84c95f73b 206 * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
jah128 0:03f84c95f73b 207 * In this situation, only one servo should be connected to the bus
jah128 0:03f84c95f73b 208 */
jah128 0:03f84c95f73b 209 int SetID(int CurrentID, int NewID);
jah128 0:03f84c95f73b 210
jah128 0:03f84c95f73b 211 /** Change the baud of a servo
jah128 0:03f84c95f73b 212 *
jah128 0:03f84c95f73b 213 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 214 * @param baud, 0-252
jah128 0:03f84c95f73b 215 *
jah128 0:03f84c95f73b 216 * baudrate = 2000000/baud (with special cases 250=2.25Mbps, 251=2.5Mbps and 252=3Mbps on MX-28 only)
jah128 0:03f84c95f73b 217 */
jah128 0:03f84c95f73b 218 int SetBaud (int ID, int baud);
jah128 0:03f84c95f73b 219
jah128 0:03f84c95f73b 220 /** Poll to see if the servo is moving
jah128 0:03f84c95f73b 221 *
jah128 0:03f84c95f73b 222 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 223 * @returns true is the servo is moving
jah128 0:03f84c95f73b 224 */
jah128 0:03f84c95f73b 225 int isMoving(int ID);
jah128 0:03f84c95f73b 226
jah128 0:03f84c95f73b 227 /** Send the broadcast "trigger" command, to activate any outstanding registered commands
jah128 0:03f84c95f73b 228 */
jah128 0:03f84c95f73b 229 void trigger(void);
jah128 0:03f84c95f73b 230
jah128 0:03f84c95f73b 231 /** Read the model number of the servo
jah128 0:03f84c95f73b 232 *
jah128 0:03f84c95f73b 233 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 234 * @returns int matching defined model number
jah128 0:03f84c95f73b 235 */
jah128 0:03f84c95f73b 236 int GetModelNumber(int ID);
jah128 0:03f84c95f73b 237
jah128 0:03f84c95f73b 238 /** Read the current angle of the servo
jah128 0:03f84c95f73b 239 *
jah128 0:03f84c95f73b 240 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 241 * @returns float in the range 0.0-300.0
jah128 0:03f84c95f73b 242 */
jah128 0:03f84c95f73b 243 float GetPositionDegrees(int ID);
jah128 0:03f84c95f73b 244
jah128 0:03f84c95f73b 245
jah128 0:03f84c95f73b 246 /** Read the raw angle reading of the servo
jah128 0:03f84c95f73b 247 *
jah128 0:03f84c95f73b 248 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 249 * @returns short in the range 0 - 4095 for MX-28
jah128 0:03f84c95f73b 250 */
jah128 0:03f84c95f73b 251 short GetPosition(int ID);
jah128 0:03f84c95f73b 252
jah128 0:03f84c95f73b 253
jah128 0:03f84c95f73b 254 /** Read the temperature of the servo
jah128 0:03f84c95f73b 255 *
jah128 0:03f84c95f73b 256 * @returns float temperature
jah128 0:03f84c95f73b 257 */
jah128 0:03f84c95f73b 258 float GetTemp(int ID);
jah128 0:03f84c95f73b 259
jah128 0:03f84c95f73b 260 /** Read the supply voltage of the servo
jah128 0:03f84c95f73b 261 *
jah128 0:03f84c95f73b 262 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 263 * @returns short voltage * 10
jah128 0:03f84c95f73b 264 */
jah128 0:03f84c95f73b 265 short GetVoltage(int ID) ;
jah128 0:03f84c95f73b 266
jah128 0:03f84c95f73b 267 /** Read the supply voltage of the servo
jah128 0:03f84c95f73b 268 *
jah128 0:03f84c95f73b 269 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 270 * @returns float voltage
jah128 0:03f84c95f73b 271 */
jah128 0:03f84c95f73b 272 float GetVolts(int ID);
jah128 0:03f84c95f73b 273
jah128 0:03f84c95f73b 274 /** Read the temperature of the servo
jah128 0:03f84c95f73b 275 *
jah128 0:03f84c95f73b 276 * @returns short temperature
jah128 0:03f84c95f73b 277 */
jah128 0:03f84c95f73b 278 short GetTemperature(int ID);
jah128 0:03f84c95f73b 279
jah128 0:03f84c95f73b 280 /** Read the torque load of the servo
jah128 0:03f84c95f73b 281 *
jah128 0:03f84c95f73b 282 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 283 * @returns short load, range 0-2048, 0 is 100% CCW, 1024 is zero and 2048 is 100% CW
jah128 0:03f84c95f73b 284 */
jah128 0:03f84c95f73b 285 short GetLoad(int ID) ;
jah128 0:03f84c95f73b 286
jah128 0:03f84c95f73b 287 /** Read the speed of the servo
jah128 0:03f84c95f73b 288 *
jah128 0:03f84c95f73b 289 * @param ID, the Bus ID of the servo 1-255
jah128 0:03f84c95f73b 290 * @returns short speed
jah128 0:03f84c95f73b 291 */
jah128 0:03f84c95f73b 292 short GetSpeed(int ID) ;
jah128 0:03f84c95f73b 293
jah128 0:03f84c95f73b 294 /** Change the internal baud rate to something other than 1000000 (note MX-28s default to 57600)
jah128 0:03f84c95f73b 295 *
jah128 0:03f84c95f73b 296 * @param baud, the baud rate to set
jah128 0:03f84c95f73b 297 * @param delaytime, the delay time to set (x2 us)
jah128 0:03f84c95f73b 298 */
jah128 0:03f84c95f73b 299 void SetInitBaud(int baud, int delaytime);
jah128 0:03f84c95f73b 300
jah128 0:03f84c95f73b 301
jah128 0:03f84c95f73b 302 private :
jah128 0:03f84c95f73b 303 SerialHalfDuplex _servo;
jah128 0:03f84c95f73b 304 int read(int ID, int start, int length, char* data);
jah128 0:03f84c95f73b 305 int write(int ID, int start, int length, char* data, int flag=0);
jah128 0:03f84c95f73b 306
jah128 0:03f84c95f73b 307 };
jah128 0:03f84c95f73b 308
jah128 0:03f84c95f73b 309
jah128 0:03f84c95f73b 310
jah128 0:03f84c95f73b 311 #endif
jah128 0:03f84c95f73b 312
jah128 0:03f84c95f73b 313 /*
jah128 0:03f84c95f73b 314 * Copyright 2017 University of York
jah128 0:03f84c95f73b 315 *
jah128 0:03f84c95f73b 316 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
jah128 0:03f84c95f73b 317 * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
jah128 0:03f84c95f73b 318 * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
jah128 0:03f84c95f73b 319 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
jah128 0:03f84c95f73b 320 * See the License for the specific language governing permissions and limitations under the License.
jah128 0:03f84c95f73b 321 *
jah128 0:03f84c95f73b 322 */
jah128 0:03f84c95f73b 323
jah128 0:03f84c95f73b 324 /*
jah128 0:03f84c95f73b 325 * Copyright (c) 2010, Chris Styles (http://mbed.org)
jah128 0:03f84c95f73b 326 *
jah128 0:03f84c95f73b 327 * Permission is hereby granted, free of charge, to any person obtaining a copy
jah128 0:03f84c95f73b 328 * of this software and associated documentation files (the "Software"), to deal
jah128 0:03f84c95f73b 329 * in the Software without restriction, including without limitation the rights
jah128 0:03f84c95f73b 330 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jah128 0:03f84c95f73b 331 * copies of the Software, and to permit persons to whom the Software is
jah128 0:03f84c95f73b 332 * furnished to do so, subject to the following conditions:
jah128 0:03f84c95f73b 333 *
jah128 0:03f84c95f73b 334 * The above copyright notice and this permission notice shall be included in
jah128 0:03f84c95f73b 335 * all copies or substantial portions of the Software.
jah128 0:03f84c95f73b 336 *
jah128 0:03f84c95f73b 337 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jah128 0:03f84c95f73b 338 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jah128 0:03f84c95f73b 339 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jah128 0:03f84c95f73b 340 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jah128 0:03f84c95f73b 341 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jah128 0:03f84c95f73b 342 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jah128 0:03f84c95f73b 343 * THE SOFTWARE.
jah128 0:03f84c95f73b 344 */