Kazutaka "Zak" Sawa / XBusServo

Library for XBus servo (under construction)

Dependents:   mbed_XBus_Test mbed_XBus_MotionTest XBusServoTest ControlYokutan2017_2 ... more

It's pre-opened page. it's still a little bit unstable to use command packet but mostly work. Tested only on KL25Z

暫定版ページです。 まだコマンドパケット使用時に時々不安定になりますが、概ね動作しています。 KL25Z上でのみ、動作確認しています

XBusServo.cpp@7:04d294709f7a, 2014-10-08 (annotated)

Committer:
sawa
Date:
Wed Oct 08 05:49:35 2014 +0000
Revision:
7:04d294709f7a
Parent:
6:62bf0f21b8d6
Child:
13:5e2aa53353a8
goxygen test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sawa 4:d88f498d259b 1 /**
sawa 4:d88f498d259b 2 * @file XBusServo.cpp
sawa 4:d88f498d259b 3 * @brief this header file will contain all required
sawa 4:d88f498d259b 4 * definitions and basic utilities functions.
sawa 4:d88f498d259b 5 * @author Zak Sawa
sawa 4:d88f498d259b 6 * @note Copyright (c) 2014-2014 JR PROPO
sawa 4:d88f498d259b 7 * @note Released under the MIT License: http://mbed.org/license/mit
sawa 0:381d475cfd6c 8 */
sawa 0:381d475cfd6c 9
sawa 0:381d475cfd6c 10 #include "XBusServo.h"
sawa 0:381d475cfd6c 11 #include "pinmap.h"
sawa 1:bd80d3e8f3a3 12 #include "gpio_api.h"
sawa 1:bd80d3e8f3a3 13
sawa 0:381d475cfd6c 14
sawa 0:381d475cfd6c 15
sawa 0:381d475cfd6c 16 #define kXBusBaudrate 250000 // bps
sawa 0:381d475cfd6c 17
sawa 0:381d475cfd6c 18 #define kStartOffsetOfCHData 4
sawa 0:381d475cfd6c 19 #define kCHDataSize 4
sawa 0:381d475cfd6c 20 #define kCHDataPacketCommand 0
sawa 0:381d475cfd6c 21 #define kCHDataPacketLength 1
sawa 0:381d475cfd6c 22 #define kCHDataPacketKey 2
sawa 0:381d475cfd6c 23 #define kCHDataPacketType 3
sawa 0:381d475cfd6c 24
sawa 0:381d475cfd6c 25 #define kCmdDataPacketSize 8
sawa 0:381d475cfd6c 26 #define kCmdDataPacketCommand 0
sawa 0:381d475cfd6c 27 #define kCmdDataPacketLength 1
sawa 0:381d475cfd6c 28 #define kCmdDataPacketKey 2
sawa 0:381d475cfd6c 29 #define kCmdDataPacketCH_ID 3
sawa 0:381d475cfd6c 30 #define kCmdDataPacketOrder 4
sawa 0:381d475cfd6c 31 #define kCmdDataPacketData1 5
sawa 0:381d475cfd6c 32 #define kCmdDataPacketData2 6
sawa 0:381d475cfd6c 33 #define kCmdDataPacketCRC 7
sawa 0:381d475cfd6c 34
sawa 0:381d475cfd6c 35
sawa 0:381d475cfd6c 36 // XBus Command
sawa 0:381d475cfd6c 37 typedef enum {
sawa 0:381d475cfd6c 38 kXBusCmd_Set = 0x20,
sawa 0:381d475cfd6c 39 kXBusCmd_Get = 0x21,
sawa 0:381d475cfd6c 40 kXBusCmd_Status = 0x22,
sawa 0:381d475cfd6c 41 kXBusCmd_ModeA = 0xa4
sawa 0:381d475cfd6c 42 } XBusCmd;
sawa 0:381d475cfd6c 43
sawa 0:381d475cfd6c 44
sawa 0:381d475cfd6c 45 // XBUS device mode
sawa 0:381d475cfd6c 46 typedef enum {
sawa 0:381d475cfd6c 47 kXBusMode_Operate = 0x01,
sawa 0:381d475cfd6c 48 kXBusMode_IDSet = 0x02
sawa 0:381d475cfd6c 49 } XBusMode;
sawa 0:381d475cfd6c 50
sawa 0:381d475cfd6c 51
sawa 0:381d475cfd6c 52 #define DEBUG
sawa 0:381d475cfd6c 53
sawa 0:381d475cfd6c 54 #ifdef DEBUG
sawa 1:bd80d3e8f3a3 55 #define DBG(fmt) printf(fmt)
sawa 1:bd80d3e8f3a3 56 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
sawa 0:381d475cfd6c 57 #else
sawa 0:381d475cfd6c 58 #define DBG(...)
sawa 0:381d475cfd6c 59 #define DBGF(...)
sawa 0:381d475cfd6c 60 #endif
sawa 0:381d475cfd6c 61
sawa 0:381d475cfd6c 62
sawa 7:04d294709f7a 63 /****************************************************************************
sawa 6:62bf0f21b8d6 64 XBusServo::XBusServo
sawa 6:62bf0f21b8d6 65
sawa 6:62bf0f21b8d6 66 @param tx pin name for tx
sawa 6:62bf0f21b8d6 67 @param rx pin name for rx
sawa 6:62bf0f21b8d6 68 @param maxServoNum max number of servo that you want to connect.
sawa 6:62bf0f21b8d6 69 (limit 50)
sawa 6:62bf0f21b8d6 70 this does just to resetve the buffer. you need to
sawa 6:62bf0f21b8d6 71 add XBus servo at the beginning of your sketch
sawa 0:381d475cfd6c 72
sawa 6:62bf0f21b8d6 73 @bref Constructor
sawa 6:62bf0f21b8d6 74
sawa 6:62bf0f21b8d6 75 @note 2014/09/02 : move from Arduino lib by Sawa
sawa 6:62bf0f21b8d6 76 *****************************************************************************/
sawa 0:381d475cfd6c 77 XBusServo::XBusServo(PinName tx, PinName rx, uint8_t maxServoNum)
sawa 1:bd80d3e8f3a3 78 : XBusPort(tx, rx)
sawa 0:381d475cfd6c 79 {
sawa 0:381d475cfd6c 80 DBG("XBusServo::XBusServo\n");
sawa 0:381d475cfd6c 81
sawa 0:381d475cfd6c 82 // initialize serial
sawa 0:381d475cfd6c 83 txPin = tx;
sawa 0:381d475cfd6c 84 txOnly = (rx == NC);
sawa 1:bd80d3e8f3a3 85 maxServo = maxServoNum;
sawa 1:bd80d3e8f3a3 86 }
sawa 1:bd80d3e8f3a3 87
sawa 1:bd80d3e8f3a3 88
sawa 1:bd80d3e8f3a3 89 //****************************************************************************
sawa 1:bd80d3e8f3a3 90 // XBusServo::start
sawa 1:bd80d3e8f3a3 91 // return : error code
sawa 1:bd80d3e8f3a3 92 // parameter : none
sawa 1:bd80d3e8f3a3 93 //
sawa 1:bd80d3e8f3a3 94 // start to use XBus
sawa 1:bd80d3e8f3a3 95 // 2014/09/02 : move from Arduino lib by Sawa
sawa 1:bd80d3e8f3a3 96 //****************************************************************************
sawa 1:bd80d3e8f3a3 97 XBusError XBusServo::start()
sawa 1:bd80d3e8f3a3 98 {
sawa 1:bd80d3e8f3a3 99 int bufferSize; // channel data packet buffer size
sawa 1:bd80d3e8f3a3 100
sawa 1:bd80d3e8f3a3 101 DBG("XBusServo::start\n");
sawa 0:381d475cfd6c 102
sawa 0:381d475cfd6c 103 // initialise vars
sawa 0:381d475cfd6c 104 numOfServo = 0;
sawa 0:381d475cfd6c 105 if (maxServo > kXBusMaxServoNum)
sawa 0:381d475cfd6c 106 maxServo = kXBusMaxServoNum;
sawa 0:381d475cfd6c 107 else if (maxServo == 0)
sawa 0:381d475cfd6c 108 maxServo = 1;
sawa 0:381d475cfd6c 109 dirty = 0;
sawa 0:381d475cfd6c 110 serialWriteBusy = 0;
sawa 0:381d475cfd6c 111 recieveBufferPointer = 0;
sawa 0:381d475cfd6c 112 modifyServosNow = 0;
sawa 0:381d475cfd6c 113 sendLength = 0;
sawa 0:381d475cfd6c 114 bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1; // add 1 for CRC
sawa 0:381d475cfd6c 115 if (bufferSize < kCmdDataPacketSize)
sawa 0:381d475cfd6c 116 bufferSize = kCmdDataPacketSize;
sawa 1:bd80d3e8f3a3 117
sawa 0:381d475cfd6c 118 chPacketBuffer = (uint8_t*)malloc(bufferSize);
sawa 0:381d475cfd6c 119 sendBuffer = (uint8_t*)malloc(bufferSize);
sawa 0:381d475cfd6c 120
sawa 0:381d475cfd6c 121 // initialize channel packer buffer
sawa 0:381d475cfd6c 122 chPacketBuffer[kCHDataPacketCommand] = kXBusCmd_ModeA;
sawa 0:381d475cfd6c 123 chPacketBuffer[kCHDataPacketLength] = 0x00;
sawa 0:381d475cfd6c 124 chPacketBuffer[kCHDataPacketKey] = 0x00;
sawa 0:381d475cfd6c 125 chPacketBuffer[kCHDataPacketType] = 0x00;
sawa 0:381d475cfd6c 126
sawa 0:381d475cfd6c 127 // initialize serial
sawa 1:bd80d3e8f3a3 128 XBusPort.baud(kXBusBaudrate);
sawa 1:bd80d3e8f3a3 129 XBusPort.format(8, RawSerial::None, 1);
sawa 0:381d475cfd6c 130 #if DEVICE_SERIAL_FC
sawa 1:bd80d3e8f3a3 131 XBusPort.set_flow_control(RawSerial::Disabled, NC, NC);
sawa 0:381d475cfd6c 132 #endif
sawa 0:381d475cfd6c 133
sawa 1:bd80d3e8f3a3 134 #ifdef TARGET_KL25Z
sawa 1:bd80d3e8f3a3 135 // do nothing here
sawa 1:bd80d3e8f3a3 136 #else
sawa 1:bd80d3e8f3a3 137 XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
sawa 1:bd80d3e8f3a3 138 #endif
sawa 1:bd80d3e8f3a3 139 XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
sawa 1:bd80d3e8f3a3 140
sawa 0:381d475cfd6c 141 serial_pinout_tx(txPin);
sawa 1:bd80d3e8f3a3 142
sawa 1:bd80d3e8f3a3 143 return kXBusError_NoError;
sawa 0:381d475cfd6c 144 }
sawa 0:381d475cfd6c 145
sawa 0:381d475cfd6c 146 //****************************************************************************
sawa 1:bd80d3e8f3a3 147 // XBusServo::stop
sawa 0:381d475cfd6c 148 // return : none
sawa 0:381d475cfd6c 149 // parameter : none
sawa 0:381d475cfd6c 150 //
sawa 1:bd80d3e8f3a3 151 // stop to use XBus
sawa 0:381d475cfd6c 152 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 153 //****************************************************************************
sawa 1:bd80d3e8f3a3 154 void XBusServo::stop()
sawa 0:381d475cfd6c 155 {
sawa 1:bd80d3e8f3a3 156 DBG("XBusServo::stop\n");
sawa 0:381d475cfd6c 157
sawa 0:381d475cfd6c 158 free(chPacketBuffer);
sawa 0:381d475cfd6c 159 free(sendBuffer);
sawa 0:381d475cfd6c 160 }
sawa 0:381d475cfd6c 161
sawa 0:381d475cfd6c 162 //****************************************************************************
sawa 0:381d475cfd6c 163 // XBusServo::write
sawa 0:381d475cfd6c 164 // return : none
sawa 0:381d475cfd6c 165 // parameter : buffer data buffer to send
sawa 0:381d475cfd6c 166 // length data length on the buffer
sawa 0:381d475cfd6c 167 //
sawa 0:381d475cfd6c 168 // start to send all packet data
sawa 0:381d475cfd6c 169 // 2014/09/30 : first write by Sawa
sawa 0:381d475cfd6c 170 //****************************************************************************
sawa 0:381d475cfd6c 171 void XBusServo::write(uint8_t* buffer, uint8_t length)
sawa 0:381d475cfd6c 172 {
sawa 1:bd80d3e8f3a3 173 // DBG("XBusServo::write\n");
sawa 0:381d475cfd6c 174
sawa 0:381d475cfd6c 175 if (serialWriteBusy)
sawa 0:381d475cfd6c 176 return;
sawa 0:381d475cfd6c 177
sawa 0:381d475cfd6c 178 serialWriteBusy = 1;
sawa 0:381d475cfd6c 179 XBusServo::sendLength = length;
sawa 0:381d475cfd6c 180 sendBufferPointer = buffer;
sawa 0:381d475cfd6c 181
sawa 1:bd80d3e8f3a3 182 if (XBusPort.putc(*sendBufferPointer++) < 0) {
sawa 0:381d475cfd6c 183 serialWriteBusy = 0;
sawa 0:381d475cfd6c 184 XBusServo::sendLength = 0;
sawa 1:bd80d3e8f3a3 185 } else {
sawa 0:381d475cfd6c 186 XBusServo::sendLength--;
sawa 1:bd80d3e8f3a3 187 #ifdef TARGET_KL25Z
sawa 1:bd80d3e8f3a3 188 XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
sawa 1:bd80d3e8f3a3 189 #endif
sawa 1:bd80d3e8f3a3 190 }
sawa 0:381d475cfd6c 191 }
sawa 0:381d475cfd6c 192
sawa 0:381d475cfd6c 193
sawa 0:381d475cfd6c 194 //****************************************************************************
sawa 0:381d475cfd6c 195 // XBusServo::flush
sawa 0:381d475cfd6c 196 // return : none
sawa 0:381d475cfd6c 197 // parameter : none
sawa 0:381d475cfd6c 198 //
sawa 0:381d475cfd6c 199 // wait to send all packet data
sawa 0:381d475cfd6c 200 // 2014/09/30 : first write by Sawa
sawa 0:381d475cfd6c 201 //****************************************************************************
sawa 0:381d475cfd6c 202 void XBusServo::flush(void)
sawa 0:381d475cfd6c 203 {
sawa 0:381d475cfd6c 204 // DBG("XBusServo::flush\n");
sawa 0:381d475cfd6c 205
sawa 0:381d475cfd6c 206 while(serialWriteBusy)
sawa 0:381d475cfd6c 207 ;
sawa 0:381d475cfd6c 208 }
sawa 0:381d475cfd6c 209
sawa 0:381d475cfd6c 210
sawa 0:381d475cfd6c 211 //****************************************************************************
sawa 0:381d475cfd6c 212 // XBusServo::TxIrqHandler
sawa 0:381d475cfd6c 213 // return : none
sawa 0:381d475cfd6c 214 // parameter : none
sawa 0:381d475cfd6c 215 //
sawa 0:381d475cfd6c 216 // handler for Tx buffer empty
sawa 0:381d475cfd6c 217 // 2014/09/30 : first write by Sawa
sawa 0:381d475cfd6c 218 //****************************************************************************
sawa 0:381d475cfd6c 219 void XBusServo::TxIrqHandler(void)
sawa 0:381d475cfd6c 220 {
sawa 0:381d475cfd6c 221 // DBG("XBusServo::TxIrqHandler\n");
sawa 0:381d475cfd6c 222
sawa 0:381d475cfd6c 223 if (! serialWriteBusy)
sawa 0:381d475cfd6c 224 return;
sawa 0:381d475cfd6c 225
sawa 1:bd80d3e8f3a3 226 if (XBusServo::sendLength <= 0) {
sawa 0:381d475cfd6c 227 serialWriteBusy = 0;
sawa 1:bd80d3e8f3a3 228 #ifdef TARGET_KL25Z
sawa 1:bd80d3e8f3a3 229 XBusPort.attach(NULL, RawSerial::TxIrq);
sawa 1:bd80d3e8f3a3 230 #endif
sawa 1:bd80d3e8f3a3 231 } else {
sawa 1:bd80d3e8f3a3 232 if (XBusPort.putc(*sendBufferPointer++) < 0) {
sawa 0:381d475cfd6c 233 serialWriteBusy = 0;
sawa 0:381d475cfd6c 234 XBusServo::sendLength = 0;
sawa 1:bd80d3e8f3a3 235 #ifdef TARGET_KL25Z
sawa 1:bd80d3e8f3a3 236 XBusPort.attach(NULL, RawSerial::TxIrq);
sawa 1:bd80d3e8f3a3 237 #endif
sawa 0:381d475cfd6c 238 } else
sawa 0:381d475cfd6c 239 XBusServo::sendLength--;
sawa 0:381d475cfd6c 240 }
sawa 0:381d475cfd6c 241 }
sawa 0:381d475cfd6c 242
sawa 0:381d475cfd6c 243
sawa 0:381d475cfd6c 244 //****************************************************************************
sawa 0:381d475cfd6c 245 // XBusServo::RxIrqHandler
sawa 0:381d475cfd6c 246 // return : none
sawa 0:381d475cfd6c 247 // parameter : none
sawa 0:381d475cfd6c 248 //
sawa 0:381d475cfd6c 249 // handler for Rx buffer full
sawa 0:381d475cfd6c 250 // 2014/09/30 : first write by Sawa
sawa 0:381d475cfd6c 251 //****************************************************************************
sawa 0:381d475cfd6c 252 void XBusServo::RxIrqHandler(void)
sawa 0:381d475cfd6c 253 {
sawa 0:381d475cfd6c 254 // DBG("XBusServo::RxIrqHandler\n");
sawa 0:381d475cfd6c 255
sawa 1:bd80d3e8f3a3 256 recieveBuffer[recieveBufferPointer++] = XBusPort.getc();
sawa 0:381d475cfd6c 257 if (recieveBufferPointer >= kRecieveBufferSize)
sawa 0:381d475cfd6c 258 recieveBufferPointer = 0;
sawa 0:381d475cfd6c 259 }
sawa 0:381d475cfd6c 260
sawa 0:381d475cfd6c 261
sawa 0:381d475cfd6c 262
sawa 0:381d475cfd6c 263 //****************************************************************************
sawa 0:381d475cfd6c 264 // XBusServo::sendChannelDataPacket
sawa 0:381d475cfd6c 265 // return : none
sawa 0:381d475cfd6c 266 // parameter : none
sawa 0:381d475cfd6c 267 //
sawa 0:381d475cfd6c 268 // This should be called on the timer handler like MsTimer2 when you
sawa 0:381d475cfd6c 269 // use the XBus servo.
sawa 0:381d475cfd6c 270 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 271 //****************************************************************************
sawa 0:381d475cfd6c 272 void XBusServo::sendChannelDataPacket(void)
sawa 0:381d475cfd6c 273 {
sawa 0:381d475cfd6c 274 // DBG("XBusServo::sendChannelDataPacket\n");
sawa 0:381d475cfd6c 275
sawa 0:381d475cfd6c 276 if (modifyServosNow)
sawa 0:381d475cfd6c 277 return;
sawa 0:381d475cfd6c 278
sawa 0:381d475cfd6c 279 if (numOfServo > 0) {
sawa 0:381d475cfd6c 280 if (dirty) {
sawa 0:381d475cfd6c 281 memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 3);
sawa 0:381d475cfd6c 282 dirty = 0;
sawa 0:381d475cfd6c 283 }
sawa 0:381d475cfd6c 284
sawa 0:381d475cfd6c 285 write(sendBuffer, sendBuffer[kCHDataPacketLength] + 3);
sawa 0:381d475cfd6c 286 }
sawa 0:381d475cfd6c 287 }
sawa 0:381d475cfd6c 288
sawa 0:381d475cfd6c 289
sawa 0:381d475cfd6c 290 //****************************************************************************
sawa 0:381d475cfd6c 291 // XBusServo::sendCommandDataPacket
sawa 0:381d475cfd6c 292 // return : error code
sawa 0:381d475cfd6c 293 // parameter : command The commnad that you want to send
sawa 0:381d475cfd6c 294 // channelID The channel ID of the XBus servo that you want to set up
sawa 0:381d475cfd6c 295 // order The order that you want to set up
sawa 0:381d475cfd6c 296 // value The value that you want to set / get
sawa 0:381d475cfd6c 297 // valueSize The value size. 1 byte(char) or 2 byte(int)
sawa 0:381d475cfd6c 298 //
sawa 0:381d475cfd6c 299 // This should NOT be called on the timer handler like MsTimer2 when you
sawa 0:381d475cfd6c 300 // setup the XBus servo.
sawa 0:381d475cfd6c 301 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 302 //****************************************************************************
sawa 0:381d475cfd6c 303 XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
sawa 0:381d475cfd6c 304 {
sawa 0:381d475cfd6c 305 int sendSize;
sawa 0:381d475cfd6c 306 uint8_t* recievedPacket;
sawa 0:381d475cfd6c 307
sawa 0:381d475cfd6c 308 DBG("XBusServo::sendCommandDataPacket\n");
sawa 0:381d475cfd6c 309
sawa 0:381d475cfd6c 310 // setup command
sawa 0:381d475cfd6c 311 sendBuffer[kCmdDataPacketCommand] = command;
sawa 0:381d475cfd6c 312 sendBuffer[kCmdDataPacketLength] = valueSize + 3;
sawa 0:381d475cfd6c 313 sendBuffer[kCmdDataPacketKey] = 0x00;
sawa 0:381d475cfd6c 314 sendBuffer[kCmdDataPacketCH_ID] = channelID;
sawa 0:381d475cfd6c 315 sendBuffer[kCmdDataPacketOrder] = order;
sawa 0:381d475cfd6c 316 if (valueSize == 1) { // 1 byte value
sawa 0:381d475cfd6c 317 sendBuffer[kCmdDataPacketData1] = *value & 0x00FF;
sawa 0:381d475cfd6c 318 sendBuffer[kCmdDataPacketData2] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
sawa 0:381d475cfd6c 319 } else {
sawa 0:381d475cfd6c 320 sendBuffer[kCmdDataPacketData1] = (*value >> 8) & 0x00FF;
sawa 0:381d475cfd6c 321 sendBuffer[kCmdDataPacketData2] = *value & 0x00FF;
sawa 0:381d475cfd6c 322 sendBuffer[kCmdDataPacketCRC] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
sawa 0:381d475cfd6c 323 }
sawa 0:381d475cfd6c 324
sawa 0:381d475cfd6c 325 // to recover channel data packet when you call sendChannelDataPacket after this
sawa 0:381d475cfd6c 326 dirty = 1;
sawa 0:381d475cfd6c 327
sawa 0:381d475cfd6c 328 // send command
sawa 0:381d475cfd6c 329 recieveBufferPointer = 0; // reset recieve buffer
sawa 0:381d475cfd6c 330 sendSize = sendBuffer[kCmdDataPacketLength] + 3;
sawa 0:381d475cfd6c 331 write(sendBuffer, sendSize);
sawa 0:381d475cfd6c 332 flush(); // wait to send all bytes
sawa 0:381d475cfd6c 333
sawa 0:381d475cfd6c 334 // if it's tx only mode, it done
sawa 0:381d475cfd6c 335 if (txOnly)
sawa 0:381d475cfd6c 336 return kXBusError_NoError;
sawa 0:381d475cfd6c 337
sawa 0:381d475cfd6c 338 if (channelID != 0) {
sawa 0:381d475cfd6c 339 // change bus direction to Rx mode
sawa 0:381d475cfd6c 340 // DigitalInOut::input();
sawa 0:381d475cfd6c 341
sawa 0:381d475cfd6c 342 recievedPacket = &(recieveBuffer[sendSize]);
sawa 0:381d475cfd6c 343
sawa 0:381d475cfd6c 344 // wait to read all packet
sawa 0:381d475cfd6c 345 while(recieveBufferPointer < (sendSize + kCmdDataPacketSize))
sawa 0:381d475cfd6c 346 ;
sawa 0:381d475cfd6c 347
sawa 0:381d475cfd6c 348 // change bus direction to Tx mode
sawa 0:381d475cfd6c 349 serial_pinout_tx(txPin);
sawa 0:381d475cfd6c 350
sawa 0:381d475cfd6c 351 // check CRC
sawa 0:381d475cfd6c 352 if (crc8(recievedPacket, recievedPacket[kCHDataPacketLength] + 3) != 0)
sawa 0:381d475cfd6c 353 return kXBusError_CRCError;
sawa 0:381d475cfd6c 354
sawa 0:381d475cfd6c 355 // check unsupported
sawa 0:381d475cfd6c 356 if (recievedPacket[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported)
sawa 0:381d475cfd6c 357 return kXBusError_Unsupported;
sawa 0:381d475cfd6c 358
sawa 0:381d475cfd6c 359 // send back the value
sawa 0:381d475cfd6c 360 if (valueSize == 1) { // 1 byte value
sawa 0:381d475cfd6c 361 *value = recievedPacket[kCmdDataPacketData1];
sawa 0:381d475cfd6c 362 if (*value & 0x0080)
sawa 0:381d475cfd6c 363 *value |= 0xFF00;
sawa 0:381d475cfd6c 364 } else {
sawa 0:381d475cfd6c 365 *value = recievedPacket[kCmdDataPacketData1];
sawa 0:381d475cfd6c 366 *value <<= 8;
sawa 0:381d475cfd6c 367 *value |= recievedPacket[kCmdDataPacketData2];
sawa 0:381d475cfd6c 368 }
sawa 0:381d475cfd6c 369 }
sawa 0:381d475cfd6c 370
sawa 0:381d475cfd6c 371 return kXBusError_NoError;
sawa 0:381d475cfd6c 372 }
sawa 0:381d475cfd6c 373
sawa 0:381d475cfd6c 374
sawa 0:381d475cfd6c 375 //****************************************************************************
sawa 0:381d475cfd6c 376 // XBusServo::addServo
sawa 0:381d475cfd6c 377 // return : error code
sawa 0:381d475cfd6c 378 // parameter : channelID channel ID of the XBus servo that you want to use
sawa 0:381d475cfd6c 379 // initValue initial value of this XBus servo
sawa 0:381d475cfd6c 380 // use kXbusServoNeutral for center of the XBus servo
sawa 0:381d475cfd6c 381 //
sawa 0:381d475cfd6c 382 // add new servo to the buffer on this library
sawa 0:381d475cfd6c 383 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 384 //****************************************************************************
sawa 0:381d475cfd6c 385 XBusError XBusServo::addServo(uint8_t channelID, uint16_t initValue)
sawa 0:381d475cfd6c 386 {
sawa 0:381d475cfd6c 387 uint16_t dataOffset;
sawa 0:381d475cfd6c 388
sawa 0:381d475cfd6c 389 DBG("XBusServo::addServo\n");
sawa 0:381d475cfd6c 390
sawa 0:381d475cfd6c 391 // max check
sawa 0:381d475cfd6c 392 if (numOfServo >= maxServo)
sawa 0:381d475cfd6c 393 return kXBusError_ServoNumOverflow;
sawa 0:381d475cfd6c 394
sawa 0:381d475cfd6c 395 // convert to servo ID
sawa 0:381d475cfd6c 396 channelID &= 0x3F;
sawa 0:381d475cfd6c 397
sawa 0:381d475cfd6c 398 // scan servo ID that is already added
sawa 0:381d475cfd6c 399 if (numOfServo > 0) {
sawa 0:381d475cfd6c 400 uint16_t servoNo;
sawa 0:381d475cfd6c 401
sawa 0:381d475cfd6c 402 for (servoNo = 0; servoNo < numOfServo; servoNo++)
sawa 0:381d475cfd6c 403 if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID)
sawa 0:381d475cfd6c 404 return kXBusError_AddWithSameID; // found same servo ID
sawa 0:381d475cfd6c 405 }
sawa 0:381d475cfd6c 406
sawa 0:381d475cfd6c 407 // atomic flag on
sawa 0:381d475cfd6c 408 modifyServosNow = 1;
sawa 0:381d475cfd6c 409
sawa 0:381d475cfd6c 410 // add new servo
sawa 0:381d475cfd6c 411 dataOffset = kStartOffsetOfCHData + kCHDataSize * numOfServo;
sawa 0:381d475cfd6c 412 numOfServo++;
sawa 0:381d475cfd6c 413 chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2; // add 2 for key and type
sawa 0:381d475cfd6c 414
sawa 0:381d475cfd6c 415 chPacketBuffer[dataOffset] = channelID;
sawa 0:381d475cfd6c 416 chPacketBuffer[dataOffset + 1] = 0x00;
sawa 0:381d475cfd6c 417 chPacketBuffer[dataOffset + 2] = (initValue >> 8) & 0x00FF;
sawa 0:381d475cfd6c 418 chPacketBuffer[dataOffset + 3] = initValue & 0x00FF;
sawa 0:381d475cfd6c 419
sawa 0:381d475cfd6c 420 // calc CRC
sawa 0:381d475cfd6c 421 chPacketBuffer[dataOffset + 4] = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
sawa 0:381d475cfd6c 422 dirty = 1;
sawa 0:381d475cfd6c 423
sawa 0:381d475cfd6c 424 // atomic flag off
sawa 0:381d475cfd6c 425 modifyServosNow = 0;
sawa 0:381d475cfd6c 426
sawa 0:381d475cfd6c 427 return kXBusError_NoError;
sawa 0:381d475cfd6c 428 }
sawa 0:381d475cfd6c 429
sawa 0:381d475cfd6c 430
sawa 0:381d475cfd6c 431 //****************************************************************************
sawa 0:381d475cfd6c 432 // XBusServo::removeServo
sawa 0:381d475cfd6c 433 // return : error code
sawa 0:381d475cfd6c 434 // parameter : channelID channel ID of the XBus servo that you want to remove
sawa 0:381d475cfd6c 435 //
sawa 0:381d475cfd6c 436 // remove the servo from the buffer on this library
sawa 0:381d475cfd6c 437 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 438 //****************************************************************************
sawa 0:381d475cfd6c 439 XBusError XBusServo::removeServo(uint8_t channelID)
sawa 0:381d475cfd6c 440 {
sawa 0:381d475cfd6c 441 DBG("XBusServo::removeServo\n");
sawa 0:381d475cfd6c 442
sawa 0:381d475cfd6c 443 // min check
sawa 0:381d475cfd6c 444 if (numOfServo == 0)
sawa 0:381d475cfd6c 445 return kXBusError_ServoNumIsZero;
sawa 0:381d475cfd6c 446
sawa 0:381d475cfd6c 447 // convert to servo ID
sawa 0:381d475cfd6c 448 channelID &= 0x3F;
sawa 0:381d475cfd6c 449
sawa 0:381d475cfd6c 450 // scan servo ID that is already added
sawa 0:381d475cfd6c 451 if (numOfServo > 0) {
sawa 0:381d475cfd6c 452 uint16_t servoNo;
sawa 0:381d475cfd6c 453
sawa 0:381d475cfd6c 454 for (servoNo = 0; servoNo < numOfServo; servoNo++)
sawa 0:381d475cfd6c 455 if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID) {
sawa 0:381d475cfd6c 456 // atomic flag on
sawa 0:381d475cfd6c 457 modifyServosNow = 1;
sawa 0:381d475cfd6c 458
sawa 0:381d475cfd6c 459 // copy data after that
sawa 0:381d475cfd6c 460 if (servoNo < (numOfServo - 1))
sawa 0:381d475cfd6c 461 memcpy(&(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo]),
sawa 0:381d475cfd6c 462 &(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * (servoNo + 1)]),
sawa 0:381d475cfd6c 463 kCHDataSize * (numOfServo - servoNo - 1));
sawa 0:381d475cfd6c 464
sawa 0:381d475cfd6c 465 // update packet size
sawa 0:381d475cfd6c 466 numOfServo--;
sawa 0:381d475cfd6c 467 chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2; // add 2 for key and type
sawa 0:381d475cfd6c 468
sawa 0:381d475cfd6c 469 // calc CRC
sawa 0:381d475cfd6c 470 chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
sawa 0:381d475cfd6c 471 = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
sawa 0:381d475cfd6c 472 dirty = 1;
sawa 0:381d475cfd6c 473
sawa 0:381d475cfd6c 474 // atomic flag off
sawa 0:381d475cfd6c 475 modifyServosNow = 0;
sawa 0:381d475cfd6c 476
sawa 0:381d475cfd6c 477 return kXBusError_NoError;
sawa 0:381d475cfd6c 478 }
sawa 0:381d475cfd6c 479 }
sawa 0:381d475cfd6c 480
sawa 0:381d475cfd6c 481 return kXBusError_IDNotFound;
sawa 0:381d475cfd6c 482 }
sawa 0:381d475cfd6c 483
sawa 0:381d475cfd6c 484
sawa 0:381d475cfd6c 485 //****************************************************************************
sawa 0:381d475cfd6c 486 // XBusServo::setServo
sawa 0:381d475cfd6c 487 // return : error code
sawa 0:381d475cfd6c 488 // parameter : channelID channel ID of the XBus servo that you want to set
sawa 0:381d475cfd6c 489 // value value of this XBus servo
sawa 0:381d475cfd6c 490 // use kXbusServoNeutral for center of the XBus servo
sawa 0:381d475cfd6c 491 //
sawa 0:381d475cfd6c 492 // remove the servo from the buffer on this library
sawa 0:381d475cfd6c 493 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 494 //****************************************************************************
sawa 0:381d475cfd6c 495 XBusError XBusServo::setServo(uint8_t channelID, uint16_t value)
sawa 0:381d475cfd6c 496 {
sawa 0:381d475cfd6c 497 uint16_t dataOffset;
sawa 0:381d475cfd6c 498
sawa 1:bd80d3e8f3a3 499 // DBG("XBusServo::setServo\n");
sawa 0:381d475cfd6c 500
sawa 0:381d475cfd6c 501 // convert to servo ID
sawa 0:381d475cfd6c 502 channelID &= 0x3F;
sawa 0:381d475cfd6c 503
sawa 0:381d475cfd6c 504 // scan servo ID that is already added
sawa 0:381d475cfd6c 505 if (numOfServo > 0) {
sawa 0:381d475cfd6c 506 int servoNo;
sawa 0:381d475cfd6c 507
sawa 0:381d475cfd6c 508 for (servoNo = 0; servoNo < numOfServo; servoNo++) {
sawa 0:381d475cfd6c 509 dataOffset = kStartOffsetOfCHData + kCHDataSize * servoNo;
sawa 0:381d475cfd6c 510
sawa 0:381d475cfd6c 511 if (chPacketBuffer[dataOffset] == channelID) {
sawa 0:381d475cfd6c 512 // atomic flag on
sawa 0:381d475cfd6c 513 modifyServosNow = 1;
sawa 0:381d475cfd6c 514
sawa 0:381d475cfd6c 515 // set value
sawa 0:381d475cfd6c 516 chPacketBuffer[dataOffset + 2] = (value >> 8) & 0x00FF;
sawa 0:381d475cfd6c 517 chPacketBuffer[dataOffset + 3] = value & 0x00FF;
sawa 0:381d475cfd6c 518
sawa 0:381d475cfd6c 519 // calc CRC
sawa 0:381d475cfd6c 520 chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
sawa 0:381d475cfd6c 521 = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
sawa 0:381d475cfd6c 522 dirty = 1;
sawa 0:381d475cfd6c 523
sawa 0:381d475cfd6c 524 // atomic flag off
sawa 0:381d475cfd6c 525 modifyServosNow = 0;
sawa 0:381d475cfd6c 526
sawa 0:381d475cfd6c 527 return kXBusError_NoError;
sawa 0:381d475cfd6c 528 }
sawa 0:381d475cfd6c 529 }
sawa 0:381d475cfd6c 530 }
sawa 0:381d475cfd6c 531
sawa 0:381d475cfd6c 532 return kXBusError_IDNotFound;
sawa 0:381d475cfd6c 533 }
sawa 0:381d475cfd6c 534
sawa 0:381d475cfd6c 535
sawa 0:381d475cfd6c 536 //****************************************************************************
sawa 0:381d475cfd6c 537 // XBusServo::setChannelID
sawa 0:381d475cfd6c 538 // return : error code
sawa 0:381d475cfd6c 539 // parameter : oldChannelID channel IDof the XBus servo to change the ID
sawa 0:381d475cfd6c 540 // newChannelID new channel ID for the XBus servo
sawa 0:381d475cfd6c 541 //
sawa 0:381d475cfd6c 542 // set new channel ID to the XBus servo
sawa 0:381d475cfd6c 543 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 544 //****************************************************************************
sawa 0:381d475cfd6c 545 XBusError XBusServo::setChannelID(uint8_t oldChannelID, uint8_t newChannelID)
sawa 0:381d475cfd6c 546 {
sawa 0:381d475cfd6c 547 XBusError result;
sawa 0:381d475cfd6c 548 int16_t value;
sawa 0:381d475cfd6c 549
sawa 0:381d475cfd6c 550 DBG("XBusServo::setChannelID\n");
sawa 0:381d475cfd6c 551
sawa 0:381d475cfd6c 552 value = kXBusMode_IDSet;
sawa 0:381d475cfd6c 553 result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_Mode, &value, 1);
sawa 0:381d475cfd6c 554 if (result != kXBusError_NoError)
sawa 0:381d475cfd6c 555 return result;
sawa 0:381d475cfd6c 556
sawa 0:381d475cfd6c 557 value = newChannelID;
sawa 0:381d475cfd6c 558 result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_ID, &value, 1);
sawa 0:381d475cfd6c 559
sawa 0:381d475cfd6c 560 return result;
sawa 0:381d475cfd6c 561 }
sawa 0:381d475cfd6c 562
sawa 0:381d475cfd6c 563
sawa 0:381d475cfd6c 564 //****************************************************************************
sawa 0:381d475cfd6c 565 // XBusServo::setChannelID
sawa 0:381d475cfd6c 566 // return : error code
sawa 0:381d475cfd6c 567 // parameter : newChannelID new channel ID for the XBus servo
sawa 0:381d475cfd6c 568 //
sawa 0:381d475cfd6c 569 // set new channel ID to the XBus servo
sawa 0:381d475cfd6c 570 // this is only for TX only mode
sawa 0:381d475cfd6c 571 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 572 //****************************************************************************
sawa 0:381d475cfd6c 573 XBusError XBusServo::setChannelID(uint8_t newChannelID)
sawa 0:381d475cfd6c 574 {
sawa 0:381d475cfd6c 575 XBusError result;
sawa 0:381d475cfd6c 576 int16_t value;
sawa 0:381d475cfd6c 577
sawa 0:381d475cfd6c 578 DBG("XBusServo::setChannelID\n");
sawa 0:381d475cfd6c 579
sawa 0:381d475cfd6c 580 if (txOnly)
sawa 0:381d475cfd6c 581 return kXBusError_OnlyForTxOnlyMode;
sawa 0:381d475cfd6c 582
sawa 0:381d475cfd6c 583 value = kXBusMode_IDSet;
sawa 0:381d475cfd6c 584 result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_Mode, &value, 1);
sawa 0:381d475cfd6c 585 if (result != kXBusError_NoError)
sawa 0:381d475cfd6c 586 return result;
sawa 0:381d475cfd6c 587
sawa 0:381d475cfd6c 588 value = newChannelID;
sawa 0:381d475cfd6c 589 result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_ID, &value, 1);
sawa 0:381d475cfd6c 590
sawa 0:381d475cfd6c 591 return result;
sawa 0:381d475cfd6c 592 }
sawa 0:381d475cfd6c 593
sawa 0:381d475cfd6c 594
sawa 0:381d475cfd6c 595 //****************************************************************************
sawa 0:381d475cfd6c 596 // XBusServo::getDataSize
sawa 0:381d475cfd6c 597 // return : data size for this order
sawa 0:381d475cfd6c 598 // parameter : order the order that you want to know
sawa 0:381d475cfd6c 599 //
sawa 0:381d475cfd6c 600 // get the data size of this order
sawa 0:381d475cfd6c 601 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 602 //****************************************************************************
sawa 0:381d475cfd6c 603 uint8_t XBusServo::getDataSize(uint8_t order)
sawa 0:381d475cfd6c 604 {
sawa 0:381d475cfd6c 605 uint8_t dataSize = 1;
sawa 0:381d475cfd6c 606
sawa 0:381d475cfd6c 607 DBG("XBusServo::getDataSize\n");
sawa 0:381d475cfd6c 608
sawa 0:381d475cfd6c 609 switch(order) {
sawa 0:381d475cfd6c 610 case kXBusOrder_2_Version: // only for get
sawa 0:381d475cfd6c 611 case kXBusOrder_2_Product: // only for get
sawa 0:381d475cfd6c 612 case kXBusOrder_2_Reset: // only for set
sawa 0:381d475cfd6c 613 case kXBusOrder_2_ParamWrite: // only for set
sawa 0:381d475cfd6c 614 case kXBusOrder_2_Reverse:
sawa 0:381d475cfd6c 615 case kXBusOrder_2_Neutral:
sawa 0:381d475cfd6c 616 case kXBusOrder_2_H_Travel:
sawa 0:381d475cfd6c 617 case kXBusOrder_2_L_Travel:
sawa 0:381d475cfd6c 618 case kXBusOrder_2_H_Limit:
sawa 0:381d475cfd6c 619 case kXBusOrder_2_L_Limit:
sawa 0:381d475cfd6c 620 case kXBusOrder_2_PowerOffset:
sawa 0:381d475cfd6c 621 case kXBusOrder_2_AlarmDelay:
sawa 0:381d475cfd6c 622 case kXBusOrder_2_CurrentPos: // only for get
sawa 0:381d475cfd6c 623 case kXBusOrder_2_MaxInteger:
sawa 0:381d475cfd6c 624 dataSize = 2;
sawa 0:381d475cfd6c 625 }
sawa 0:381d475cfd6c 626
sawa 0:381d475cfd6c 627 return dataSize;
sawa 0:381d475cfd6c 628 }
sawa 0:381d475cfd6c 629
sawa 0:381d475cfd6c 630
sawa 0:381d475cfd6c 631 //****************************************************************************
sawa 0:381d475cfd6c 632 // XBusServo::setCommand
sawa 0:381d475cfd6c 633 // return : error code
sawa 0:381d475cfd6c 634 // parameter : channelID channel ID of the XBus servo that you want to set to
sawa 0:381d475cfd6c 635 // order the order that you want
sawa 0:381d475cfd6c 636 // value the value that you want to set and return current value
sawa 0:381d475cfd6c 637 //
sawa 0:381d475cfd6c 638 // send set command to the XBus servo
sawa 0:381d475cfd6c 639 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 640 //****************************************************************************
sawa 0:381d475cfd6c 641 XBusError XBusServo::setCommand(uint8_t channelID, uint8_t order, int16_t* value)
sawa 0:381d475cfd6c 642 {
sawa 0:381d475cfd6c 643 DBG("XBusServo::setCommand\n");
sawa 0:381d475cfd6c 644
sawa 0:381d475cfd6c 645 return sendCommandDataPacket(kXBusCmd_Set, channelID, order, value, getDataSize(order));
sawa 0:381d475cfd6c 646 }
sawa 0:381d475cfd6c 647
sawa 0:381d475cfd6c 648
sawa 0:381d475cfd6c 649 //****************************************************************************
sawa 0:381d475cfd6c 650 // XBusServo::getCommand
sawa 0:381d475cfd6c 651 // return : error code
sawa 0:381d475cfd6c 652 // parameter : channelID channel ID of the XBus servo that you want to get from
sawa 0:381d475cfd6c 653 // order the order that you want
sawa 0:381d475cfd6c 654 // value the value that you want to get from
sawa 0:381d475cfd6c 655 //
sawa 0:381d475cfd6c 656 // send get command to the XBus servo
sawa 0:381d475cfd6c 657 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 658 //****************************************************************************
sawa 0:381d475cfd6c 659 XBusError XBusServo::getCommand(uint8_t channelID, uint8_t order, int16_t* value)
sawa 0:381d475cfd6c 660 {
sawa 0:381d475cfd6c 661 DBG("XBusServo::getCommand\n");
sawa 0:381d475cfd6c 662
sawa 0:381d475cfd6c 663 return sendCommandDataPacket(kXBusCmd_Get, channelID, order, value, getDataSize(order));
sawa 0:381d475cfd6c 664 }
sawa 0:381d475cfd6c 665
sawa 0:381d475cfd6c 666
sawa 0:381d475cfd6c 667 //****************************************************************************
sawa 0:381d475cfd6c 668 // XBusServo::setCommand
sawa 0:381d475cfd6c 669 // return : error code
sawa 0:381d475cfd6c 670 // parameter : order the order that you want
sawa 0:381d475cfd6c 671 // value the value that you want to set and return current value
sawa 0:381d475cfd6c 672 //
sawa 0:381d475cfd6c 673 // send set command to the XBus servo
sawa 0:381d475cfd6c 674 // this is only for TX only mode
sawa 0:381d475cfd6c 675 // 2014/09/02 : move from Arduino lib by Sawa
sawa 0:381d475cfd6c 676 //****************************************************************************
sawa 0:381d475cfd6c 677 XBusError XBusServo::setCommand(uint8_t order, int16_t* value)
sawa 0:381d475cfd6c 678 {
sawa 0:381d475cfd6c 679 DBG("XBusServo::setCommand\n");
sawa 0:381d475cfd6c 680
sawa 0:381d475cfd6c 681 if (txOnly)
sawa 0:381d475cfd6c 682 return kXBusError_OnlyForTxOnlyMode;
sawa 0:381d475cfd6c 683
sawa 0:381d475cfd6c 684 return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));
sawa 0:381d475cfd6c 685 }
sawa 0:381d475cfd6c 686
sawa 0:381d475cfd6c 687
sawa 0:381d475cfd6c 688
sawa 0:381d475cfd6c 689
sawa 0:381d475cfd6c 690
sawa 0:381d475cfd6c 691
sawa 0:381d475cfd6c 692 //****************************************************************************
sawa 0:381d475cfd6c 693 // for CRC
sawa 0:381d475cfd6c 694 static uint8_t s_crc_array[256] = {
sawa 0:381d475cfd6c 695 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
sawa 0:381d475cfd6c 696 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
sawa 0:381d475cfd6c 697 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
sawa 0:381d475cfd6c 698 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
sawa 0:381d475cfd6c 699 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
sawa 0:381d475cfd6c 700 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
sawa 0:381d475cfd6c 701 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
sawa 0:381d475cfd6c 702 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
sawa 0:381d475cfd6c 703 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
sawa 0:381d475cfd6c 704 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
sawa 0:381d475cfd6c 705 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
sawa 0:381d475cfd6c 706 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
sawa 0:381d475cfd6c 707 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
sawa 0:381d475cfd6c 708 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
sawa 0:381d475cfd6c 709 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
sawa 0:381d475cfd6c 710 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
sawa 0:381d475cfd6c 711 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
sawa 0:381d475cfd6c 712 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
sawa 0:381d475cfd6c 713 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
sawa 0:381d475cfd6c 714 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
sawa 0:381d475cfd6c 715 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
sawa 0:381d475cfd6c 716 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
sawa 0:381d475cfd6c 717 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
sawa 0:381d475cfd6c 718 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
sawa 0:381d475cfd6c 719 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
sawa 0:381d475cfd6c 720 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
sawa 0:381d475cfd6c 721 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
sawa 0:381d475cfd6c 722 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
sawa 0:381d475cfd6c 723 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
sawa 0:381d475cfd6c 724 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
sawa 0:381d475cfd6c 725 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
sawa 0:381d475cfd6c 726 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
sawa 0:381d475cfd6c 727 };
sawa 0:381d475cfd6c 728
sawa 0:381d475cfd6c 729
sawa 0:381d475cfd6c 730 uint8_t XBusServo::crc_table(uint8_t data, uint8_t crc)
sawa 0:381d475cfd6c 731 {
sawa 0:381d475cfd6c 732 uint16_t index = (data ^ crc) & 0xff;
sawa 0:381d475cfd6c 733
sawa 0:381d475cfd6c 734 crc = s_crc_array[index];
sawa 0:381d475cfd6c 735 return crc;
sawa 0:381d475cfd6c 736 }
sawa 0:381d475cfd6c 737
sawa 0:381d475cfd6c 738
sawa 0:381d475cfd6c 739 uint8_t XBusServo::crc8(uint8_t* buffer, uint8_t length)
sawa 0:381d475cfd6c 740 {
sawa 0:381d475cfd6c 741 uint8_t crc = 0;
sawa 0:381d475cfd6c 742
sawa 0:381d475cfd6c 743 while (length-- > 0)
sawa 0:381d475cfd6c 744 crc = crc_table(*buffer++, crc);
sawa 0:381d475cfd6c 745 return crc;
sawa 0:381d475cfd6c 746 }
sawa 0:381d475cfd6c 747
sawa 0:381d475cfd6c 748
sawa 0:381d475cfd6c 749
sawa 0:381d475cfd6c 750