Yoonseok Pyo / herkulex

herkulex servo control library

Dependents:   HerkuleX-HelloWorld

This herkulex library is based on DongBu Robot documentation and protocol.

http://dasarobot.com/guide/herkulexeng.pdf

/media/uploads/passionvirus/mbedandherkulex_i.png /media/uploads/passionvirus/range.png

herkulex.h@6:1dacff31b77a, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 21:03:43 2013 +0000
Revision:
6:1dacff31b77a
Parent:
5:f737e5c70115 
add function of getPos, getStatus, rxPacket etc.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
passionvirus 0:0eef242852bb 1 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 2 /* herkulex servo library for mbed
passionvirus 0:0eef242852bb 3 *
passionvirus 1:874d1f42989c 4 * Copyright (c) 2012-2013 Yoonseok Pyo, MIT License
passionvirus 0:0eef242852bb 5 *
passionvirus 1:874d1f42989c 6 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
passionvirus 1:874d1f42989c 7 * and associated documentation files (the "Software"), to deal in the Software without restriction,
passionvirus 1:874d1f42989c 8 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
passionvirus 1:874d1f42989c 9 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
passionvirus 1:874d1f42989c 10 * furnished to do so, subject to the following conditions:
passionvirus 0:0eef242852bb 11 *
passionvirus 1:874d1f42989c 12 * The above copyright notice and this permission notice shall be included in all copies or
passionvirus 1:874d1f42989c 13 * substantial portions of the Software.
passionvirus 0:0eef242852bb 14 *
passionvirus 1:874d1f42989c 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
passionvirus 1:874d1f42989c 16 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
passionvirus 1:874d1f42989c 17 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
passionvirus 1:874d1f42989c 18 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
passionvirus 1:874d1f42989c 19 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
passionvirus 0:0eef242852bb 20 */
passionvirus 0:0eef242852bb 21 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 22 #ifndef MBED_HERKULEX_H
passionvirus 0:0eef242852bb 23 #define MBED_HERKULEX_H
passionvirus 0:0eef242852bb 24
passionvirus 0:0eef242852bb 25 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 26 #include "mbed.h"
passionvirus 0:0eef242852bb 27
passionvirus 0:0eef242852bb 28 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 29 #define HERKULEX_DEBUG
passionvirus 0:0eef242852bb 30
passionvirus 0:0eef242852bb 31 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 32 // Herkulex ROM Register
passionvirus 0:0eef242852bb 33 #define ROM_MODEL_NO1 0
passionvirus 0:0eef242852bb 34 #define ROM_MODEL_NO2 1
passionvirus 0:0eef242852bb 35 #define ROM_VERSION1 2
passionvirus 0:0eef242852bb 36 #define ROM_VERSION2 3
passionvirus 0:0eef242852bb 37 #define ROM_BAUD_RATE 4
passionvirus 0:0eef242852bb 38 #define ROM_RESERVED5 5
passionvirus 0:0eef242852bb 39 #define ROM_ID 6
passionvirus 0:0eef242852bb 40 #define ROM_ACK_POLICY 7
passionvirus 0:0eef242852bb 41 #define ROM_ALARM_LED_POLICY 8
passionvirus 0:0eef242852bb 42 #define ROM_TORQUE_POLICY 9
passionvirus 0:0eef242852bb 43 #define ROM_RESERVED10 10
passionvirus 0:0eef242852bb 44 #define ROM_MAX_TEMPERATURE 11
passionvirus 0:0eef242852bb 45 #define ROM_MIN_VOLTAGE 12
passionvirus 0:0eef242852bb 46 #define ROM_MAX_VOLTAGE 13
passionvirus 0:0eef242852bb 47 #define ROM_ACCELERATION_RATIO 14
passionvirus 0:0eef242852bb 48 #define ROM_MAX_ACCELERATION_TIME 15
passionvirus 0:0eef242852bb 49 #define ROM_DEAD_ZONE 16
passionvirus 0:0eef242852bb 50 #define ROM_SATURATOR_OFFSET 17
passionvirus 0:0eef242852bb 51 #define ROM_SATURATOR_SLOPE 18 // 2Byte
passionvirus 0:0eef242852bb 52 #define ROM_PWM_OFFSET 20
passionvirus 0:0eef242852bb 53 #define ROM_MIN_PWM 21
passionvirus 0:0eef242852bb 54 #define ROM_MAX_PWM 22 // 2Byte
passionvirus 0:0eef242852bb 55 #define ROM_OVERLOAD_PWM_THRESHOLD 24 // 2Byte
passionvirus 0:0eef242852bb 56 #define ROM_MIN_POSITION 26 // 2Byte
passionvirus 0:0eef242852bb 57 #define ROM_MAX_POSITION 28 // 2Byte
passionvirus 0:0eef242852bb 58 #define ROM_POSITION_KP 30 // 2Byte
passionvirus 0:0eef242852bb 59 #define ROM_POSITION_KD 32 // 2Byte
passionvirus 0:0eef242852bb 60 #define ROM_POSITION_KI 34 // 2Byte
passionvirus 0:0eef242852bb 61 #define ROM_POSITION_FEEDFORWARD_1ST_GAIN 36 // 2Byte
passionvirus 0:0eef242852bb 62 #define ROM_POSITION FEEDFORWARD_2ND_GAIN 38 // 2Byte
passionvirus 0:0eef242852bb 63 #define ROM_RESERVED40 40 // 2Byte
passionvirus 0:0eef242852bb 64 #define ROM_RESERVED42 42 // 2Byte
passionvirus 0:0eef242852bb 65 #define ROM_LED_BLINK_PERIOD 44
passionvirus 0:0eef242852bb 66 #define ROM_ADC_FAULT_CHECK_PERIOD 45
passionvirus 0:0eef242852bb 67 #define ROM_PACKET_GARBAGE_CHECK_PERIOD 46
passionvirus 0:0eef242852bb 68 #define ROM_STOP_DETECTION_PERIOD 47
passionvirus 0:0eef242852bb 69 #define ROM_OVERLOAD_DETECTION_PERIOD 48
passionvirus 0:0eef242852bb 70 #define ROM_STOP_THRESHOLD 49
passionvirus 0:0eef242852bb 71 #define ROM_INPOSITION_MARGIN 50
passionvirus 0:0eef242852bb 72 #define ROM_RESERVED51 51
passionvirus 0:0eef242852bb 73 #define ROM_RESERVED52 52
passionvirus 0:0eef242852bb 74 #define ROM_CALIBRATION_DIFFERENCE 53
passionvirus 0:0eef242852bb 75
passionvirus 0:0eef242852bb 76 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 77 // Herkulex RAM Register
passionvirus 0:0eef242852bb 78 #define RAM_ID 0
passionvirus 0:0eef242852bb 79 #define RAM_ACK_POLICY 1
passionvirus 0:0eef242852bb 80 #define RAM_ALARM_LED_POLICY 2
passionvirus 0:0eef242852bb 81 #define RAM_TORQUE_POLICY 3
passionvirus 0:0eef242852bb 82 #define RAM_RESERVED4 4
passionvirus 0:0eef242852bb 83 #define RAM_MAX_TEMPERATURE 5
passionvirus 0:0eef242852bb 84 #define RAM_MIN_VOLTAGE 6
passionvirus 0:0eef242852bb 85 #define RAM_MAX_VOLTAGE 7
passionvirus 0:0eef242852bb 86 #define RAM_ACCELERATION_RATIO 8
passionvirus 0:0eef242852bb 87 #define RAM_MAX_ACCELERATION 9
passionvirus 0:0eef242852bb 88 #define RAM_DEAD_ZONE 10
passionvirus 0:0eef242852bb 89 #define RAM_SATURATOR_OFFSET 11
passionvirus 0:0eef242852bb 90 #define RAM_SATURATOR_SLOPE 12 // 2Byte
passionvirus 0:0eef242852bb 91 #define RAM_PWM_OFFSET 14
passionvirus 0:0eef242852bb 92 #define RAM_MIN_PWM 15
passionvirus 0:0eef242852bb 93 #define RAM_MAX_PWM 16 // 2Byte
passionvirus 0:0eef242852bb 94 #define RAM_OVERLOAD_PWM_THRESHOLD 18 // 2Byte
passionvirus 0:0eef242852bb 95 #define RAM_MIN_POSITION 20 // 2Byte
passionvirus 0:0eef242852bb 96 #define RAM_MAX_POSITION 22 // 2Byte
passionvirus 0:0eef242852bb 97 #define RAM_POSITION_KP 24 // 2Byte
passionvirus 0:0eef242852bb 98 #define RAM_POSITION_KD 26 // 2Byte
passionvirus 0:0eef242852bb 99 #define RAM_POSITION_KI 28 // 2Byte
passionvirus 0:0eef242852bb 100 #define RAM_POSITION_FEEDFORWARD_1ST_GAIN 30 // 2Byte
passionvirus 0:0eef242852bb 101 #define RAM_POSITION_FEEDFORWARD 2ND GAIN 32 // 2Byte
passionvirus 0:0eef242852bb 102 #define RAM_RESERVED34 34 // 2Byte
passionvirus 0:0eef242852bb 103 #define RAM_RESERVED36 36 // 2Byte
passionvirus 0:0eef242852bb 104 #define RAM_LED_BLINK_PERIOD 38
passionvirus 0:0eef242852bb 105 #define RAM_ADC_FAULT_DETECTION_PERIOD 39
passionvirus 0:0eef242852bb 106 #define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40
passionvirus 0:0eef242852bb 107 #define RAM_STOP_DETECTION_PERIOD 41
passionvirus 0:0eef242852bb 108 #define RAM_OVERLOAD_DETECTION_PERIOD 42
passionvirus 0:0eef242852bb 109 #define RAM_STOP_THRESHOLD 43
passionvirus 0:0eef242852bb 110 #define RAM_INPOSITION_MARGIN 44
passionvirus 0:0eef242852bb 111 #define RAM_RESERVED45 45
passionvirus 0:0eef242852bb 112 #define RAM_RESERVED46 46
passionvirus 0:0eef242852bb 113 #define RAM_CALIBRATION_DIFFERENCE 47
passionvirus 0:0eef242852bb 114 #define RAM_STATUS_ERROR 48
passionvirus 0:0eef242852bb 115 #define RAM_STATUS_DETAIL 49
passionvirus 0:0eef242852bb 116 #define RAM_RESERVED50 50
passionvirus 0:0eef242852bb 117 #define RAM_RESERVED51 51
passionvirus 0:0eef242852bb 118 #define RAM_TORQUE_CONTROL 52
passionvirus 0:0eef242852bb 119 #define RAM_LED_CONTROL 53
passionvirus 0:0eef242852bb 120 #define RAM_VOLTAGE 54
passionvirus 0:0eef242852bb 121 #define RAM_TEMPERATURE 55
passionvirus 0:0eef242852bb 122 #define RAM_CURRENT_CONTROL_MODE 56
passionvirus 0:0eef242852bb 123 #define RAM_TICK 57
passionvirus 0:0eef242852bb 124 #define RAM_CALIBRATED_POSITION 58 // 2Byte
passionvirus 0:0eef242852bb 125 #define RAM_ABSOLUTE_POSITION 60 // 2Byte
passionvirus 0:0eef242852bb 126 #define RAM_DIFFERENTIAL_POSITION 62 // 2Byte
passionvirus 0:0eef242852bb 127 #define RAM_PWM 64 // 2Byte
passionvirus 0:0eef242852bb 128 #define RAM_RESERVED66 66 // 2Byte
passionvirus 0:0eef242852bb 129 #define RAM_ABSOLUTE_GOAL_POSITION 68 // 2Byte
passionvirus 0:0eef242852bb 130 #define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION 70 // 2Byte
passionvirus 0:0eef242852bb 131 #define RAM_DESIRED_VELOCITY 72 // 2Byte
passionvirus 0:0eef242852bb 132
passionvirus 0:0eef242852bb 133 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 134 // Request Packet [To Servo Module]
passionvirus 0:0eef242852bb 135 #define CMD_ROM_WRITE 0x01 // Write Length number of values to EEP Register Address
passionvirus 0:0eef242852bb 136 #define CMD_ROM_READ 0x02 // Request Length number of values from EEP Register Address
passionvirus 0:0eef242852bb 137 #define CMD_RAM_WRITE 0x03 // Write Length number of values to RAM Register Address
passionvirus 0:0eef242852bb 138 #define CMD_RAM_READ 0x04 // Request Lenght number of values from RAM Register Address
passionvirus 0:0eef242852bb 139 #define CMD_I_JOG 0x05 // Able to send JOG command to maximum 43 servos (operate timing of individual Servo)
passionvirus 0:0eef242852bb 140 #define CMD_S_JOG 0x06 // Able to send JOG command to maximum 53 servos (operate simultaneously at same time)
passionvirus 0:0eef242852bb 141 #define CMD_STAT 0x07 // Status Error, Status Detail request
passionvirus 0:0eef242852bb 142 #define CMD_ROLLBACK 0x08 // Change all EEP Regsters to Factory Default value
passionvirus 0:0eef242852bb 143 #define CMD_REBOOT 0x09 // Request Reboot
passionvirus 0:0eef242852bb 144
passionvirus 0:0eef242852bb 145 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 146 // ACK Packet [To Controller(ACK)]
passionvirus 6:1dacff31b77a 147 #define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40
passionvirus 0:0eef242852bb 148 #define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 149 #define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 150 #define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 151 #define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 152 #define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 153 #define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 154 #define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 155 #define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 156 #define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 157
passionvirus 0:0eef242852bb 158 //------------------------------------------------------------------------------
passionvirus 6:1dacff31b77a 159 // Status Error
passionvirus 6:1dacff31b77a 160 #define STATUS_OK = 0x00;
passionvirus 6:1dacff31b77a 161 #define ERROR_EXCEED_INPUT_VOLTAGE = 0x01;
passionvirus 6:1dacff31b77a 162 #define ERROR_EXCEED_POT_LIMIT = 0x02;
passionvirus 6:1dacff31b77a 163 #define ERROR_EXCEED_TEMPERATURE_LIMIT = 0x04;
passionvirus 6:1dacff31b77a 164 #define ERROR_INVALID_PACKET = 0x08;
passionvirus 6:1dacff31b77a 165 #define ERROR_OVERLOAD = 0x10;
passionvirus 6:1dacff31b77a 166 #define ERROR_DRIVER_FAULT = 0x20;
passionvirus 6:1dacff31b77a 167 #define ERROR_EEP_REG_DISTORT = 0x40;
passionvirus 6:1dacff31b77a 168
passionvirus 6:1dacff31b77a 169 //------------------------------------------------------------------------------
passionvirus 6:1dacff31b77a 170 // Status Detail
passionvirus 6:1dacff31b77a 171 #define MOVING_FLAG = 0x01;
passionvirus 6:1dacff31b77a 172 #define INPOSITION_FLAG = 0x02;
passionvirus 6:1dacff31b77a 173 #define CHECKSUM_ERROR = 0x04; // Invalid packet`s detailed information
passionvirus 6:1dacff31b77a 174 #define UNKNOWN_COMMAND = 0x08; // Invalid packet`s detailed information
passionvirus 6:1dacff31b77a 175 #define EXCEED_REG_RANGE = 0x10; // Invalid packet`s detailed information
passionvirus 6:1dacff31b77a 176 #define GARBAGE_DETECTED = 0x20; // Invalid packet`s detailed information
passionvirus 6:1dacff31b77a 177 #define MOTOR_ON_FLAG = 0x40;
passionvirus 6:1dacff31b77a 178
passionvirus 6:1dacff31b77a 179 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 180 // Header
passionvirus 0:0eef242852bb 181 #define HEADER 0xFF
passionvirus 0:0eef242852bb 182
passionvirus 0:0eef242852bb 183 // Size
passionvirus 0:0eef242852bb 184 #define MIN_PACKET_SIZE 7
passionvirus 0:0eef242852bb 185 #define MIN_ACK_PACKET_SIZE 9
passionvirus 0:0eef242852bb 186 #define WRITE_PACKET_SIZE 13
passionvirus 0:0eef242852bb 187 #define MAX_PACKET_SIZE 223
passionvirus 0:0eef242852bb 188 #define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
passionvirus 0:0eef242852bb 189
passionvirus 0:0eef242852bb 190 // ID
passionvirus 0:0eef242852bb 191 #define MAX_PID 0xFD
passionvirus 0:0eef242852bb 192 #define DEFAULT_ID 0xFD
passionvirus 0:0eef242852bb 193 #define MAX_ID 0xFD
passionvirus 0:0eef242852bb 194 #define BROADCAST_ID 0xFE
passionvirus 0:0eef242852bb 195
passionvirus 0:0eef242852bb 196 // Checksum
passionvirus 0:0eef242852bb 197 #define CHKSUM_MASK 0xFE
passionvirus 0:0eef242852bb 198
passionvirus 0:0eef242852bb 199 // Torque CMD
passionvirus 0:0eef242852bb 200 #define TORQUE_FREE 0x00
passionvirus 2:971b320e1e8b 201 #define BREAK_ON 0x40
passionvirus 0:0eef242852bb 202 #define TORQUE_ON 0x60
passionvirus 0:0eef242852bb 203
passionvirus 0:0eef242852bb 204 // Register Size
passionvirus 0:0eef242852bb 205 #define BYTE1 1
passionvirus 0:0eef242852bb 206 #define BYTE2 2
passionvirus 0:0eef242852bb 207
passionvirus 0:0eef242852bb 208 // Jog Set CMD
passionvirus 5:f737e5c70115 209 #define STOP 0x01
passionvirus 5:f737e5c70115 210 #define POS_MODE 0x00
passionvirus 5:f737e5c70115 211 #define TURN_MODE 0x02
passionvirus 5:f737e5c70115 212 #define GLED_ON 0x04
passionvirus 5:f737e5c70115 213 #define BLED_ON 0x08
passionvirus 5:f737e5c70115 214 #define RLED_ON 0x10
passionvirus 0:0eef242852bb 215
passionvirus 0:0eef242852bb 216 //------------------------------------------------------------------------------
passionvirus 3:3aa495712025 217 /** herkulex Servo control class, based packet protocol on a serial
passionvirus 3:3aa495712025 218 *
passionvirus 3:3aa495712025 219 * Example:
passionvirus 3:3aa495712025 220 * @code
passionvirus 4:e6873d42df32 221 * #include "mbed.h"
passionvirus 4:e6873d42df32 222 * #include "herkulex.h"
passionvirus 3:3aa495712025 223 *
passionvirus 4:e6873d42df32 224 * Herkulex sv(p9, p10, 115200);
passionvirus 4:e6873d42df32 225 * DigitalOut led(LED1);
passionvirus 3:3aa495712025 226 *
passionvirus 4:e6873d42df32 227 * int main()
passionvirus 4:e6873d42df32 228 * {
passionvirus 4:e6873d42df32 229 * wait(1);
passionvirus 4:e6873d42df32 230 * sv.setTorque(0xFD, TORQUE_ON);
passionvirus 4:e6873d42df32 231 * while(1)
passionvirus 4:e6873d42df32 232 * {
passionvirus 4:e6873d42df32 233 * led=1;
passionvirus 4:e6873d42df32 234 * sv.movePos(0xFD, 1002, 100, SET_MODE_POS, SET_LED_GREEN_ON);
passionvirus 4:e6873d42df32 235 * wait(3);
passionvirus 4:e6873d42df32 236 *
passionvirus 4:e6873d42df32 237 * led=0;
passionvirus 4:e6873d42df32 238 * sv.movePos(0xFD, 21, 100, SET_MODE_POS, SET_LED_BLUE_ON);
passionvirus 4:e6873d42df32 239 * wait(3);
passionvirus 4:e6873d42df32 240 * }
passionvirus 4:e6873d42df32 241 * }
passionvirus 3:3aa495712025 242 * @endcode
passionvirus 3:3aa495712025 243 */
passionvirus 3:3aa495712025 244 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 245 class Herkulex
passionvirus 0:0eef242852bb 246 {
passionvirus 0:0eef242852bb 247 public:
passionvirus 0:0eef242852bb 248
passionvirus 2:971b320e1e8b 249 /** Create an Herkulex servo object connected to the serial pins and baudrate
passionvirus 2:971b320e1e8b 250 *
passionvirus 2:971b320e1e8b 251 * @param tx Transmit pin.
passionvirus 2:971b320e1e8b 252 * @param rx Receive pin.
passionvirus 2:971b320e1e8b 253 * @param baudRate The serial tx/rx speed.
passionvirus 2:971b320e1e8b 254 */
passionvirus 0:0eef242852bb 255 Herkulex(PinName tx, PinName rx, uint32_t baudRate);
passionvirus 2:971b320e1e8b 256
passionvirus 2:971b320e1e8b 257 /** Destroy an Herkulex servo object
passionvirus 2:971b320e1e8b 258 */
passionvirus 0:0eef242852bb 259 ~Herkulex();
passionvirus 0:0eef242852bb 260
passionvirus 2:971b320e1e8b 261 /** Transmit packet datas
passionvirus 2:971b320e1e8b 262 *
passionvirus 2:971b320e1e8b 263 * @param packetSize The packet size.
passionvirus 2:971b320e1e8b 264 * @param data The transmit packet data array.
passionvirus 2:971b320e1e8b 265 */
passionvirus 0:0eef242852bb 266 void txPacket(uint8_t packetSize, uint8_t* data);
passionvirus 0:0eef242852bb 267
passionvirus 6:1dacff31b77a 268 /** Receive packet datas
passionvirus 6:1dacff31b77a 269 *
passionvirus 6:1dacff31b77a 270 * @param packetSize The packet size.
passionvirus 6:1dacff31b77a 271 * @param data The receive packet data array.
passionvirus 6:1dacff31b77a 272 */
passionvirus 6:1dacff31b77a 273 void rxPacket(uint8_t packetSize, uint8_t* data);
passionvirus 6:1dacff31b77a 274
passionvirus 5:f737e5c70115 275 /** Clear error status
passionvirus 5:f737e5c70115 276 *
passionvirus 5:f737e5c70115 277 * @param id The herkulex servo ID.
passionvirus 5:f737e5c70115 278 */
passionvirus 5:f737e5c70115 279 void clear(uint8_t id);
passionvirus 5:f737e5c70115 280
passionvirus 5:f737e5c70115 281 /** Set torque setting
passionvirus 2:971b320e1e8b 282 *
passionvirus 2:971b320e1e8b 283 * @param id The herkulex servo ID.
passionvirus 6:1dacff31b77a 284 * @param cmdTorue The Command for setting of torque (TORQUE_FREE 0x00, BREAK_ON 0x40, TORQUE_ON 0x60)
passionvirus 2:971b320e1e8b 285 */
passionvirus 0:0eef242852bb 286 void setTorque(uint8_t id, uint8_t cmdTorue);
passionvirus 0:0eef242852bb 287
passionvirus 6:1dacff31b77a 288 /** Position Control
passionvirus 2:971b320e1e8b 289 *
passionvirus 2:971b320e1e8b 290 * @param id The herkulex servo ID.
passionvirus 5:f737e5c70115 291 * @param position The goal position of herkulex servo.
passionvirus 2:971b320e1e8b 292 * @param playtime Time to target position.
passionvirus 6:1dacff31b77a 293 * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
passionvirus 2:971b320e1e8b 294 */
passionvirus 6:1dacff31b77a 295 void positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED);
passionvirus 5:f737e5c70115 296
passionvirus 6:1dacff31b77a 297 /** Velocity Control
passionvirus 5:f737e5c70115 298 *
passionvirus 5:f737e5c70115 299 * @param id The herkulex servo ID.
passionvirus 5:f737e5c70115 300 * @param speed The goal position of herkulex servo.
passionvirus 6:1dacff31b77a 301 * @param setLED Select LED and on/off controll (GLED_ON 0x00,BLED_ON 0x08, RLED_ON 0x10)
passionvirus 5:f737e5c70115 302 */
passionvirus 6:1dacff31b77a 303 void velocityControl(uint8_t id, int16_t speed,uint8_t setLED);
passionvirus 5:f737e5c70115 304
passionvirus 6:1dacff31b77a 305 /** Get Status
passionvirus 6:1dacff31b77a 306 *
passionvirus 6:1dacff31b77a 307 * @param id The herkulex servo ID.
passionvirus 6:1dacff31b77a 308 * @return -1 is getStatus failed. other is servo`s status error value.
passionvirus 6:1dacff31b77a 309 */
passionvirus 6:1dacff31b77a 310 int8_t getStatus(uint8_t id);
passionvirus 6:1dacff31b77a 311
passionvirus 6:1dacff31b77a 312 /** Get Position
passionvirus 6:1dacff31b77a 313 *
passionvirus 6:1dacff31b77a 314 * @param id The herkulex servo ID.
passionvirus 6:1dacff31b77a 315 * @return -1 is getPos failed. other is servo's current position.
passionvirus 6:1dacff31b77a 316 */
passionvirus 6:1dacff31b77a 317 int16_t getPos(uint8_t id);
passionvirus 5:f737e5c70115 318
passionvirus 0:0eef242852bb 319 private :
passionvirus 0:0eef242852bb 320
passionvirus 2:971b320e1e8b 321 /** PC serial connection used in debug mode.
passionvirus 2:971b320e1e8b 322 */
passionvirus 0:0eef242852bb 323 Serial *pc;
passionvirus 2:971b320e1e8b 324
passionvirus 2:971b320e1e8b 325 /** Serial connection (mbed <-> herkulex).
passionvirus 2:971b320e1e8b 326 */
passionvirus 0:0eef242852bb 327 Serial *txd, *rxd;
passionvirus 0:0eef242852bb 328 };
passionvirus 0:0eef242852bb 329
passionvirus 0:0eef242852bb 330 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 331 #endif // MBED_HERKULEX_H
passionvirus 0:0eef242852bb 332
passionvirus 0:0eef242852bb 333 //------------------------------------------------------------------------------