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