herkulex servo control library (fixes velocityControl and positionControl)
Fork of herkulex by
herkulex.cpp@7:fb5f919f2700, 2015-05-31 (annotated)
- Committer:
- XavierBouvard
- Date:
- Sun May 31 15:08:32 2015 +0000
- Revision:
- 7:fb5f919f2700
- Parent:
- 6:1dacff31b77a
fixes herkulex protocole issues
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 | #include "mbed.h" |
passionvirus | 0:0eef242852bb | 23 | #include "herkulex.h" |
passionvirus | 0:0eef242852bb | 24 | |
passionvirus | 0:0eef242852bb | 25 | //------------------------------------------------------------------------------ |
passionvirus | 0:0eef242852bb | 26 | Herkulex::Herkulex(PinName tx, PinName rx, uint32_t baudRate) |
passionvirus | 0:0eef242852bb | 27 | { |
passionvirus | 0:0eef242852bb | 28 | #ifdef HERKULEX_DEBUG |
passionvirus | 0:0eef242852bb | 29 | pc = new Serial(USBTX, USBRX); |
passionvirus | 0:0eef242852bb | 30 | pc->baud(57600); |
passionvirus | 6:1dacff31b77a | 31 | pc->printf("\n\nHerkulex Init!\n"); |
passionvirus | 0:0eef242852bb | 32 | #endif |
passionvirus | 0:0eef242852bb | 33 | |
passionvirus | 0:0eef242852bb | 34 | txd = new Serial(tx, NC); |
passionvirus | 0:0eef242852bb | 35 | rxd = new Serial(NC, rx); |
passionvirus | 0:0eef242852bb | 36 | |
passionvirus | 0:0eef242852bb | 37 | txd->baud(baudRate); |
passionvirus | 0:0eef242852bb | 38 | rxd->baud(baudRate); |
passionvirus | 0:0eef242852bb | 39 | } |
passionvirus | 0:0eef242852bb | 40 | |
passionvirus | 0:0eef242852bb | 41 | //------------------------------------------------------------------------------ |
passionvirus | 0:0eef242852bb | 42 | Herkulex::~Herkulex() |
passionvirus | 0:0eef242852bb | 43 | { |
passionvirus | 0:0eef242852bb | 44 | #ifdef HERKULEX_DEBUG |
passionvirus | 0:0eef242852bb | 45 | if(pc != NULL) |
passionvirus | 0:0eef242852bb | 46 | delete pc; |
passionvirus | 0:0eef242852bb | 47 | #endif |
passionvirus | 0:0eef242852bb | 48 | |
passionvirus | 0:0eef242852bb | 49 | if(txd != NULL) |
passionvirus | 0:0eef242852bb | 50 | delete txd; |
passionvirus | 0:0eef242852bb | 51 | if(rxd != NULL) |
passionvirus | 0:0eef242852bb | 52 | delete rxd; |
passionvirus | 0:0eef242852bb | 53 | } |
passionvirus | 0:0eef242852bb | 54 | |
passionvirus | 0:0eef242852bb | 55 | //------------------------------------------------------------------------------ |
passionvirus | 0:0eef242852bb | 56 | void Herkulex::txPacket(uint8_t packetSize, uint8_t* data) |
passionvirus | 0:0eef242852bb | 57 | { |
passionvirus | 0:0eef242852bb | 58 | #ifdef HERKULEX_DEBUG |
passionvirus | 0:0eef242852bb | 59 | pc->printf("[TX]"); |
passionvirus | 0:0eef242852bb | 60 | #endif |
passionvirus | 0:0eef242852bb | 61 | |
passionvirus | 0:0eef242852bb | 62 | for(uint8_t i = 0; i < packetSize ; i++) |
passionvirus | 0:0eef242852bb | 63 | { |
passionvirus | 0:0eef242852bb | 64 | #ifdef HERKULEX_DEBUG |
passionvirus | 0:0eef242852bb | 65 | pc->printf("%02X ",data[i]); |
passionvirus | 0:0eef242852bb | 66 | #endif |
passionvirus | 0:0eef242852bb | 67 | |
passionvirus | 0:0eef242852bb | 68 | txd->putc(data[i]); |
passionvirus | 0:0eef242852bb | 69 | } |
passionvirus | 0:0eef242852bb | 70 | |
passionvirus | 0:0eef242852bb | 71 | #ifdef HERKULEX_DEBUG |
passionvirus | 0:0eef242852bb | 72 | pc->printf("\n"); |
passionvirus | 0:0eef242852bb | 73 | #endif |
passionvirus | 0:0eef242852bb | 74 | } |
passionvirus | 0:0eef242852bb | 75 | |
passionvirus | 0:0eef242852bb | 76 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 77 | void Herkulex::rxPacket(uint8_t packetSize, uint8_t* data) |
passionvirus | 6:1dacff31b77a | 78 | { |
passionvirus | 6:1dacff31b77a | 79 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 80 | pc->printf("[RX]"); |
passionvirus | 6:1dacff31b77a | 81 | #endif |
passionvirus | 6:1dacff31b77a | 82 | |
passionvirus | 6:1dacff31b77a | 83 | for (uint8_t i=0; i < packetSize; i++) |
passionvirus | 6:1dacff31b77a | 84 | { |
passionvirus | 6:1dacff31b77a | 85 | data[i] = rxd->getc(); |
passionvirus | 6:1dacff31b77a | 86 | |
passionvirus | 6:1dacff31b77a | 87 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 88 | pc->printf("%02X ",data[i]); |
passionvirus | 6:1dacff31b77a | 89 | #endif |
passionvirus | 6:1dacff31b77a | 90 | } |
passionvirus | 6:1dacff31b77a | 91 | |
passionvirus | 6:1dacff31b77a | 92 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 93 | pc->printf("\n"); |
passionvirus | 6:1dacff31b77a | 94 | #endif |
passionvirus | 6:1dacff31b77a | 95 | } |
passionvirus | 6:1dacff31b77a | 96 | |
passionvirus | 6:1dacff31b77a | 97 | //------------------------------------------------------------------------------ |
passionvirus | 5:f737e5c70115 | 98 | void Herkulex::clear(uint8_t id) |
passionvirus | 5:f737e5c70115 | 99 | { |
passionvirus | 5:f737e5c70115 | 100 | uint8_t txBuf[11]; |
passionvirus | 5:f737e5c70115 | 101 | |
passionvirus | 5:f737e5c70115 | 102 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 5:f737e5c70115 | 103 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 5:f737e5c70115 | 104 | txBuf[2] = MIN_PACKET_SIZE + 4; // Packet Size |
passionvirus | 5:f737e5c70115 | 105 | txBuf[3] = id; // Servo ID |
passionvirus | 5:f737e5c70115 | 106 | txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) |
passionvirus | 5:f737e5c70115 | 107 | txBuf[5] = 0; // Checksum1 |
passionvirus | 5:f737e5c70115 | 108 | txBuf[6] = 0; // Checksum2 |
passionvirus | 5:f737e5c70115 | 109 | txBuf[7] = RAM_STATUS_ERROR; // Address 48 |
passionvirus | 5:f737e5c70115 | 110 | txBuf[8] = BYTE2; // Length |
passionvirus | 5:f737e5c70115 | 111 | txBuf[9] = 0; // Clear RAM_STATUS_ERROR |
passionvirus | 5:f737e5c70115 | 112 | txBuf[10]= 0; // Clear RAM_STATUS_DETAIL |
passionvirus | 5:f737e5c70115 | 113 | |
passionvirus | 5:f737e5c70115 | 114 | // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE |
passionvirus | 5:f737e5c70115 | 115 | // Checksum2 = (~Checksum1)&0xFE |
passionvirus | 5:f737e5c70115 | 116 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]) & 0xFE; |
passionvirus | 5:f737e5c70115 | 117 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 5:f737e5c70115 | 118 | |
passionvirus | 5:f737e5c70115 | 119 | // send packet (mbed -> herkulex) |
passionvirus | 5:f737e5c70115 | 120 | txPacket(11, txBuf); |
passionvirus | 5:f737e5c70115 | 121 | } |
passionvirus | 5:f737e5c70115 | 122 | |
passionvirus | 5:f737e5c70115 | 123 | //------------------------------------------------------------------------------ |
passionvirus | 0:0eef242852bb | 124 | void Herkulex::setTorque(uint8_t id, uint8_t cmdTorue) |
passionvirus | 0:0eef242852bb | 125 | { |
passionvirus | 0:0eef242852bb | 126 | uint8_t txBuf[10]; |
passionvirus | 0:0eef242852bb | 127 | |
passionvirus | 0:0eef242852bb | 128 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 0:0eef242852bb | 129 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 0:0eef242852bb | 130 | txBuf[2] = MIN_PACKET_SIZE + 3; // Packet Size |
passionvirus | 0:0eef242852bb | 131 | txBuf[3] = id; // Servo ID |
passionvirus | 0:0eef242852bb | 132 | txBuf[4] = CMD_RAM_WRITE; // Command Ram Write (0x03) |
passionvirus | 0:0eef242852bb | 133 | txBuf[5] = 0; // Checksum1 |
passionvirus | 0:0eef242852bb | 134 | txBuf[6] = 0; // Checksum2 |
passionvirus | 0:0eef242852bb | 135 | txBuf[7] = RAM_TORQUE_CONTROL; // Address 52 |
passionvirus | 0:0eef242852bb | 136 | txBuf[8] = BYTE1; // Length |
passionvirus | 6:1dacff31b77a | 137 | txBuf[9] = cmdTorue; // Torque ON |
passionvirus | 0:0eef242852bb | 138 | |
passionvirus | 0:0eef242852bb | 139 | // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE |
passionvirus | 0:0eef242852bb | 140 | // Checksum2 = (~Checksum1)&0xFE |
passionvirus | 0:0eef242852bb | 141 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]) & 0xFE; |
passionvirus | 0:0eef242852bb | 142 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 0:0eef242852bb | 143 | |
passionvirus | 0:0eef242852bb | 144 | // send packet (mbed -> herkulex) |
passionvirus | 0:0eef242852bb | 145 | txPacket(10, txBuf); |
passionvirus | 0:0eef242852bb | 146 | } |
passionvirus | 0:0eef242852bb | 147 | |
passionvirus | 0:0eef242852bb | 148 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 149 | void Herkulex::positionControl(uint8_t id, uint16_t position, uint8_t playtime, uint8_t setLED) |
passionvirus | 0:0eef242852bb | 150 | { |
passionvirus | 5:f737e5c70115 | 151 | if (position > 1023) return; |
passionvirus | 0:0eef242852bb | 152 | if (playtime > 255) return; |
passionvirus | 0:0eef242852bb | 153 | |
passionvirus | 0:0eef242852bb | 154 | uint8_t txBuf[12]; |
passionvirus | 0:0eef242852bb | 155 | |
passionvirus | 0:0eef242852bb | 156 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 0:0eef242852bb | 157 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 0:0eef242852bb | 158 | txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size |
XavierBouvard | 7:fb5f919f2700 | 159 | txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253) |
passionvirus | 0:0eef242852bb | 160 | txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) |
passionvirus | 0:0eef242852bb | 161 | txBuf[5] = 0; // Checksum1 |
passionvirus | 0:0eef242852bb | 162 | txBuf[6] = 0; // Checksum2 |
passionvirus | 0:0eef242852bb | 163 | txBuf[7] = playtime; // Playtime |
passionvirus | 5:f737e5c70115 | 164 | txBuf[8] = position & 0x00FF; // Position (LSB, Least Significant Bit) |
passionvirus | 5:f737e5c70115 | 165 | txBuf[9] =(position & 0xFF00) >> 8;// position (MSB, Most Significanct Bit) |
passionvirus | 5:f737e5c70115 | 166 | txBuf[10] = POS_MODE | setLED; // Pos Mode and LED on/off |
passionvirus | 5:f737e5c70115 | 167 | txBuf[11] = id; // Servo ID |
passionvirus | 5:f737e5c70115 | 168 | |
passionvirus | 5:f737e5c70115 | 169 | // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE |
passionvirus | 5:f737e5c70115 | 170 | // Checksum2 = (~Checksum1)&0xFE |
passionvirus | 5:f737e5c70115 | 171 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; |
passionvirus | 5:f737e5c70115 | 172 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 5:f737e5c70115 | 173 | |
passionvirus | 5:f737e5c70115 | 174 | // send packet (mbed -> herkulex) |
passionvirus | 5:f737e5c70115 | 175 | txPacket(12, txBuf); |
passionvirus | 5:f737e5c70115 | 176 | } |
passionvirus | 5:f737e5c70115 | 177 | |
passionvirus | 5:f737e5c70115 | 178 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 179 | void Herkulex::velocityControl(uint8_t id, int16_t speed, uint8_t setLED) |
passionvirus | 5:f737e5c70115 | 180 | { |
passionvirus | 5:f737e5c70115 | 181 | if (speed > 1023 || speed < -1023) return; |
passionvirus | 5:f737e5c70115 | 182 | |
passionvirus | 5:f737e5c70115 | 183 | uint8_t txBuf[12]; |
passionvirus | 5:f737e5c70115 | 184 | |
passionvirus | 5:f737e5c70115 | 185 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 5:f737e5c70115 | 186 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 5:f737e5c70115 | 187 | txBuf[2] = MIN_PACKET_SIZE + 5; // Packet Size |
XavierBouvard | 7:fb5f919f2700 | 188 | txBuf[3] = id; // pID is total number of servos in the network (0 ~ 253) |
passionvirus | 5:f737e5c70115 | 189 | txBuf[4] = CMD_S_JOG; // Command S JOG (0x06) |
passionvirus | 5:f737e5c70115 | 190 | txBuf[5] = 0; // Checksum1 |
passionvirus | 5:f737e5c70115 | 191 | txBuf[6] = 0; // Checksum2 |
passionvirus | 5:f737e5c70115 | 192 | txBuf[7] = 0; // Playtime, unmeaningful in turn mode |
XavierBouvard | 7:fb5f919f2700 | 193 | |
XavierBouvard | 7:fb5f919f2700 | 194 | int16_t temp; |
XavierBouvard | 7:fb5f919f2700 | 195 | if (speed < 0) { |
XavierBouvard | 7:fb5f919f2700 | 196 | temp = speed * -1; |
XavierBouvard | 7:fb5f919f2700 | 197 | temp |= 0x4000; |
XavierBouvard | 7:fb5f919f2700 | 198 | } else { |
XavierBouvard | 7:fb5f919f2700 | 199 | temp = speed; |
XavierBouvard | 7:fb5f919f2700 | 200 | } |
XavierBouvard | 7:fb5f919f2700 | 201 | |
XavierBouvard | 7:fb5f919f2700 | 202 | txBuf[8] = temp & 0x00FF; // Speed (LSB, Least Significant Bit) |
XavierBouvard | 7:fb5f919f2700 | 203 | txBuf[9] =(temp & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit) |
passionvirus | 5:f737e5c70115 | 204 | txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off |
passionvirus | 0:0eef242852bb | 205 | txBuf[11] = id; // Servo ID |
passionvirus | 0:0eef242852bb | 206 | |
passionvirus | 0:0eef242852bb | 207 | // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE |
passionvirus | 0:0eef242852bb | 208 | // Checksum2 = (~Checksum1)&0xFE |
passionvirus | 0:0eef242852bb | 209 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE; |
passionvirus | 0:0eef242852bb | 210 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 0:0eef242852bb | 211 | |
passionvirus | 0:0eef242852bb | 212 | // send packet (mbed -> herkulex) |
passionvirus | 0:0eef242852bb | 213 | txPacket(12, txBuf); |
passionvirus | 0:0eef242852bb | 214 | } |
passionvirus | 0:0eef242852bb | 215 | |
passionvirus | 0:0eef242852bb | 216 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 217 | int8_t Herkulex::getStatus(uint8_t id) |
passionvirus | 6:1dacff31b77a | 218 | { |
passionvirus | 6:1dacff31b77a | 219 | uint8_t status; |
passionvirus | 6:1dacff31b77a | 220 | uint8_t txBuf[7]; |
passionvirus | 6:1dacff31b77a | 221 | |
passionvirus | 6:1dacff31b77a | 222 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 6:1dacff31b77a | 223 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 6:1dacff31b77a | 224 | txBuf[2] = MIN_PACKET_SIZE; // Packet Size |
passionvirus | 6:1dacff31b77a | 225 | txBuf[3] = id; // Servo ID |
passionvirus | 6:1dacff31b77a | 226 | txBuf[4] = CMD_STAT; // Status Error, Status Detail request |
passionvirus | 0:0eef242852bb | 227 | |
passionvirus | 6:1dacff31b77a | 228 | // Check Sum1 and Check Sum2 |
passionvirus | 6:1dacff31b77a | 229 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE; |
passionvirus | 6:1dacff31b77a | 230 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 5:f737e5c70115 | 231 | |
passionvirus | 6:1dacff31b77a | 232 | // send packet (mbed -> herkulex) |
passionvirus | 6:1dacff31b77a | 233 | txPacket(7, txBuf); |
passionvirus | 6:1dacff31b77a | 234 | |
passionvirus | 6:1dacff31b77a | 235 | uint8_t rxBuf[9]; |
passionvirus | 6:1dacff31b77a | 236 | rxPacket(9, rxBuf); |
passionvirus | 6:1dacff31b77a | 237 | |
passionvirus | 6:1dacff31b77a | 238 | // Checksum1 |
passionvirus | 6:1dacff31b77a | 239 | uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE; |
passionvirus | 6:1dacff31b77a | 240 | if (chksum1 != rxBuf[5]) |
passionvirus | 6:1dacff31b77a | 241 | { |
passionvirus | 6:1dacff31b77a | 242 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 243 | pc->printf("Checksum1 fault\n"); |
passionvirus | 6:1dacff31b77a | 244 | #endif |
passionvirus | 6:1dacff31b77a | 245 | |
passionvirus | 6:1dacff31b77a | 246 | return -1; |
passionvirus | 6:1dacff31b77a | 247 | } |
passionvirus | 6:1dacff31b77a | 248 | |
passionvirus | 6:1dacff31b77a | 249 | // Checksum2 |
passionvirus | 6:1dacff31b77a | 250 | uint8_t chksum2 = (~rxBuf[5]&0xFE); |
passionvirus | 6:1dacff31b77a | 251 | if (chksum2 != rxBuf[6]) |
passionvirus | 6:1dacff31b77a | 252 | { |
passionvirus | 6:1dacff31b77a | 253 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 254 | pc->printf("Checksum2 fault\n"); |
passionvirus | 6:1dacff31b77a | 255 | #endif |
passionvirus | 6:1dacff31b77a | 256 | |
passionvirus | 6:1dacff31b77a | 257 | return -1; |
passionvirus | 6:1dacff31b77a | 258 | } |
passionvirus | 6:1dacff31b77a | 259 | |
passionvirus | 6:1dacff31b77a | 260 | status = rxBuf[7]; // Status Error |
passionvirus | 6:1dacff31b77a | 261 | //status = rxBuf[8]; // Status Detail |
passionvirus | 6:1dacff31b77a | 262 | |
passionvirus | 6:1dacff31b77a | 263 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 264 | pc->printf("Status = %02X\n", status); |
passionvirus | 6:1dacff31b77a | 265 | #endif |
passionvirus | 6:1dacff31b77a | 266 | |
passionvirus | 6:1dacff31b77a | 267 | return status; |
passionvirus | 6:1dacff31b77a | 268 | } |
passionvirus | 6:1dacff31b77a | 269 | |
passionvirus | 6:1dacff31b77a | 270 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 271 | int16_t Herkulex::getPos(uint8_t id) |
passionvirus | 6:1dacff31b77a | 272 | { |
passionvirus | 6:1dacff31b77a | 273 | uint16_t position = 0; |
passionvirus | 6:1dacff31b77a | 274 | |
passionvirus | 6:1dacff31b77a | 275 | uint8_t txBuf[9]; |
passionvirus | 6:1dacff31b77a | 276 | |
passionvirus | 6:1dacff31b77a | 277 | txBuf[0] = HEADER; // Packet Header (0xFF) |
passionvirus | 6:1dacff31b77a | 278 | txBuf[1] = HEADER; // Packet Header (0xFF) |
passionvirus | 6:1dacff31b77a | 279 | txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size |
passionvirus | 6:1dacff31b77a | 280 | txBuf[3] = id; // Servo ID |
passionvirus | 6:1dacff31b77a | 281 | txBuf[4] = CMD_RAM_READ; // Status Error, Status Detail request |
passionvirus | 6:1dacff31b77a | 282 | txBuf[5] = 0; // Checksum1 |
passionvirus | 6:1dacff31b77a | 283 | txBuf[6] = 0; // Checksum2 |
passionvirus | 6:1dacff31b77a | 284 | txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52 |
passionvirus | 6:1dacff31b77a | 285 | txBuf[8] = BYTE2; // Address 52 and 53 |
passionvirus | 6:1dacff31b77a | 286 | |
passionvirus | 6:1dacff31b77a | 287 | // Check Sum1 and Check Sum2 |
passionvirus | 6:1dacff31b77a | 288 | txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE; |
passionvirus | 6:1dacff31b77a | 289 | txBuf[6] = (~txBuf[5])&0xFE; |
passionvirus | 6:1dacff31b77a | 290 | |
passionvirus | 6:1dacff31b77a | 291 | // send packet (mbed -> herkulex) |
passionvirus | 6:1dacff31b77a | 292 | txPacket(9, txBuf); |
passionvirus | 6:1dacff31b77a | 293 | |
passionvirus | 6:1dacff31b77a | 294 | uint8_t rxBuf[13]; |
passionvirus | 6:1dacff31b77a | 295 | rxPacket(13, rxBuf); |
passionvirus | 6:1dacff31b77a | 296 | |
passionvirus | 6:1dacff31b77a | 297 | // Checksum1 |
passionvirus | 6:1dacff31b77a | 298 | uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]^rxBuf[9]^rxBuf[10]^rxBuf[11]^rxBuf[12]) & 0xFE; |
passionvirus | 6:1dacff31b77a | 299 | if (chksum1 != rxBuf[5]) |
passionvirus | 6:1dacff31b77a | 300 | { |
passionvirus | 6:1dacff31b77a | 301 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 302 | pc->printf("Checksum1 fault\n"); |
passionvirus | 6:1dacff31b77a | 303 | #endif |
passionvirus | 6:1dacff31b77a | 304 | |
passionvirus | 6:1dacff31b77a | 305 | return -1; |
passionvirus | 6:1dacff31b77a | 306 | } |
passionvirus | 6:1dacff31b77a | 307 | |
passionvirus | 6:1dacff31b77a | 308 | // Checksum2 |
passionvirus | 6:1dacff31b77a | 309 | uint8_t chksum2 = (~rxBuf[5]&0xFE); |
passionvirus | 6:1dacff31b77a | 310 | if (chksum2 != rxBuf[6]) |
passionvirus | 6:1dacff31b77a | 311 | { |
passionvirus | 6:1dacff31b77a | 312 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 313 | pc->printf("Checksum2 fault\n"); |
passionvirus | 6:1dacff31b77a | 314 | #endif |
passionvirus | 6:1dacff31b77a | 315 | |
passionvirus | 6:1dacff31b77a | 316 | return -1; |
passionvirus | 6:1dacff31b77a | 317 | } |
passionvirus | 6:1dacff31b77a | 318 | |
passionvirus | 6:1dacff31b77a | 319 | position = ((rxBuf[10]&0x03)<<8) | rxBuf[9]; |
passionvirus | 6:1dacff31b77a | 320 | |
passionvirus | 6:1dacff31b77a | 321 | #ifdef HERKULEX_DEBUG |
passionvirus | 6:1dacff31b77a | 322 | pc->printf("position = %04X(%d)\n", position, position); |
passionvirus | 6:1dacff31b77a | 323 | #endif |
passionvirus | 6:1dacff31b77a | 324 | |
passionvirus | 6:1dacff31b77a | 325 | return position; |
passionvirus | 6:1dacff31b77a | 326 | } |
passionvirus | 6:1dacff31b77a | 327 | |
passionvirus | 6:1dacff31b77a | 328 | //------------------------------------------------------------------------------ |
passionvirus | 6:1dacff31b77a | 329 |