herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
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?
User | Revision | Line number | New 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 | //------------------------------------------------------------------------------ |