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@1:874d1f42989c, 2013-01-14 (annotated)

Committer:
passionvirus
Date:
Mon Jan 14 06:46:31 2013 +0000
Revision:
1:874d1f42989c
Parent:
0:0eef242852bb
Child:
2:971b320e1e8b
change licence

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 0:0eef242852bb 147 #define CMD_ACK_MASK 0x40 // ACK Packet CMD is Request Packet CMD + 0x40
passionvirus 0:0eef242852bb 148
passionvirus 0:0eef242852bb 149 #define CMD_EEP_WRITE_ACK (CMD_EEP_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 150 #define CMD_EEP_READ_ACK (CMD_EEP_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 151 #define CMD_RAM_WRITE_ACK (CMD_RAM_WRITE|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 152 #define CMD_RAM_READ_ACK (CMD_RAM_READ|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 153 #define CMD_I_JOG_ACK (CMD_I_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 154 #define CMD_S_JOG_ACK (CMD_S_JOG|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 155 #define CMD_STAT_ACK (CMD_STAT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 156 #define CMD_ROLLBACK_ACK (CMD_ROLLBACK|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 157 #define CMD_REBOOT_ACK (CMD_REBOOT|CMD_ACK_MASK)
passionvirus 0:0eef242852bb 158
passionvirus 0:0eef242852bb 159 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 160 // Header
passionvirus 0:0eef242852bb 161 #define HEADER 0xFF
passionvirus 0:0eef242852bb 162
passionvirus 0:0eef242852bb 163 // Size
passionvirus 0:0eef242852bb 164 #define MIN_PACKET_SIZE 7
passionvirus 0:0eef242852bb 165 #define MIN_ACK_PACKET_SIZE 9
passionvirus 0:0eef242852bb 166 #define WRITE_PACKET_SIZE 13
passionvirus 0:0eef242852bb 167 #define MAX_PACKET_SIZE 223
passionvirus 0:0eef242852bb 168 #define MAX_DATA_SIZE (MAX_PACKET_SIZE-MIN_PACKET_SIZE)
passionvirus 0:0eef242852bb 169
passionvirus 0:0eef242852bb 170 // ID
passionvirus 0:0eef242852bb 171 #define MAX_PID 0xFD
passionvirus 0:0eef242852bb 172 #define DEFAULT_ID 0xFD
passionvirus 0:0eef242852bb 173 #define MAX_ID 0xFD
passionvirus 0:0eef242852bb 174 #define BROADCAST_ID 0xFE
passionvirus 0:0eef242852bb 175
passionvirus 0:0eef242852bb 176 // Checksum
passionvirus 0:0eef242852bb 177 #define CHKSUM_MASK 0xFE
passionvirus 0:0eef242852bb 178
passionvirus 0:0eef242852bb 179 // Torque CMD
passionvirus 0:0eef242852bb 180 #define TORQUE_FREE 0x00
passionvirus 0:0eef242852bb 181 #define BREAK ON 0x40
passionvirus 0:0eef242852bb 182 #define TORQUE_ON 0x60
passionvirus 0:0eef242852bb 183
passionvirus 0:0eef242852bb 184 // Register Size
passionvirus 0:0eef242852bb 185 #define BYTE1 1
passionvirus 0:0eef242852bb 186 #define BYTE2 2
passionvirus 0:0eef242852bb 187
passionvirus 0:0eef242852bb 188 // Jog Set CMD
passionvirus 0:0eef242852bb 189 #define SET_STOP 0x01
passionvirus 0:0eef242852bb 190 #define SET_MODE_POS 0x00
passionvirus 0:0eef242852bb 191 #define SET_MODE_TURN 0x02
passionvirus 0:0eef242852bb 192 #define SET_LED_GREEN_ON 0x04
passionvirus 0:0eef242852bb 193 #define SET_LED_BLUE_ON 0x08
passionvirus 0:0eef242852bb 194 #define SET_LED_RED_ON 0x10
passionvirus 0:0eef242852bb 195
passionvirus 0:0eef242852bb 196
passionvirus 0:0eef242852bb 197 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 198
passionvirus 0:0eef242852bb 199 class Herkulex
passionvirus 0:0eef242852bb 200 {
passionvirus 0:0eef242852bb 201 public:
passionvirus 0:0eef242852bb 202
passionvirus 0:0eef242852bb 203 Herkulex(PinName tx, PinName rx, uint32_t baudRate);
passionvirus 0:0eef242852bb 204 ~Herkulex();
passionvirus 0:0eef242852bb 205
passionvirus 0:0eef242852bb 206 void txPacket(uint8_t packetSize, uint8_t* data);
passionvirus 0:0eef242852bb 207
passionvirus 0:0eef242852bb 208 void setTorque(uint8_t id, uint8_t cmdTorue);
passionvirus 0:0eef242852bb 209
passionvirus 0:0eef242852bb 210 void movePos(uint8_t id, uint16_t pos, uint8_t playtime, uint8_t setMode, uint8_t setLED);
passionvirus 0:0eef242852bb 211
passionvirus 0:0eef242852bb 212 private :
passionvirus 0:0eef242852bb 213
passionvirus 0:0eef242852bb 214 Serial *pc;
passionvirus 0:0eef242852bb 215 Serial *txd, *rxd;
passionvirus 0:0eef242852bb 216 };
passionvirus 0:0eef242852bb 217
passionvirus 0:0eef242852bb 218 //------------------------------------------------------------------------------
passionvirus 0:0eef242852bb 219 #endif // MBED_HERKULEX_H
passionvirus 0:0eef242852bb 220
passionvirus 0:0eef242852bb 221 //------------------------------------------------------------------------------