Genath N / herkulex

herkulex servo control library

Fork of herkulex by Yoonseok Pyo

herkulex.cpp@7:0d98883391a2, 2018-06-21 (annotated)

Committer:
genath6
Date:
Thu Jun 21 10:28:43 2018 +0000
Revision:
7:0d98883391a2
Parent:
6:1dacff31b77a 
Cr?ation du programme

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