XBusServo.cpp

00001 /**
00002  *  @file   XBusServo.cpp
00003  *  @brief  this header file will contain all required
00004  *          definitions and basic utilities functions.
00005  *          for controling XBus servo.
00006  *  @author Zak Sawa
00007  *  @note   Copyright (c) 2014-2014 JR PROPO
00008  *  @note   Released under the MIT License: http://mbed.org/license/mit
00009  */
00010 
00011 #include "XBusServo.h"
00012 #include "pinmap.h"
00013 
00014 
00015 #define kXBusBaudrate           250000          // bps
00016 
00017 #define kPacketHeaderFooterSize 3
00018 #define kPacketPreDataSize      3
00019 #define kStartOffsetOfCHData    4
00020 
00021 #define kCHDataSize             4
00022 #define kCHDataPacketCommand    0
00023 #define kCHDataPacketLength     1
00024 #define kCHDataPacketKey        2
00025 #define kCHDataPacketType       3
00026 
00027 #define kCmdDataPacketSize      8
00028 #define kCmdDataPacketCommand   0
00029 #define kCmdDataPacketLength    1
00030 #define kCmdDataPacketKey       2
00031 #define kCmdDataPacketCH_ID     3
00032 #define kCmdDataPacketOrder     4
00033 #define kCmdDataPacketData1     5
00034 #define kCmdDataPacketData2     6
00035 #define kCmdDataPacketCRC       7
00036 
00037 
00038 // XBus Command
00039 typedef enum {
00040     kXBusCmd_Set =              0x20,
00041     kXBusCmd_Get =              0x21,
00042     kXBusCmd_Status =           0x22,
00043     kXBusCmd_ModeA =            0xa4
00044 } XBusCmd;
00045 
00046 
00047 // XBus device mode
00048 typedef enum {
00049     kXBusMode_Operate =         0x01,
00050     kXBusMode_IDSet =           0x02
00051 } XBusMode;
00052 
00053 
00054 #define DEBUG
00055 
00056 #ifdef DEBUG
00057 #define DBG(fmt) printf(fmt)
00058 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
00059 #else
00060 #define DBG(...)
00061 #define DBGF(...)
00062 #endif
00063 
00064 
00065 /****************************************************************************
00066     XBusServo::XBusServo
00067 
00068     @param tx           pin name for tx
00069     @param rx           pin name for rx
00070     @param maxServoNum  max number of servo that you want to connect.
00071                         (limit 50)
00072                         this does just to reserve the buffer.  you need to
00073                         add XBus servo at the beginning of your sketch
00074 
00075     @bref               Constructor
00076 
00077     @note               2014/09/02 : move from Arduino lib by Sawa
00078 *****************************************************************************/
00079 XBusServo::XBusServo (PinName tx, PinName rx, PinName sw, uint8_t maxServoNum)
00080     : XBusPort(tx, rx), TxSwitch(sw, 0)
00081 {
00082     DBG("XBusServo::XBusServo\n");
00083 
00084     // initialize serial
00085     txPin = tx;
00086     txOnly = false;
00087     if (rx == NC)
00088         txOnly = true;
00089     maxServo = maxServoNum;
00090 
00091     // initialise vars
00092     numOfServo = 0;
00093     if (maxServo > kXBusMaxServoNum)
00094         maxServo = kXBusMaxServoNum;
00095     else if (maxServo == 0)
00096         maxServo = 1;
00097     dirty = false;
00098     serialCommandBusy = false;
00099     recieveBufferPointer = 0;
00100     modifyServosNow = false;
00101     sendLength = 0;
00102     need2ReadData = false;
00103     chPacketBuffer = NULL;
00104     sendBuffer = NULL;
00105 }
00106 
00107 
00108 //****************************************************************************
00109 //  XBusServo::start
00110 //    return :    error code
00111 //    parameter : none
00112 //
00113 //    start to use XBus
00114 //    2014/09/02 : move from Arduino lib by Sawa
00115 //****************************************************************************
00116 XBusError XBusServo::start()
00117 {
00118     int        bufferSize;            // channel data packet buffer size
00119 
00120     DBG("XBusServo::start\n");
00121 
00122     // allocate my buffer
00123     bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1;     // add 1 for CRC
00124     if (bufferSize < kCmdDataPacketSize)
00125         bufferSize = kCmdDataPacketSize;
00126 
00127     chPacketBuffer = new uint8_t[bufferSize];
00128     if (chPacketBuffer == NULL) {
00129         stop();
00130         return kXBusError_MemoryFull;
00131     }
00132 
00133     sendBuffer = new uint8_t[bufferSize];
00134     if (sendBuffer == NULL) {
00135         stop();
00136         return kXBusError_MemoryFull;
00137     }
00138 
00139     // initialize channel packer buffer
00140     chPacketBuffer[kCHDataPacketCommand]    = kXBusCmd_ModeA;
00141     chPacketBuffer[kCHDataPacketLength]     = 0x00;
00142     chPacketBuffer[kCHDataPacketKey]        = 0x00;
00143     chPacketBuffer[kCHDataPacketType]       = 0x00;
00144 
00145     // initialize serial
00146     XBusPort.baud(kXBusBaudrate);
00147     XBusPort.format(8, Serial::None, 1);
00148 #if DEVICE_SERIAL_FC
00149     XBusPort.set_flow_control(RawSerial::Disabled, NC, NC);
00150 #endif
00151 
00152 #ifdef TARGET_KL25Z
00153     // do nothing here
00154 #else
00155     XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
00156 #endif
00157 
00158     serial_pinout_tx(txPin);
00159 
00160     return kXBusError_NoError;
00161 }
00162 
00163 
00164 //****************************************************************************
00165 //  XBusServo::stop
00166 //    return :    none
00167 //    parameter : none
00168 //
00169 //    stop to use XBus
00170 //    2014/09/02 : move from Arduino lib by Sawa
00171 //****************************************************************************
00172 void XBusServo::stop()
00173 {
00174     DBG("XBusServo::stop\n");
00175 
00176     delete[] chPacketBuffer;
00177     chPacketBuffer = NULL;
00178 
00179     delete[] sendBuffer;
00180     sendBuffer = NULL;
00181 }
00182 
00183 
00184 //****************************************************************************
00185 //  XBusServo::write
00186 //    return :    none
00187 //    parameter : buffer      data buffer to send
00188 //                length      data length on the buffer
00189 //
00190 //    start to send all packet data
00191 //    2014/09/30 : first write by Sawa
00192 //****************************************************************************
00193 void XBusServo::write(uint8_t* buffer, uint8_t length)
00194 {
00195 //  DBG("XBusServo::write\n");
00196 
00197     if (serialCommandBusy)
00198         return;
00199 
00200     serialCommandBusy = true;
00201     XBusServo::sendLength = length;
00202     sendBufferPointer = buffer;
00203 
00204     if (XBusPort.putc(*sendBufferPointer++) < 0) {
00205         serialCommandBusy = false;
00206         XBusServo::sendLength = 0;
00207     } else {
00208         XBusServo::sendLength--;
00209     }
00210 
00211 #ifdef TARGET_KL25Z
00212     XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
00213 #endif
00214 
00215 }
00216 
00217 
00218 //****************************************************************************
00219 //  XBusServo::flush
00220 //    return :    none
00221 //    parameter : none
00222 //
00223 //    wait to send all packet data.  never call from interrupt.
00224 //    2014/09/30 : first write by Sawa
00225 //****************************************************************************
00226 void XBusServo::flush(void)
00227 {
00228 //    DBG("XBusServo::flush\n");
00229 
00230     while(serialCommandBusy)
00231         ;
00232 }
00233 
00234 
00235 //****************************************************************************
00236 //  XBusServo::TxSwitchHandler
00237 //    return :    none
00238 //    parameter : none
00239 //
00240 //    handler for Tx switch
00241 //    2014/10/29 : first write by Sawa
00242 //****************************************************************************
00243 void XBusServo::TxSwitchHandler(void)
00244 {
00245     TxSwitch.write(1);
00246 }
00247 
00248 
00249 //****************************************************************************
00250 //  XBusServo::TxIrqHandler
00251 //    return :    none
00252 //    parameter : none
00253 //
00254 //    handler for Tx buffer empty
00255 //    2014/09/30 : first write by Sawa
00256 //    2014/10/24 : add to setup reading for command data packet
00257 //****************************************************************************
00258 void XBusServo::TxIrqHandler(void)
00259 {
00260     int             result = 0;
00261 //    DBG("XBusServo::TxIrqHandler\n");
00262 
00263     if (! serialCommandBusy)
00264         return;                                       // Is it noise ?
00265 
00266     if (XBusServo::sendLength > 0) {
00267         result = XBusPort.putc(*sendBufferPointer++);
00268         if (result < 0) {
00269             // error occured
00270             XBusServo::sendLength = 0;
00271             serialCommandBusy = false;
00272         } else {
00273             XBusServo::sendLength--;
00274             return;
00275         }
00276     }
00277 
00278 #ifdef TARGET_KL25Z
00279     XBusPort.attach(NULL, RawSerial::TxIrq);
00280 #endif
00281 
00282     if (result < 0)
00283         return;
00284 
00285     if (need2ReadData) {
00286 #ifdef TARGET_KL25Z
00287         TxSwitchTimer.attach_us(this, &XBusServo::TxSwitchHandler, 27);
00288 #endif
00289     }
00290 
00291     serialCommandBusy = false;
00292 }
00293 
00294 
00295 //****************************************************************************
00296 //  XBusServo::RxIrqHandler
00297 //    return :    none
00298 //    parameter : none
00299 //
00300 //    handler for Rx buffer full
00301 //    2014/09/30 : first write by Sawa
00302 //    2014/10/21 : add to ignore the data myself
00303 //    2014/10/24 : modify to get the data from 1st byte of buffer
00304 //****************************************************************************
00305 void XBusServo::RxIrqHandler(void)
00306 {
00307 //    DBG("XBusServo::RxIrqHandler\n");
00308 
00309     while (XBusPort.readable()) {
00310         recieveBuffer[recieveBufferPointer++] = XBusPort.getc();
00311         if (recieveBufferPointer >= kRecieveBufferSize)
00312             recieveBufferPointer = 0;
00313     }
00314 }
00315 
00316 
00317 //****************************************************************************
00318 //  XBusServo::sendChannelDataPacket
00319 //    return :    none
00320 //    parameter : none
00321 //
00322 //    This should be called on the timer handler like MsTimer2 when you
00323 //    use the XBus servo.
00324 //    2014/09/02 : move from Arduino lib by Sawa
00325 //****************************************************************************
00326 void XBusServo::sendChannelDataPacket(void)
00327 {
00328 //    DBG("XBusServo::sendChannelDataPacket\n");
00329 
00330     if (modifyServosNow)
00331         return;
00332 
00333     if (numOfServo > 0) {
00334         if (dirty) {
00335             memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
00336             dirty = false;
00337         }
00338 
00339         need2ReadData = false;
00340         write(sendBuffer, sendBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
00341     }
00342 }
00343 
00344 
00345 //****************************************************************************
00346 //  XBusServo::sendCommandDataPacket
00347 //    return :    error code
00348 //    parameter : command     The commnad that you want to send
00349 //                channelID   The channel ID of the XBus servo that you want to set up
00350 //                order       The order that you want to set up
00351 //                value       The value that you want to set / get
00352 //                valueSize   The value size.  1 byte(char) or 2 byte(int)
00353 //
00354 //    This should NOT be called on interrupt handler when you
00355 //    setup the XBus servo.
00356 //    2014/09/02 : move from Arduino lib by Sawa
00357 //****************************************************************************
00358 XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
00359 {
00360     int                 sendSize;
00361     uint8_t             dummy;
00362     volatile uint8_t*   recieveDataPtr;
00363 
00364     DBG("XBusServo::sendCommandDataPacket\n");
00365 
00366     // setup command
00367     sendBuffer[kCmdDataPacketCommand] = command;
00368     sendBuffer[kCmdDataPacketLength] = valueSize + kPacketPreDataSize;
00369     sendBuffer[kCmdDataPacketKey] = 0x00;
00370     sendBuffer[kCmdDataPacketCH_ID] = channelID;
00371     sendBuffer[kCmdDataPacketOrder] = order;
00372     if (valueSize == 1) {                   // 1 byte value
00373         sendBuffer[kCmdDataPacketData1] = *value & 0x00FF;
00374         sendBuffer[kCmdDataPacketData2] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
00375     } else {
00376         sendBuffer[kCmdDataPacketData1] = (*value >> 8) & 0x00FF;
00377         sendBuffer[kCmdDataPacketData2] = *value & 0x00FF;
00378         sendBuffer[kCmdDataPacketCRC] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
00379     }
00380 
00381     // to recover channel data packet when you call sendChannelDataPacket after this
00382     dirty = true;
00383 
00384     // prepair recieve data
00385     XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
00386     while (XBusPort.readable())
00387         dummy = XBusPort.getc();
00388     recieveBufferPointer = 0;                                   // reset recieve buffer
00389 
00390     // send command
00391     sendSize = sendBuffer[kCmdDataPacketLength] + kPacketHeaderFooterSize;
00392     need2ReadData = ((! txOnly) && (channelID != 0));
00393     recieveDataPtr = &(recieveBuffer[sendSize]);
00394     recieveDataPtr[kCmdDataPacketLength] = 5;            // dummy to get real packet size
00395     write(sendBuffer, sendSize);
00396 //    DBG("XBusServo::sendCommandDataPacket 2\n");
00397 
00398     // if it's tx only mode or ID=0, it's done
00399     if (! need2ReadData) {
00400         flush();
00401         return kXBusError_NoError;
00402     }
00403 
00404 // wait to read all packet
00405     while(recieveBufferPointer < (recieveDataPtr[kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
00406         if (recieveBufferPointer > 16)
00407             break;                                  // some trouble happen
00408             
00409         //                                           need to add time out
00410     }
00411 
00412     TxSwitch.write(0);
00413 //    DBG("XBusServo::sendCommandDataPacket 4\n");
00414     // change bus direction to Tx mode
00415     XBusPort.attach(NULL, RawSerial::RxIrq);
00416     need2ReadData = false;
00417     serialCommandBusy = false;
00418 
00419     // check CRC
00420     if (crc8((uint8_t*)recieveDataPtr, recieveDataPtr[kCHDataPacketLength] + 3) != 0) {
00421         DBG("XBusServo::sendCommandDataPacket kXBusError_CRCError\n");
00422         return kXBusError_CRCError;
00423     }
00424 
00425     // check unsupported
00426     if (recieveDataPtr[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
00427         DBG("XBusServo::sendCommandDataPacket kXBusError_Unsupported\n");
00428         return kXBusError_Unsupported;
00429     }
00430 
00431     // send back the value
00432     if (valueSize == 1) {                   // 1 byte value
00433         *value = recieveDataPtr[kCmdDataPacketData1];
00434         if (*value & 0x0080)
00435             *value |= 0xFF00;
00436     } else {
00437         *value = recieveDataPtr[kCmdDataPacketData1];
00438         *value <<= 8;
00439         *value |= recieveDataPtr[kCmdDataPacketData2];
00440     }
00441 
00442     return kXBusError_NoError;
00443 }
00444 
00445 
00446 //****************************************************************************
00447 //  XBusServo::addServo
00448 //    return :    error code
00449 //    parameter : channelID   channel ID of the XBus servo that you want to use
00450 //                initValue   initial value of this XBus servo
00451 //                            use kXbusServoNeutral for center of the XBus servo
00452 //
00453 //    add new servo to the buffer on this library
00454 //    2014/09/02 : move from Arduino lib by Sawa
00455 //****************************************************************************
00456 XBusError XBusServo::addServo(uint8_t channelID, uint16_t initValue)
00457 {
00458     uint16_t         dataOffset;
00459 
00460     DBG("XBusServo::addServo\n");
00461 
00462     // max check
00463     if (numOfServo >= maxServo)
00464         return kXBusError_ServoNumOverflow;
00465 
00466     // convert to servo ID
00467     channelID &= 0x3F;
00468 
00469     // scan servo ID that is already added
00470     if (numOfServo > 0) {
00471         uint16_t     servoNo;
00472 
00473         for (servoNo = 0; servoNo < numOfServo; servoNo++)
00474             if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID)
00475                 return kXBusError_AddWithSameID;                            // found same servo ID
00476     }
00477 
00478     // atomic flag on
00479     modifyServosNow = true;
00480 
00481     // add new servo
00482     dataOffset = kStartOffsetOfCHData + kCHDataSize * numOfServo;
00483     numOfServo++;
00484     chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type
00485 
00486     chPacketBuffer[dataOffset] = channelID;
00487     chPacketBuffer[dataOffset + 1] = 0x00;
00488     chPacketBuffer[dataOffset + 2] = (initValue >> 8) & 0x00FF;
00489     chPacketBuffer[dataOffset + 3] = initValue & 0x00FF;
00490 
00491     // calc CRC
00492     chPacketBuffer[dataOffset + 4] = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
00493     dirty = true;
00494 
00495     // atomic flag off
00496     modifyServosNow = false;
00497 
00498     return kXBusError_NoError;
00499 }
00500 
00501 
00502 //****************************************************************************
00503 //  XBusServo::removeServo
00504 //    return :    error code
00505 //    parameter : channelID   channel ID of the XBus servo that you want to remove
00506 //
00507 //    remove the servo from the buffer on this library
00508 //    2014/09/02 : move from Arduino lib by Sawa
00509 //****************************************************************************
00510 XBusError XBusServo::removeServo(uint8_t channelID)
00511 {
00512     DBG("XBusServo::removeServo\n");
00513 
00514     // min check
00515     if (numOfServo == 0)
00516         return kXBusError_ServoNumIsZero;
00517 
00518     // convert to servo ID
00519     channelID &= 0x3F;
00520 
00521     // scan servo ID that is already added
00522     if (numOfServo > 0) {
00523         uint16_t     servoNo;
00524 
00525         for (servoNo = 0; servoNo < numOfServo; servoNo++)
00526             if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID) {
00527                 // atomic flag on
00528                 modifyServosNow = true;
00529 
00530                 // copy data after that
00531                 if (servoNo < (numOfServo - 1))
00532                     memcpy(&(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo]),
00533                            &(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * (servoNo + 1)]),
00534                            kCHDataSize * (numOfServo - servoNo - 1));
00535 
00536                 // update packet size
00537                 numOfServo--;
00538                 chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type
00539 
00540                 // calc CRC
00541                 chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
00542                 = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
00543                 dirty = true;
00544 
00545                 // atomic flag off
00546                 modifyServosNow = false;
00547 
00548                 return kXBusError_NoError;
00549             }
00550     }
00551 
00552     return kXBusError_IDNotFound;
00553 }
00554 
00555 
00556 //****************************************************************************
00557 //  XBusServo::setServo
00558 //    return :    error code
00559 //    parameter : channelID   channel ID of the XBus servo that you want to set
00560 //                value       value of this XBus servo
00561 //                            use kXbusServoNeutral for center of the XBus servo
00562 //
00563 //    set new angle to the servo
00564 //    2014/09/02 : move from Arduino lib by Sawa
00565 //****************************************************************************
00566 XBusError XBusServo::setServo(uint8_t channelID, uint16_t value)
00567 {
00568     uint16_t         dataOffset;
00569 
00570 //   DBG("XBusServo::setServo\n");
00571 
00572     // convert to servo ID
00573     channelID &= 0x3F;
00574 
00575     // scan servo ID that is already added
00576     if (numOfServo > 0) {
00577         int     servoNo;
00578 
00579         for (servoNo = 0; servoNo < numOfServo; servoNo++) {
00580             dataOffset = kStartOffsetOfCHData + kCHDataSize * servoNo;
00581 
00582             if (chPacketBuffer[dataOffset] == channelID) {
00583                 // atomic flag on
00584                 modifyServosNow = true;
00585 
00586                 // set value
00587                 chPacketBuffer[dataOffset + 2] = (value >> 8) & 0x00FF;
00588                 chPacketBuffer[dataOffset + 3] = value & 0x00FF;
00589 
00590                 // calc CRC
00591                 chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
00592                 = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
00593                 dirty = 1;
00594 
00595                 // atomic flag off
00596                 modifyServosNow = false;
00597 
00598                 return kXBusError_NoError;
00599             }
00600         }
00601     }
00602 
00603     return kXBusError_IDNotFound;
00604 }
00605 
00606 
00607 //****************************************************************************
00608 //  XBusServo::setChannelID
00609 //    return :    error code
00610 //    parameter : oldChannelID    channel IDof the XBus servo to change the ID
00611 //                newChannelID    new channel ID for the XBus servo
00612 //
00613 //    set new channel ID to the XBus servo
00614 //    2014/09/02 : move from Arduino lib by Sawa
00615 //****************************************************************************
00616 XBusError XBusServo::setChannelID(uint8_t oldChannelID, uint8_t newChannelID)
00617 {
00618     XBusError       result;
00619     int16_t         value;
00620 
00621     DBG("XBusServo::setChannelID\n");
00622 
00623     value = kXBusMode_IDSet;
00624     result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_Mode, &value, 1);
00625     if (result != kXBusError_NoError)
00626         return result;
00627 
00628     value = newChannelID;
00629     result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_ID, &value, 1);
00630 
00631     return result;
00632 }
00633 
00634 
00635 //****************************************************************************
00636 //  XBusServo::setChannelID
00637 //    return :    error code
00638 //    parameter : newChannelID    new channel ID for the XBus servo
00639 //
00640 //    set new channel ID to the XBus servo
00641 //    this is only for TX only mode
00642 //    2014/09/02 : move from Arduino lib by Sawa
00643 //****************************************************************************
00644 XBusError XBusServo::setChannelID(uint8_t newChannelID)
00645 {
00646     XBusError       result;
00647     int16_t         value;
00648 
00649     DBG("XBusServo::setChannelID\n");
00650 
00651     if (! txOnly)
00652         return kXBusError_OnlyForTxOnlyMode;
00653 
00654     value = kXBusMode_IDSet;
00655     result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_Mode, &value, 1);
00656     if (result != kXBusError_NoError)
00657         return result;
00658 
00659     value = newChannelID;
00660     result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_ID, &value, 1);
00661 
00662     return result;
00663 }
00664 
00665 
00666 //****************************************************************************
00667 //  XBusServo::getDataSize
00668 //    return :    data size for this order
00669 //    parameter : order       the order that you want to know
00670 //
00671 //    get the data size of this order
00672 //    2014/09/02 : move from Arduino lib by Sawa
00673 //****************************************************************************
00674 uint8_t XBusServo::getDataSize(uint8_t order)
00675 {
00676  //   DBG("XBusServo::getDataSize\n");
00677 
00678     switch(order) {
00679         case kXBusOrder_2_Version:      // only for get
00680         case kXBusOrder_2_Product:      // only for get
00681         case kXBusOrder_2_Reset:        // only for set
00682         case kXBusOrder_2_ParamWrite:   // only for set
00683         case kXBusOrder_2_Reverse:
00684         case kXBusOrder_2_Neutral:
00685         case kXBusOrder_2_H_Travel:
00686         case kXBusOrder_2_L_Travel:
00687         case kXBusOrder_2_H_Limit:
00688         case kXBusOrder_2_L_Limit:
00689         case kXBusOrder_2_PowerOffset:
00690         case kXBusOrder_2_AlarmDelay:
00691         case kXBusOrder_2_CurrentPos:   // only for get
00692         case kXBusOrder_2_MaxInteger:
00693             return 2;
00694         default:
00695             return 1;
00696     }
00697 }
00698 
00699 
00700 //****************************************************************************
00701 //  XBusServo::setCommand
00702 //    return :    error code
00703 //    parameter : channelID   channel ID of the XBus servo that you want to set to
00704 //                order       the order that you want
00705 //                value       the value that you want to set and return current value
00706 //
00707 //    send set command to the XBus servo
00708 //    2014/09/02 : move from Arduino lib by Sawa
00709 //****************************************************************************
00710 XBusError XBusServo::setCommand(uint8_t channelID, uint8_t order, int16_t* value)
00711 {
00712     DBG("XBusServo::setCommand\n");
00713 
00714     return sendCommandDataPacket(kXBusCmd_Set, channelID, order, value, getDataSize(order));
00715 }
00716 
00717 
00718 //****************************************************************************
00719 //  XBusServo::getCommand
00720 //    return :    error code
00721 //    parameter : channelID   channel ID of the XBus servo that you want to get from
00722 //                order       the order that you want
00723 //                value       the value that you want to get from
00724 //
00725 //    send get command to the XBus servo
00726 //    2014/09/02 : move from Arduino lib by Sawa
00727 //****************************************************************************
00728 XBusError XBusServo::getCommand(uint8_t channelID, uint8_t order, int16_t* value)
00729 {
00730     DBG("XBusServo::getCommand\n");
00731 
00732     return sendCommandDataPacket(kXBusCmd_Get, channelID, order, value, getDataSize(order));
00733 }
00734 
00735 
00736 //****************************************************************************
00737 //  XBusServo::setCommand
00738 //    return :    error code
00739 //    parameter : order       the order that you want
00740 //                value       the value that you want to set current value
00741 //
00742 //    send set command to the XBus servo
00743 //    this is only for TX only mode
00744 //    2014/09/02 : move from Arduino lib by Sawa
00745 //****************************************************************************
00746 XBusError XBusServo::setCommand(uint8_t order, int16_t* value)
00747 {
00748     DBG("XBusServo::setCommand\n");
00749 
00750     if (! txOnly)
00751         return kXBusError_OnlyForTxOnlyMode;
00752 
00753     return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));
00754 }
00755 
00756 
00757 
00758 
00759 
00760 
00761 //****************************************************************************
00762 // for CRC
00763 static uint8_t s_crc_array[256] = {
00764     0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
00765     0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
00766     0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
00767     0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
00768     0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
00769     0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
00770     0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
00771     0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
00772     0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
00773     0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
00774     0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
00775     0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
00776     0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
00777     0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
00778     0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
00779     0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
00780     0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
00781     0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
00782     0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
00783     0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
00784     0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
00785     0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
00786     0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
00787     0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
00788     0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
00789     0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
00790     0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
00791     0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
00792     0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
00793     0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
00794     0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
00795     0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
00796 };
00797 
00798 
00799 uint8_t XBusServo::crc_table(uint8_t  data, uint8_t  crc)
00800 {
00801     uint16_t  index = (data ^ crc) & 0xff;
00802 
00803     crc = s_crc_array[index];
00804     return crc;
00805 }
00806 
00807 
00808 uint8_t XBusServo::crc8(uint8_t* buffer, uint8_t  length)
00809 {
00810     uint8_t  crc = 0;
00811 
00812     while (length-- > 0)
00813         crc = crc_table(*buffer++, crc);
00814     return crc;
00815 }
00816 
00817 
00818 
00819