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@0:0eef242852bb, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 06:06:30 2013 +0000
Revision:
0:0eef242852bb
Child:
1:874d1f42989c
new lib for herkulex servo

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 0:0eef242852bb 4 * Copyright (c) 2012-2013, Yoonseok Pyo
passionvirus 0:0eef242852bb 5 * All rights reserved.
passionvirus 0:0eef242852bb 6 *
passionvirus 0:0eef242852bb 7 * New BSD License
passionvirus 0:0eef242852bb 8 *
passionvirus 0:0eef242852bb 9 * Redistribution and use in source and binary forms, with or without
passionvirus 0:0eef242852bb 10 * modification, are permitted provided that the following conditions are met:
passionvirus 0:0eef242852bb 11 * * Redistributions of source code must retain the above copyright
passionvirus 0:0eef242852bb 12 * notice, this list of conditions and the following disclaimer.
passionvirus 0:0eef242852bb 13 * * Redistributions in binary form must reproduce the above copyright
passionvirus 0:0eef242852bb 14 * notice, this list of conditions and the following disclaimer in the
passionvirus 0:0eef242852bb 15 * documentation and/or other materials provided with the distribution.
passionvirus 0:0eef242852bb 16 * * Neither the name of the <organization> nor the
passionvirus 0:0eef242852bb 17 * names of its contributors may be used to endorse or promote products
passionvirus 0:0eef242852bb 18 * derived from this software without specific prior written permission.
passionvirus 0:0eef242852bb 19 *
passionvirus 0:0eef242852bb 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
passionvirus 0:0eef242852bb 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
passionvirus 0:0eef242852bb 22 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
passionvirus 0:0eef242852bb 23 * DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
passionvirus 0:0eef242852bb 24 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
passionvirus 0:0eef242852bb 25 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
passionvirus 0:0eef242852bb 26 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
passionvirus 0:0eef242852bb 27 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
passionvirus 0:0eef242852bb 28 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
passionvirus 0:0eef242852bb 29 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
passionvirus 0:0eef242852bb 30 */
passionvirus 0:0eef242852bb 31 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 32 #ifndef MBED_HERKULEX_H
passionvirus 0:0eef242852bb 33 #define MBED_HERKULEX_H
passionvirus 0:0eef242852bb 34
passionvirus 0:0eef242852bb 35 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 36 #include "mbed.h"
passionvirus 0:0eef242852bb 37
passionvirus 0:0eef242852bb 38 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 39 #define HERKULEX_DEBUG
passionvirus 0:0eef242852bb 40
passionvirus 0:0eef242852bb 41 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 42 // Herkulex ROM Register
passionvirus 0:0eef242852bb 43 #define ROM_MODEL_NO1 0
passionvirus 0:0eef242852bb 44 #define ROM_MODEL_NO2 1
passionvirus 0:0eef242852bb 45 #define ROM_VERSION1 2
passionvirus 0:0eef242852bb 46 #define ROM_VERSION2 3
passionvirus 0:0eef242852bb 47 #define ROM_BAUD_RATE 4
passionvirus 0:0eef242852bb 48 #define ROM_RESERVED5 5
passionvirus 0:0eef242852bb 49 #define ROM_ID 6
passionvirus 0:0eef242852bb 50 #define ROM_ACK_POLICY 7
passionvirus 0:0eef242852bb 51 #define ROM_ALARM_LED_POLICY 8
passionvirus 0:0eef242852bb 52 #define ROM_TORQUE_POLICY 9
passionvirus 0:0eef242852bb 53 #define ROM_RESERVED10 10
passionvirus 0:0eef242852bb 54 #define ROM_MAX_TEMPERATURE 11
passionvirus 0:0eef242852bb 55 #define ROM_MIN_VOLTAGE 12
passionvirus 0:0eef242852bb 56 #define ROM_MAX_VOLTAGE 13
passionvirus 0:0eef242852bb 57 #define ROM_ACCELERATION_RATIO 14
passionvirus 0:0eef242852bb 58 #define ROM_MAX_ACCELERATION_TIME 15
passionvirus 0:0eef242852bb 59 #define ROM_DEAD_ZONE 16
passionvirus 0:0eef242852bb 60 #define ROM_SATURATOR_OFFSET 17
passionvirus 0:0eef242852bb 61 #define ROM_SATURATOR_SLOPE 18 // 2Byte
passionvirus 0:0eef242852bb 62 #define ROM_PWM_OFFSET 20
passionvirus 0:0eef242852bb 63 #define ROM_MIN_PWM 21
passionvirus 0:0eef242852bb 64 #define ROM_MAX_PWM 22 // 2Byte
passionvirus 0:0eef242852bb 65 #define ROM_OVERLOAD_PWM_THRESHOLD 24 // 2Byte
passionvirus 0:0eef242852bb 66 #define ROM_MIN_POSITION 26 // 2Byte
passionvirus 0:0eef242852bb 67 #define ROM_MAX_POSITION 28 // 2Byte
passionvirus 0:0eef242852bb 68 #define ROM_POSITION_KP 30 // 2Byte
passionvirus 0:0eef242852bb 69 #define ROM_POSITION_KD 32 // 2Byte
passionvirus 0:0eef242852bb 70 #define ROM_POSITION_KI 34 // 2Byte
passionvirus 0:0eef242852bb 71 #define ROM_POSITION_FEEDFORWARD_1ST_GAIN 36 // 2Byte
passionvirus 0:0eef242852bb 72 #define ROM_POSITION FEEDFORWARD_2ND_GAIN 38 // 2Byte
passionvirus 0:0eef242852bb 73 #define ROM_RESERVED40 40 // 2Byte
passionvirus 0:0eef242852bb 74 #define ROM_RESERVED42 42 // 2Byte
passionvirus 0:0eef242852bb 75 #define ROM_LED_BLINK_PERIOD 44
passionvirus 0:0eef242852bb 76 #define ROM_ADC_FAULT_CHECK_PERIOD 45
passionvirus 0:0eef242852bb 77 #define ROM_PACKET_GARBAGE_CHECK_PERIOD 46
passionvirus 0:0eef242852bb 78 #define ROM_STOP_DETECTION_PERIOD 47
passionvirus 0:0eef242852bb 79 #define ROM_OVERLOAD_DETECTION_PERIOD 48
passionvirus 0:0eef242852bb 80 #define ROM_STOP_THRESHOLD 49
passionvirus 0:0eef242852bb 81 #define ROM_INPOSITION_MARGIN 50
passionvirus 0:0eef242852bb 82 #define ROM_RESERVED51 51
passionvirus 0:0eef242852bb 83 #define ROM_RESERVED52 52
passionvirus 0:0eef242852bb 84 #define ROM_CALIBRATION_DIFFERENCE 53
passionvirus 0:0eef242852bb 85
passionvirus 0:0eef242852bb 86 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 87 // Herkulex RAM Register
passionvirus 0:0eef242852bb 88 #define RAM_ID 0
passionvirus 0:0eef242852bb 89 #define RAM_ACK_POLICY 1
passionvirus 0:0eef242852bb 90 #define RAM_ALARM_LED_POLICY 2
passionvirus 0:0eef242852bb 91 #define RAM_TORQUE_POLICY 3
passionvirus 0:0eef242852bb 92 #define RAM_RESERVED4 4
passionvirus 0:0eef242852bb 93 #define RAM_MAX_TEMPERATURE 5
passionvirus 0:0eef242852bb 94 #define RAM_MIN_VOLTAGE 6
passionvirus 0:0eef242852bb 95 #define RAM_MAX_VOLTAGE 7
passionvirus 0:0eef242852bb 96 #define RAM_ACCELERATION_RATIO 8
passionvirus 0:0eef242852bb 97 #define RAM_MAX_ACCELERATION 9
passionvirus 0:0eef242852bb 98 #define RAM_DEAD_ZONE 10
passionvirus 0:0eef242852bb 99 #define RAM_SATURATOR_OFFSET 11
passionvirus 0:0eef242852bb 100 #define RAM_SATURATOR_SLOPE 12 // 2Byte
passionvirus 0:0eef242852bb 101 #define RAM_PWM_OFFSET 14
passionvirus 0:0eef242852bb 102 #define RAM_MIN_PWM 15
passionvirus 0:0eef242852bb 103 #define RAM_MAX_PWM 16 // 2Byte
passionvirus 0:0eef242852bb 104 #define RAM_OVERLOAD_PWM_THRESHOLD 18 // 2Byte
passionvirus 0:0eef242852bb 105 #define RAM_MIN_POSITION 20 // 2Byte
passionvirus 0:0eef242852bb 106 #define RAM_MAX_POSITION 22 // 2Byte
passionvirus 0:0eef242852bb 107 #define RAM_POSITION_KP 24 // 2Byte
passionvirus 0:0eef242852bb 108 #define RAM_POSITION_KD 26 // 2Byte
passionvirus 0:0eef242852bb 109 #define RAM_POSITION_KI 28 // 2Byte
passionvirus 0:0eef242852bb 110 #define RAM_POSITION_FEEDFORWARD_1ST_GAIN 30 // 2Byte
passionvirus 0:0eef242852bb 111 #define RAM_POSITION_FEEDFORWARD 2ND GAIN 32 // 2Byte
passionvirus 0:0eef242852bb 112 #define RAM_RESERVED34 34 // 2Byte
passionvirus 0:0eef242852bb 113 #define RAM_RESERVED36 36 // 2Byte
passionvirus 0:0eef242852bb 114 #define RAM_LED_BLINK_PERIOD 38
passionvirus 0:0eef242852bb 115 #define RAM_ADC_FAULT_DETECTION_PERIOD 39
passionvirus 0:0eef242852bb 116 #define RAM_PACKET_GARBAGE_DETECTION_PERIOD 40
passionvirus 0:0eef242852bb 117 #define RAM_STOP_DETECTION_PERIOD 41
passionvirus 0:0eef242852bb 118 #define RAM_OVERLOAD_DETECTION_PERIOD 42
passionvirus 0:0eef242852bb 119 #define RAM_STOP_THRESHOLD 43
passionvirus 0:0eef242852bb 120 #define RAM_INPOSITION_MARGIN 44
passionvirus 0:0eef242852bb 121 #define RAM_RESERVED45 45
passionvirus 0:0eef242852bb 122 #define RAM_RESERVED46 46
passionvirus 0:0eef242852bb 123 #define RAM_CALIBRATION_DIFFERENCE 47
passionvirus 0:0eef242852bb 124 #define RAM_STATUS_ERROR 48
passionvirus 0:0eef242852bb 125 #define RAM_STATUS_DETAIL 49
passionvirus 0:0eef242852bb 126 #define RAM_RESERVED50 50
passionvirus 0:0eef242852bb 127 #define RAM_RESERVED51 51
passionvirus 0:0eef242852bb 128 #define RAM_TORQUE_CONTROL 52
passionvirus 0:0eef242852bb 129 #define RAM_LED_CONTROL 53
passionvirus 0:0eef242852bb 130 #define RAM_VOLTAGE 54
passionvirus 0:0eef242852bb 131 #define RAM_TEMPERATURE 55
passionvirus 0:0eef242852bb 132 #define RAM_CURRENT_CONTROL_MODE 56
passionvirus 0:0eef242852bb 133 #define RAM_TICK 57
passionvirus 0:0eef242852bb 134 #define RAM_CALIBRATED_POSITION 58 // 2Byte
passionvirus 0:0eef242852bb 135 #define RAM_ABSOLUTE_POSITION 60 // 2Byte
passionvirus 0:0eef242852bb 136 #define RAM_DIFFERENTIAL_POSITION 62 // 2Byte
passionvirus 0:0eef242852bb 137 #define RAM_PWM 64 // 2Byte
passionvirus 0:0eef242852bb 138 #define RAM_RESERVED66 66 // 2Byte
passionvirus 0:0eef242852bb 139 #define RAM_ABSOLUTE_GOAL_POSITION 68 // 2Byte
passionvirus 0:0eef242852bb 140 #define RAM_ABSOLUTE_DESIRED_TRAJECTORY_POSITION 70 // 2Byte
passionvirus 0:0eef242852bb 141 #define RAM_DESIRED_VELOCITY 72 // 2Byte
passionvirus 0:0eef242852bb 142
passionvirus 0:0eef242852bb 143 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 144 // Request Packet [To Servo Module]
passionvirus 0:0eef242852bb 145 #define CMD_ROM_WRITE 0x01 // Write Length number of values to EEP Register Address
passionvirus 0:0eef242852bb 146 #define CMD_ROM_READ 0x02 // Request Length number of values from EEP Register Address
passionvirus 0:0eef242852bb 147 #define CMD_RAM_WRITE 0x03 // Write Length number of values to RAM Register Address
passionvirus 0:0eef242852bb 148 #define CMD_RAM_READ 0x04 // Request Lenght number of values from RAM Register Address
passionvirus 0:0eef242852bb 149 #define CMD_I_JOG 0x05 // Able to send JOG command to maximum 43 servos (operate timing of individual Servo)
passionvirus 0:0eef242852bb 150 #define CMD_S_JOG 0x06 // Able to send JOG command to maximum 53 servos (operate simultaneously at same time)
passionvirus 0:0eef242852bb 151 #define CMD_STAT 0x07 // Status Error, Status Detail request
passionvirus 0:0eef242852bb 152 #define CMD_ROLLBACK 0x08 // Change all EEP Regsters to Factory Default value
passionvirus 0:0eef242852bb 153 #define CMD_REBOOT 0x09 // Request Reboot
passionvirus 0:0eef242852bb 154
passionvirus 0:0eef242852bb 155 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 156 // ACK Packet [To Controller(ACK)]
passionvirus 0:0eef242852bb 157 #define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40
passionvirus 0:0eef242852bb 158
passionvirus 0:0eef242852bb 159 #define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 160 #define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 161 #define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 162 #define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 163 #define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 164 #define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 165 #define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 166 #define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 167 #define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 168
passionvirus 0:0eef242852bb 169 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 170 // Header
passionvirus 0:0eef242852bb 171 #define HEADER 0xFF
passionvirus 0:0eef242852bb 172
passionvirus 0:0eef242852bb 173 // Size
passionvirus 0:0eef242852bb 174 #define MIN_PACKET_SIZE 7
passionvirus 0:0eef242852bb 175 #define MIN_ACK_PACKET_SIZE 9
passionvirus 0:0eef242852bb 176 #define WRITE_PACKET_SIZE 13
passionvirus 0:0eef242852bb 177 #define MAX_PACKET_SIZE 223
passionvirus 0:0eef242852bb 178 #define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
passionvirus 0:0eef242852bb 179
passionvirus 0:0eef242852bb 180 // ID
passionvirus 0:0eef242852bb 181 #define MAX_PID 0xFD
passionvirus 0:0eef242852bb 182 #define DEFAULT_ID 0xFD
passionvirus 0:0eef242852bb 183 #define MAX_ID 0xFD
passionvirus 0:0eef242852bb 184 #define BROADCAST_ID 0xFE
passionvirus 0:0eef242852bb 185
passionvirus 0:0eef242852bb 186 // Checksum
passionvirus 0:0eef242852bb 187 #define CHKSUM_MASK 0xFE
passionvirus 0:0eef242852bb 188
passionvirus 0:0eef242852bb 189 // Torque CMD
passionvirus 0:0eef242852bb 190 #define TORQUE_FREE 0x00
passionvirus 0:0eef242852bb 191 #define BREAK ON 0x40
passionvirus 0:0eef242852bb 192 #define TORQUE_ON 0x60
passionvirus 0:0eef242852bb 193
passionvirus 0:0eef242852bb 194 // Register Size
passionvirus 0:0eef242852bb 195 #define BYTE1 1
passionvirus 0:0eef242852bb 196 #define BYTE2 2
passionvirus 0:0eef242852bb 197
passionvirus 0:0eef242852bb 198 // Jog Set CMD
passionvirus 0:0eef242852bb 199 #define SET_STOP 0x01
passionvirus 0:0eef242852bb 200 #define SET_MODE_POS 0x00
passionvirus 0:0eef242852bb 201 #define SET_MODE_TURN 0x02
passionvirus 0:0eef242852bb 202 #define SET_LED_GREEN_ON 0x04
passionvirus 0:0eef242852bb 203 #define SET_LED_BLUE_ON 0x08
passionvirus 0:0eef242852bb 204 #define SET_LED_RED_ON 0x10
passionvirus 0:0eef242852bb 205
passionvirus 0:0eef242852bb 206
passionvirus 0:0eef242852bb 207 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 208
passionvirus 0:0eef242852bb 209 class Herkulex
passionvirus 0:0eef242852bb 210 {
passionvirus 0:0eef242852bb 211 public:
passionvirus 0:0eef242852bb 212
passionvirus 0:0eef242852bb 213 Herkulex(PinName tx, PinName rx, uint32_t baudRate);
passionvirus 0:0eef242852bb 214 ~Herkulex();
passionvirus 0:0eef242852bb 215
passionvirus 0:0eef242852bb 216 void txPacket(uint8_t packetSize, uint8_t* data);
passionvirus 0:0eef242852bb 217
passionvirus 0:0eef242852bb 218 void setTorque(uint8_t id, uint8_t cmdTorue);
passionvirus 0:0eef242852bb 219
passionvirus 0:0eef242852bb 220 void movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED);
passionvirus 0:0eef242852bb 221
passionvirus 0:0eef242852bb 222 private :
passionvirus 0:0eef242852bb 223
passionvirus 0:0eef242852bb 224 Serial *pc;
passionvirus 0:0eef242852bb 225 Serial *txd, *rxd;
passionvirus 0:0eef242852bb 226 };
passionvirus 0:0eef242852bb 227
passionvirus 0:0eef242852bb 228 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 229 #endif // MBED_HERKULEX_H
passionvirus 0:0eef242852bb 230
passionvirus 0:0eef242852bb 231 //------------------------------------------------------------------------------