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.cpp@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 #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
passionvirus 0:0eef242852bb 159 txBuf[3] = MAX_PID; // 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
passionvirus 5:f737e5c70115 188 txBuf[3] = MAX_PID; // 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
passionvirus 5:f737e5c70115 193 txBuf[8] = speed & 0x00FF; // Speed (LSB, Least Significant Bit)
passionvirus 5:f737e5c70115 194 txBuf[9] =(speed & 0xFF00) >> 8; // Speed (MSB, Most Significanct Bit)
passionvirus 5:f737e5c70115 195 txBuf[10] = TURN_MODE | setLED; // Turn Mode and LED on/off
passionvirus 0:0eef242852bb 196 txBuf[11] = id; // Servo ID
passionvirus 0:0eef242852bb 197
passionvirus 0:0eef242852bb 198 // Checksum1 = (PacketSize ^ pID ^ CMD ^ Data[0] ^ Data[1] ^ ... ^ Data[n]) & 0xFE
passionvirus 0:0eef242852bb 199 // Checksum2 = (~Checksum1)&0xFE
passionvirus 0:0eef242852bb 200 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]^txBuf[9]^txBuf[10]^txBuf[11]) & 0xFE;
passionvirus 0:0eef242852bb 201 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 0:0eef242852bb 202
passionvirus 0:0eef242852bb 203 // send packet (mbed -> herkulex)
passionvirus 0:0eef242852bb 204 txPacket(12, txBuf);
passionvirus 0:0eef242852bb 205 }
passionvirus 0:0eef242852bb 206
passionvirus 0:0eef242852bb 207 //------------------------------------------------------------------------------
passionvirus 6:1dacff31b77a 208 int8_t Herkulex::getStatus(uint8_t id)
passionvirus 6:1dacff31b77a 209 {
passionvirus 6:1dacff31b77a 210 uint8_t status;
passionvirus 6:1dacff31b77a 211 uint8_t txBuf[7];
passionvirus 6:1dacff31b77a 212
passionvirus 6:1dacff31b77a 213 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 6:1dacff31b77a 214 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 6:1dacff31b77a 215 txBuf[2] = MIN_PACKET_SIZE; // Packet Size
passionvirus 6:1dacff31b77a 216 txBuf[3] = id; // Servo ID
passionvirus 6:1dacff31b77a 217 txBuf[4] = CMD_STAT; // Status Error, Status Detail request
passionvirus 0:0eef242852bb 218
passionvirus 6:1dacff31b77a 219 // Check Sum1 and Check Sum2
passionvirus 6:1dacff31b77a 220 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]) & 0xFE;
passionvirus 6:1dacff31b77a 221 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 5:f737e5c70115 222
passionvirus 6:1dacff31b77a 223 // send packet (mbed -> herkulex)
passionvirus 6:1dacff31b77a 224 txPacket(7, txBuf);
passionvirus 6:1dacff31b77a 225
passionvirus 6:1dacff31b77a 226 uint8_t rxBuf[9];
passionvirus 6:1dacff31b77a 227 rxPacket(9, rxBuf);
passionvirus 6:1dacff31b77a 228
passionvirus 6:1dacff31b77a 229 // Checksum1
passionvirus 6:1dacff31b77a 230 uint8_t chksum1 = (rxBuf[2]^rxBuf[3]^rxBuf[4]^rxBuf[7]^rxBuf[8]) & 0xFE;
passionvirus 6:1dacff31b77a 231 if (chksum1 != rxBuf[5])
passionvirus 6:1dacff31b77a 232 {
passionvirus 6:1dacff31b77a 233 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 234 pc->printf("Checksum1 fault\n");
passionvirus 6:1dacff31b77a 235 #endif
passionvirus 6:1dacff31b77a 236
passionvirus 6:1dacff31b77a 237 return -1;
passionvirus 6:1dacff31b77a 238 }
passionvirus 6:1dacff31b77a 239
passionvirus 6:1dacff31b77a 240 // Checksum2
passionvirus 6:1dacff31b77a 241 uint8_t chksum2 = (~rxBuf[5]&0xFE);
passionvirus 6:1dacff31b77a 242 if (chksum2 != rxBuf[6])
passionvirus 6:1dacff31b77a 243 {
passionvirus 6:1dacff31b77a 244 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 245 pc->printf("Checksum2 fault\n");
passionvirus 6:1dacff31b77a 246 #endif
passionvirus 6:1dacff31b77a 247
passionvirus 6:1dacff31b77a 248 return -1;
passionvirus 6:1dacff31b77a 249 }
passionvirus 6:1dacff31b77a 250
passionvirus 6:1dacff31b77a 251 status = rxBuf[7]; // Status Error
passionvirus 6:1dacff31b77a 252 //status = rxBuf[8]; // Status Detail
passionvirus 6:1dacff31b77a 253
passionvirus 6:1dacff31b77a 254 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 255 pc->printf("Status = %02X\n", status);
passionvirus 6:1dacff31b77a 256 #endif
passionvirus 6:1dacff31b77a 257
passionvirus 6:1dacff31b77a 258 return status;
passionvirus 6:1dacff31b77a 259 }
passionvirus 6:1dacff31b77a 260
passionvirus 6:1dacff31b77a 261 //------------------------------------------------------------------------------
passionvirus 6:1dacff31b77a 262 int16_t Herkulex::getPos(uint8_t id)
passionvirus 6:1dacff31b77a 263 {
passionvirus 6:1dacff31b77a 264 uint16_t position = 0;
passionvirus 6:1dacff31b77a 265
passionvirus 6:1dacff31b77a 266 uint8_t txBuf[9];
passionvirus 6:1dacff31b77a 267
passionvirus 6:1dacff31b77a 268 txBuf[0] = HEADER; // Packet Header (0xFF)
passionvirus 6:1dacff31b77a 269 txBuf[1] = HEADER; // Packet Header (0xFF)
passionvirus 6:1dacff31b77a 270 txBuf[2] = MIN_PACKET_SIZE + 2; // Packet Size
passionvirus 6:1dacff31b77a 271 txBuf[3] = id; // Servo ID
passionvirus 6:1dacff31b77a 272 txBuf[4] = CMD_RAM_READ; // Status Error, Status Detail request
passionvirus 6:1dacff31b77a 273 txBuf[5] = 0; // Checksum1
passionvirus 6:1dacff31b77a 274 txBuf[6] = 0; // Checksum2
passionvirus 6:1dacff31b77a 275 txBuf[7] = RAM_CALIBRATED_POSITION; // Address 52
passionvirus 6:1dacff31b77a 276 txBuf[8] = BYTE2; // Address 52 and 53
passionvirus 6:1dacff31b77a 277
passionvirus 6:1dacff31b77a 278 // Check Sum1 and Check Sum2
passionvirus 6:1dacff31b77a 279 txBuf[5] = (txBuf[2]^txBuf[3]^txBuf[4]^txBuf[7]^txBuf[8]) & 0xFE;
passionvirus 6:1dacff31b77a 280 txBuf[6] = (~txBuf[5])&0xFE;
passionvirus 6:1dacff31b77a 281
passionvirus 6:1dacff31b77a 282 // send packet (mbed -> herkulex)
passionvirus 6:1dacff31b77a 283 txPacket(9, txBuf);
passionvirus 6:1dacff31b77a 284
passionvirus 6:1dacff31b77a 285 uint8_t rxBuf[13];
passionvirus 6:1dacff31b77a 286 rxPacket(13, rxBuf);
passionvirus 6:1dacff31b77a 287
passionvirus 6:1dacff31b77a 288 // Checksum1
passionvirus 6:1dacff31b77a 289 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 290 if (chksum1 != rxBuf[5])
passionvirus 6:1dacff31b77a 291 {
passionvirus 6:1dacff31b77a 292 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 293 pc->printf("Checksum1 fault\n");
passionvirus 6:1dacff31b77a 294 #endif
passionvirus 6:1dacff31b77a 295
passionvirus 6:1dacff31b77a 296 return -1;
passionvirus 6:1dacff31b77a 297 }
passionvirus 6:1dacff31b77a 298
passionvirus 6:1dacff31b77a 299 // Checksum2
passionvirus 6:1dacff31b77a 300 uint8_t chksum2 = (~rxBuf[5]&0xFE);
passionvirus 6:1dacff31b77a 301 if (chksum2 != rxBuf[6])
passionvirus 6:1dacff31b77a 302 {
passionvirus 6:1dacff31b77a 303 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 304 pc->printf("Checksum2 fault\n");
passionvirus 6:1dacff31b77a 305 #endif
passionvirus 6:1dacff31b77a 306
passionvirus 6:1dacff31b77a 307 return -1;
passionvirus 6:1dacff31b77a 308 }
passionvirus 6:1dacff31b77a 309
passionvirus 6:1dacff31b77a 310 position = ((rxBuf[10]&0x03)<<8) | rxBuf[9];
passionvirus 6:1dacff31b77a 311
passionvirus 6:1dacff31b77a 312 #ifdef HERKULEX_DEBUG
passionvirus 6:1dacff31b77a 313 pc->printf("position = %04X(%d)\n", position, position);
passionvirus 6:1dacff31b77a 314 #endif
passionvirus 6:1dacff31b77a 315
passionvirus 6:1dacff31b77a 316 return position;
passionvirus 6:1dacff31b77a 317 }
passionvirus 6:1dacff31b77a 318
passionvirus 6:1dacff31b77a 319 //------------------------------------------------------------------------------
passionvirus 6:1dacff31b77a 320