Diff: XBusServo.cpp

Revision:

0:381d475cfd6c

Child:

1:bd80d3e8f3a3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/XBusServo.cpp	Thu Oct 02 08:46:53 2014 +0000
@@ -0,0 +1,720 @@
+/* XBusServo.cpp file
+ *
+ * for mbed
+ *
+ * Copyright (c) 2014-2014 JR PROPO
+ * by Zak Sawa
+ */
+
+#include "XBusServo.h"
+#include "pinmap.h"
+
+
+#define kXBusBaudrate           250000          // bps
+
+#define kStartOffsetOfCHData    4
+#define kCHDataSize             4
+#define kCHDataPacketCommand    0
+#define kCHDataPacketLength     1
+#define kCHDataPacketKey        2
+#define kCHDataPacketType       3
+
+#define kCmdDataPacketSize      8
+#define kCmdDataPacketCommand   0
+#define kCmdDataPacketLength    1
+#define kCmdDataPacketKey       2
+#define kCmdDataPacketCH_ID     3
+#define kCmdDataPacketOrder     4
+#define kCmdDataPacketData1     5
+#define kCmdDataPacketData2     6
+#define kCmdDataPacketCRC       7
+
+
+// XBus Command
+typedef enum {
+    kXBusCmd_Set =              0x20,
+    kXBusCmd_Get =              0x21,
+    kXBusCmd_Status =           0x22,
+    kXBusCmd_ModeA =            0xa4
+} XBusCmd;
+
+
+// XBUS device mode
+typedef enum {
+    kXBusMode_Operate =         0x01,
+    kXBusMode_IDSet =           0x02
+} XBusMode;
+
+
+#define DEBUG
+
+#ifdef DEBUG
+Serial      pc(USBTX, USBRX); // tx, rx
+#define DBG(fmt) pc.printf(fmt)
+#define DBGF(fmt, ...) pc.printf(fmt, __VA_ARGS__)
+#else
+#define DBG(...)
+#define DBGF(...)
+#endif
+
+
+
+//****************************************************************************
+//  XBusServo::XBusServo
+//    return :    none
+//    parameter : tx           pin name for tx
+//                rx           pin name for rx
+//                maxServoNum  max number of servo that you want to connect.
+//                             (limit 50)
+//                             this does just to resetve the buffer.  you need to
+//                             add XBus servo at the beginning of your sketch
+//
+//    Constructor
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusServo::XBusServo(PinName tx, PinName rx, uint8_t maxServoNum)
+    : RawSerial(tx, rx)                 //, DigitalInOut(tx)
+{
+    int        bufferSize;            // channel data packet buffer size
+
+    DBG("XBusServo::XBusServo\n");
+
+    // initialize serial
+    txPin = tx;
+    txOnly = (rx == NC);
+
+    // initialise vars
+    numOfServo = 0;
+    maxServo = maxServoNum;
+    if (maxServo > kXBusMaxServoNum)
+        maxServo = kXBusMaxServoNum;
+    else if (maxServo == 0)
+        maxServo = 1;
+    dirty = 0;
+    serialWriteBusy = 0;
+    recieveBufferPointer = 0;
+    modifyServosNow = 0;
+    sendLength = 0;
+    bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1;     // add 1 for CRC
+    if (bufferSize < kCmdDataPacketSize)
+        bufferSize = kCmdDataPacketSize;
+    chPacketBuffer = (uint8_t*)malloc(bufferSize);
+    sendBuffer = (uint8_t*)malloc(bufferSize);
+
+    // initialize channel packer buffer
+    chPacketBuffer[kCHDataPacketCommand]    = kXBusCmd_ModeA;
+    chPacketBuffer[kCHDataPacketLength]     = 0x00;
+    chPacketBuffer[kCHDataPacketKey]        = 0x00;
+    chPacketBuffer[kCHDataPacketType]       = 0x00;
+
+    // initialize serial
+    RawSerial::baud(kXBusBaudrate);
+    RawSerial::format(8, RawSerial::None, 1);
+#if DEVICE_SERIAL_FC
+    RawSerial::set_flow_control(RawSerial::Disabled, NC, NC);
+#endif
+    RawSerial::attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
+    RawSerial::attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
+
+//    DigitalInOut::mode(PullNone);
+//    DigitalInOut::input();
+    serial_pinout_tx(txPin);
+}
+
+
+//****************************************************************************
+//  XBusServo::~XBusServo
+//    return :    none
+//    parameter : none
+//
+//    Destructor
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusServo::~XBusServo()
+{
+    DBG("XBusServo::~XBusServo\n");
+
+    free(chPacketBuffer);
+    free(sendBuffer);
+}
+
+
+//****************************************************************************
+//  XBusServo::write
+//    return :    none
+//    parameter : buffer      data buffer to send
+//                length      data length on the buffer
+//
+//    start to send all packet data
+//    2014/09/30 : first write by Sawa
+//****************************************************************************
+void XBusServo::write(uint8_t* buffer, uint8_t length)
+{
+ //  DBG("XBusServo::write\n");
+
+    if (serialWriteBusy)
+        return;
+
+    serialWriteBusy = 1;
+    XBusServo::sendLength = length;
+    sendBufferPointer = buffer;
+
+    if (putc(*sendBufferPointer++) < 0) {
+        serialWriteBusy = 0;
+        XBusServo::sendLength = 0;
+    } else
+        XBusServo::sendLength--;
+}
+
+
+//****************************************************************************
+//  XBusServo::flush
+//    return :    none
+//    parameter : none
+//
+//    wait to send all packet data
+//    2014/09/30 : first write by Sawa
+//****************************************************************************
+void XBusServo::flush(void)
+{
+//    DBG("XBusServo::flush\n");
+
+    while(serialWriteBusy)
+        ;
+}
+
+
+//****************************************************************************
+//  XBusServo::TxIrqHandler
+//    return :    none
+//    parameter : none
+//
+//    handler for Tx buffer empty
+//    2014/09/30 : first write by Sawa
+//****************************************************************************
+void XBusServo::TxIrqHandler(void)
+{
+//    DBG("XBusServo::TxIrqHandler\n");
+
+    if (! serialWriteBusy)
+        return;
+
+    if (XBusServo::sendLength <= 0)
+        serialWriteBusy = 0;
+    else {
+        if (putc(*sendBufferPointer++) < 0) {
+            serialWriteBusy = 0;
+            XBusServo::sendLength = 0;
+        } else
+            XBusServo::sendLength--;
+    }
+}
+
+
+//****************************************************************************
+//  XBusServo::RxIrqHandler
+//    return :    none
+//    parameter : none
+//
+//    handler for Rx buffer full
+//    2014/09/30 : first write by Sawa
+//****************************************************************************
+void XBusServo::RxIrqHandler(void)
+{
+//    DBG("XBusServo::RxIrqHandler\n");
+
+    recieveBuffer[recieveBufferPointer++] = getc();
+    if (recieveBufferPointer >= kRecieveBufferSize)
+        recieveBufferPointer = 0;
+}
+
+
+
+//****************************************************************************
+//  XBusServo::sendChannelDataPacket
+//    return :    none
+//    parameter : none
+//
+//    This should be called on the timer handler like MsTimer2 when you
+//    use the XBus servo.
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+void XBusServo::sendChannelDataPacket(void)
+{
+//    DBG("XBusServo::sendChannelDataPacket\n");
+
+    if (modifyServosNow)
+        return;
+
+    if (numOfServo > 0) {
+        if (dirty) {
+            memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 3);
+            dirty = 0;
+        }
+
+        write(sendBuffer, sendBuffer[kCHDataPacketLength] + 3);
+    }
+}
+
+
+//****************************************************************************
+//  XBusServo::sendCommandDataPacket
+//    return :    error code
+//    parameter : command     The commnad that you want to send
+//                channelID   The channel ID of the XBus servo that you want to set up
+//                order       The order that you want to set up
+//                value       The value that you want to set / get
+//                valueSize   The value size.  1 byte(char) or 2 byte(int)
+//
+//    This should NOT be called on the timer handler like MsTimer2 when you
+//    setup the XBus servo.
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
+{
+    int                 sendSize;
+    uint8_t*            recievedPacket;
+
+    DBG("XBusServo::sendCommandDataPacket\n");
+
+    // setup command
+    sendBuffer[kCmdDataPacketCommand] = command;
+    sendBuffer[kCmdDataPacketLength] = valueSize + 3;
+    sendBuffer[kCmdDataPacketKey] = 0x00;
+    sendBuffer[kCmdDataPacketCH_ID] = channelID;
+    sendBuffer[kCmdDataPacketOrder] = order;
+    if (valueSize == 1) {                   // 1 byte value
+        sendBuffer[kCmdDataPacketData1] = *value & 0x00FF;
+        sendBuffer[kCmdDataPacketData2] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
+    } else {
+        sendBuffer[kCmdDataPacketData1] = (*value >> 8) & 0x00FF;
+        sendBuffer[kCmdDataPacketData2] = *value & 0x00FF;
+        sendBuffer[kCmdDataPacketCRC] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
+    }
+
+    // to recover channel data packet when you call sendChannelDataPacket after this
+    dirty = 1;
+
+    // send command
+    recieveBufferPointer = 0;                                   // reset recieve buffer
+    sendSize = sendBuffer[kCmdDataPacketLength] + 3;
+    write(sendBuffer, sendSize);
+    flush();                                                    // wait to send all bytes
+
+    // if it's tx only mode, it done
+    if (txOnly)
+        return kXBusError_NoError;
+
+    if (channelID != 0) {
+        // change bus direction to Rx mode
+//       DigitalInOut::input();
+
+        recievedPacket = &(recieveBuffer[sendSize]);
+
+        // wait to read all packet
+        while(recieveBufferPointer < (sendSize + kCmdDataPacketSize))
+            ;
+
+        // change bus direction to Tx mode
+        serial_pinout_tx(txPin);
+
+        // check CRC
+        if (crc8(recievedPacket, recievedPacket[kCHDataPacketLength] + 3) != 0)
+            return kXBusError_CRCError;
+
+        // check unsupported
+        if (recievedPacket[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported)
+            return kXBusError_Unsupported;
+
+        // send back the value
+        if (valueSize == 1) {                   // 1 byte value
+            *value = recievedPacket[kCmdDataPacketData1];
+            if (*value & 0x0080)
+                *value |= 0xFF00;
+        } else {
+            *value = recievedPacket[kCmdDataPacketData1];
+            *value <<= 8;
+            *value |= recievedPacket[kCmdDataPacketData2];
+        }
+    }
+
+    return kXBusError_NoError;
+}
+
+
+//****************************************************************************
+//  XBusServo::addServo
+//    return :    error code
+//    parameter : channelID   channel ID of the XBus servo that you want to use
+//                initValue   initial value of this XBus servo
+//                            use kXbusServoNeutral for center of the XBus servo
+//
+//    add new servo to the buffer on this library
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::addServo(uint8_t channelID, uint16_t initValue)
+{
+    uint16_t         dataOffset;
+
+    DBG("XBusServo::addServo\n");
+
+    // max check
+    if (numOfServo >= maxServo)
+        return kXBusError_ServoNumOverflow;
+
+    // convert to servo ID
+    channelID &= 0x3F;
+
+    // scan servo ID that is already added
+    if (numOfServo > 0) {
+        uint16_t     servoNo;
+
+        for (servoNo = 0; servoNo < numOfServo; servoNo++)
+            if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID)
+                return kXBusError_AddWithSameID;                            // found same servo ID
+    }
+
+    // atomic flag on
+    modifyServosNow = 1;
+
+    // add new servo
+    dataOffset = kStartOffsetOfCHData + kCHDataSize * numOfServo;
+    numOfServo++;
+    chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type
+
+    chPacketBuffer[dataOffset] = channelID;
+    chPacketBuffer[dataOffset + 1] = 0x00;
+    chPacketBuffer[dataOffset + 2] = (initValue >> 8) & 0x00FF;
+    chPacketBuffer[dataOffset + 3] = initValue & 0x00FF;
+
+    // calc CRC
+    chPacketBuffer[dataOffset + 4] = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
+    dirty = 1;
+
+    // atomic flag off
+    modifyServosNow = 0;
+
+    return kXBusError_NoError;
+}
+
+
+//****************************************************************************
+//  XBusServo::removeServo
+//    return :    error code
+//    parameter : channelID   channel ID of the XBus servo that you want to remove
+//
+//    remove the servo from the buffer on this library
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::removeServo(uint8_t channelID)
+{
+    DBG("XBusServo::removeServo\n");
+
+    // min check
+    if (numOfServo == 0)
+        return kXBusError_ServoNumIsZero;
+
+    // convert to servo ID
+    channelID &= 0x3F;
+
+    // scan servo ID that is already added
+    if (numOfServo > 0) {
+        uint16_t     servoNo;
+
+        for (servoNo = 0; servoNo < numOfServo; servoNo++)
+            if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID) {
+                // atomic flag on
+                modifyServosNow = 1;
+
+                // copy data after that
+                if (servoNo < (numOfServo - 1))
+                    memcpy(&(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo]),
+                           &(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * (servoNo + 1)]),
+                           kCHDataSize * (numOfServo - servoNo - 1));
+
+                // update packet size
+                numOfServo--;
+                chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type
+
+                // calc CRC
+                chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
+                = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
+                dirty = 1;
+
+                // atomic flag off
+                modifyServosNow = 0;
+
+                return kXBusError_NoError;
+            }
+    }
+
+    return kXBusError_IDNotFound;
+}
+
+
+//****************************************************************************
+//  XBusServo::setServo
+//    return :    error code
+//    parameter : channelID   channel ID of the XBus servo that you want to set
+//                value       value of this XBus servo
+//                            use kXbusServoNeutral for center of the XBus servo
+//
+//    remove the servo from the buffer on this library
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::setServo(uint8_t channelID, uint16_t value)
+{
+    uint16_t         dataOffset;
+
+    DBG("XBusServo::setServo\n");
+
+    // convert to servo ID
+    channelID &= 0x3F;
+
+    // scan servo ID that is already added
+    if (numOfServo > 0) {
+        int     servoNo;
+
+        for (servoNo = 0; servoNo < numOfServo; servoNo++) {
+            dataOffset = kStartOffsetOfCHData + kCHDataSize * servoNo;
+
+            if (chPacketBuffer[dataOffset] == channelID) {
+                // atomic flag on
+                modifyServosNow = 1;
+
+                // set value
+                chPacketBuffer[dataOffset + 2] = (value >> 8) & 0x00FF;
+                chPacketBuffer[dataOffset + 3] = value & 0x00FF;
+
+                // calc CRC
+                chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
+                = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
+                dirty = 1;
+
+                // atomic flag off
+                modifyServosNow = 0;
+
+                return kXBusError_NoError;
+            }
+        }
+    }
+
+    return kXBusError_IDNotFound;
+}
+
+
+//****************************************************************************
+//  XBusServo::setChannelID
+//    return :    error code
+//    parameter : oldChannelID    channel IDof the XBus servo to change the ID
+//                newChannelID    new channel ID for the XBus servo
+//
+//    set new channel ID to the XBus servo
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::setChannelID(uint8_t oldChannelID, uint8_t newChannelID)
+{
+    XBusError       result;
+    int16_t         value;
+
+    DBG("XBusServo::setChannelID\n");
+
+    value = kXBusMode_IDSet;
+    result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_Mode, &value, 1);
+    if (result != kXBusError_NoError)
+        return result;
+
+    value = newChannelID;
+    result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_ID, &value, 1);
+
+    return result;
+}
+
+
+//****************************************************************************
+//  XBusServo::setChannelID
+//    return :    error code
+//    parameter : newChannelID    new channel ID for the XBus servo
+//
+//    set new channel ID to the XBus servo
+//    this is only for TX only mode
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::setChannelID(uint8_t newChannelID)
+{
+    XBusError       result;
+    int16_t         value;
+
+    DBG("XBusServo::setChannelID\n");
+
+    if (txOnly)
+        return kXBusError_OnlyForTxOnlyMode;
+
+    value = kXBusMode_IDSet;
+    result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_Mode, &value, 1);
+    if (result != kXBusError_NoError)
+        return result;
+
+    value = newChannelID;
+    result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_ID, &value, 1);
+
+    return result;
+}
+
+
+//****************************************************************************
+//  XBusServo::getDataSize
+//    return :    data size for this order
+//    parameter : order       the order that you want to know
+//
+//    get the data size of this order
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+uint8_t XBusServo::getDataSize(uint8_t order)
+{
+    uint8_t        dataSize = 1;
+
+    DBG("XBusServo::getDataSize\n");
+
+    switch(order) {
+        case kXBusOrder_2_Version:      // only for get
+        case kXBusOrder_2_Product:      // only for get
+        case kXBusOrder_2_Reset:        // only for set
+        case kXBusOrder_2_ParamWrite:   // only for set
+        case kXBusOrder_2_Reverse:
+        case kXBusOrder_2_Neutral:
+        case kXBusOrder_2_H_Travel:
+        case kXBusOrder_2_L_Travel:
+        case kXBusOrder_2_H_Limit:
+        case kXBusOrder_2_L_Limit:
+        case kXBusOrder_2_PowerOffset:
+        case kXBusOrder_2_AlarmDelay:
+        case kXBusOrder_2_CurrentPos:   // only for get
+        case kXBusOrder_2_MaxInteger:
+            dataSize = 2;
+    }
+
+    return dataSize;
+}
+
+
+//****************************************************************************
+//  XBusServo::setCommand
+//    return :    error code
+//    parameter : channelID   channel ID of the XBus servo that you want to set to
+//                order       the order that you want
+//                value       the value that you want to set and return current value
+//
+//    send set command to the XBus servo
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::setCommand(uint8_t channelID, uint8_t order, int16_t* value)
+{
+    DBG("XBusServo::setCommand\n");
+
+    return sendCommandDataPacket(kXBusCmd_Set, channelID, order, value, getDataSize(order));
+}
+
+
+//****************************************************************************
+//  XBusServo::getCommand
+//    return :    error code
+//    parameter : channelID   channel ID of the XBus servo that you want to get from
+//                order       the order that you want
+//                value       the value that you want to get from
+//
+//    send get command to the XBus servo
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::getCommand(uint8_t channelID, uint8_t order, int16_t* value)
+{
+    DBG("XBusServo::getCommand\n");
+
+    return sendCommandDataPacket(kXBusCmd_Get, channelID, order, value, getDataSize(order));
+}
+
+
+//****************************************************************************
+//  XBusServo::setCommand
+//    return :    error code
+//    parameter : order       the order that you want
+//                value       the value that you want to set and return current value
+//
+//    send set command to the XBus servo
+//    this is only for TX only mode
+//    2014/09/02 : move from Arduino lib by Sawa
+//****************************************************************************
+XBusError XBusServo::setCommand(uint8_t order, int16_t* value)
+{
+    DBG("XBusServo::setCommand\n");
+
+    if (txOnly)
+        return kXBusError_OnlyForTxOnlyMode;
+
+    return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));
+}
+
+
+
+
+
+
+//****************************************************************************
+// for CRC
+static uint8_t s_crc_array[256] = {
+    0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
+    0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
+    0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
+    0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
+    0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
+    0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+    0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
+    0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
+    0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
+    0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
+    0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
+    0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+    0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
+    0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
+    0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
+    0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
+    0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
+    0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+    0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
+    0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
+    0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
+    0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
+    0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
+    0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+    0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
+    0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
+    0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
+    0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
+    0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
+    0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+    0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
+    0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
+};
+
+
+uint8_t XBusServo::crc_table(uint8_t  data, uint8_t  crc)
+{
+    uint16_t  index = (data ^ crc) & 0xff;
+
+    crc = s_crc_array[index];
+    return crc;
+}
+
+
+uint8_t XBusServo::crc8(uint8_t* buffer, uint8_t  length)
+{
+    uint8_t  crc = 0;
+
+    while (length-- > 0)
+        crc = crc_table(*buffer++, crc);
+    return crc;
+}
+
+
+
+