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@17:3ffb2e3e3bec, 2014-10-30 (annotated)

Committer:
sawa
Date:
Thu Oct 30 05:11:46 2014 +0000
Revision:
17:3ffb2e3e3bec
Parent:
16:e283810b53c3
Child:
18:75ddf12d93b6
temp update (working proto)

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