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