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
- Committer:
- sawa
- Date:
- 2014-10-08
- Revision:
- 1:bd80d3e8f3a3
- Parent:
- 0:381d475cfd6c
- Child:
- 2:4aca5ffce457
File content as of revision 1:bd80d3e8f3a3:
/* XBusServo.cpp file * * for mbed * * Copyright (c) 2014-2014 JR PROPO * Released under the MIT License: http://mbed.org/license/mit * * by Zak Sawa */ #include "XBusServo.h" #include "pinmap.h" #include "gpio_api.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 #define DBG(fmt) printf(fmt) #define DBGF(fmt, ...) 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) : XBusPort(tx, rx) { DBG("XBusServo::XBusServo\n"); // initialize serial txPin = tx; txOnly = (rx == NC); maxServo = maxServoNum; } //**************************************************************************** // XBusServo::start // return : error code // parameter : none // // start to use XBus // 2014/09/02 : move from Arduino lib by Sawa //**************************************************************************** XBusError XBusServo::start() { int bufferSize; // channel data packet buffer size DBG("XBusServo::start\n"); // initialise vars numOfServo = 0; 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 XBusPort.baud(kXBusBaudrate); XBusPort.format(8, RawSerial::None, 1); #if DEVICE_SERIAL_FC XBusPort.set_flow_control(RawSerial::Disabled, NC, NC); #endif #ifdef TARGET_KL25Z // do nothing here #else XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq); #endif XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq); serial_pinout_tx(txPin); return kXBusError_NoError; } //**************************************************************************** // XBusServo::stop // return : none // parameter : none // // stop to use XBus // 2014/09/02 : move from Arduino lib by Sawa //**************************************************************************** void XBusServo::stop() { DBG("XBusServo::stop\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 (XBusPort.putc(*sendBufferPointer++) < 0) { serialWriteBusy = 0; XBusServo::sendLength = 0; } else { XBusServo::sendLength--; #ifdef TARGET_KL25Z XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq); #endif } } //**************************************************************************** // 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; #ifdef TARGET_KL25Z XBusPort.attach(NULL, RawSerial::TxIrq); #endif } else { if (XBusPort.putc(*sendBufferPointer++) < 0) { serialWriteBusy = 0; XBusServo::sendLength = 0; #ifdef TARGET_KL25Z XBusPort.attach(NULL, RawSerial::TxIrq); #endif } 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++] = XBusPort.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; }