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