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上でのみ、動作確認しています
Diff: XBusServo.h
- Revision:
- 17:3ffb2e3e3bec
- Parent:
- 14:8f44990ecb74
- Child:
- 18:75ddf12d93b6
diff -r e283810b53c3 -r 3ffb2e3e3bec XBusServo.h --- a/XBusServo.h Wed Oct 08 07:39:19 2014 +0000 +++ b/XBusServo.h Thu Oct 30 05:11:46 2014 +0000 @@ -12,6 +12,7 @@ #define XBusServo_h #include "mbed.h" +//#include "MODSERIAL.h" /// standard interval time for XBus servo #define kXBusStandardInterval 14 // mSec @@ -24,7 +25,50 @@ /// maximum number of XBus servo #define kXBusMaxServoNum 50 -/// XBus Get/Set/Status command order + +//! XBus error code +typedef enum { + kXBusError_NoError = 0x0000, + kXBusError_CRCError, + kXBusError_ServoNumOverflow, + kXBusError_ServoNumIsZero, + kXBusError_AddWithSameID, + kXBusError_IDNotFound, + kXBusError_Unsupported, + kXBusError_OnlyForTxOnlyMode, + kXBusError_OnlyForNormalMode, + kXBusError_MemoryFull, + + kXBusError_NumOfError, +} XBusError; + +//! XBus parameter index +typedef enum { + kParamIdx_Unused0 = 0x0000, + kParamIdx_AllData_wID = 0x0001, + kParamIdx_AllData_woID = 0x0002, + kParamIdx_ServoID = 0x0003, + kParamIdx_Reversed = 0x0004, + kParamIdx_NeutralOffset = 0x0005, + kParamIdx_TravelHigh = 0x0006, + kParamIdx_TravelLow = 0x0007, + kParamIdx_LimitHigh = 0x0008, + kParamIdx_LimitLow = 0x0009, + kParamIdx_PGainDiff = 0x000A, + kParamIdx_IGainDiff = 0x000B, + kParamIdx_DGainDiff = 0x000C, + kParamIdx_DeadBandDiff = 0x000D, + kParamIdx_PWOffsetDiff = 0x000E, + kParamIdx_AlarmLevel = 0x000F, + kParamIdx_AlarmDelay = 0x0010, + kParamIdx_Angle_180 = 0x0011, + kParamIdx_SlowStart = 0x0012, + kParamIdx_StopMode = 0x0013, + kParamIdx_SpeedLimit = 0x0014, + kParamIdx_MaxIntegerDiff = 0x0015, +} XBusParamIdx; + +//! XBus Get/Set/Status command order typedef enum { kXBusOrder_1_Mode = 0x01, kXBusOrder_1_ID = 0x03, @@ -57,59 +101,19 @@ } XBusOrder; -/// XBus parameter index -typedef enum { - kParamIdx_Unused0 = 0x0000, - kParamIdx_AllData_wID = 0x0001, - kParamIdx_AllData_woID = 0x0002, - kParamIdx_ServoID = 0x0003, - kParamIdx_Reversed = 0x0004, - kParamIdx_NeutralOffset = 0x0005, - kParamIdx_TravelHigh = 0x0006, - kParamIdx_TravelLow = 0x0007, - kParamIdx_LimitHigh = 0x0008, - kParamIdx_LimitLow = 0x0009, - kParamIdx_PGainDiff = 0x000A, - kParamIdx_IGainDiff = 0x000B, - kParamIdx_DGainDiff = 0x000C, - kParamIdx_DeadBandDiff = 0x000D, - kParamIdx_PWOffsetDiff = 0x000E, - kParamIdx_AlarmLevel = 0x000F, - kParamIdx_AlarmDelay = 0x0010, - kParamIdx_Angle_180 = 0x0011, - kParamIdx_SlowStart = 0x0012, - kParamIdx_StopMode = 0x0013, - kParamIdx_SpeedLimit = 0x0014, - kParamIdx_MaxIntegerDiff = 0x0015, -} XBusParamIdx; - - -/// XBus error code -typedef enum { - kXBusError_NoError = 0x0000, - kXBusError_CRCError, - kXBusError_ServoNumOverflow, - kXBusError_ServoNumIsZero, - kXBusError_AddWithSameID, - kXBusError_IDNotFound, - kXBusError_Unsupported, - kXBusError_OnlyForTxOnlyMode, - kXBusError_OnlyForNormalMode, - - kXBusError_NumOfError, -} XBusError; - - /** * @brief Library class for XBus servo * @note 2014/09/02 : move from Arduino lib by Sawa */ -class XBusServo // : private RawSerial +class XBusServo { public: + + /** * @param tx pin name for tx * @param rx pin name for rx + * @param sw pin name for switch tx * @param maxServoNum max number of servo that you want to connect. * (limit 50) * this does just to resetve the buffer. you need to @@ -117,17 +121,20 @@ * * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ - XBusServo(PinName tx, PinName rx, uint8_t maxServoNum); + XBusServo(PinName tx, PinName rx, PinName sw, uint8_t maxServoNum); public: /** * @return XBus error code - * @brief start function to use XBus + * @brief Start function to use XBus + * This should NOT be called on interrupt handler. * @author 2014/10/07 : Zak Sawa */ XBusError start(); + /** - * @brief stop function to use XBus + * @brief Stop function to use XBus + * This should NOT be called on interrupt handler. * @author 2014/10/07 : Zak Sawa */ void stop(); @@ -137,23 +144,31 @@ * @param channelID channel ID of the XBus servo that you want to use * @param initValue initial value of this XBus servo * use kXbusServoNeutral for center of the XBus servo - * @brief add new servo to the buffer on this library + * @brief Add new servo to the buffer on this library + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError addServo(uint8_t channelID, uint16_t initValue); + /** * @return XBus error code * @param channelID channel ID of the XBus servo that you want to remove - * @brief remove the servo from the buffer on this library + * @brief Remove the servo from the buffer on this library + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError removeServo(uint8_t channelID); + /** * @return XBus error code * @param channelID channel ID of the XBus servo that you want to set * @param value value of this XBus servo * use kXbusServoNeutral for center of the XBus servo - * @brief set new value to the servo + * @brief Set new value to the servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError setServo(uint8_t channelID, uint16_t value); @@ -169,25 +184,33 @@ * @return XBus error code * @param oldChannelID channel IDof the XBus servo to change the ID * @param newChannelID new channel ID for the XBus servo - * @brief set new channel ID to the XBus servo + * @brief Set new channel ID to the XBus servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError setChannelID(uint8_t oldChannelID, uint8_t newChannelID); + /** * @return XBus error code * @param channelID channel ID of the XBus servo that you want to set to * @param order the order that you want * @param value the value that you want to set and return current value - * @brief send set command to the XBus servo + * @brief Send set command to the XBus servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError setCommand(uint8_t channelID, uint8_t order, int16_t* value); + /** * @return XBus error code * @param channelID channel ID of the XBus servo that you want to get from * @param order the order that you want * @param value the value that you want to get from - * @brief send get command to the XBus servo + * @brief Send get command to the XBus servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError getCommand(uint8_t channelID, uint8_t order, int16_t* value); @@ -195,16 +218,21 @@ /** * @return XBus error code * @param newChannelID new channel ID for the XBus servo - * @brief set new channel ID to the XBus servo + * @brief Set new channel ID to the XBus servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @note this is only for TX only mode * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError setChannelID(uint8_t newChannelID); + /** * @return XBus error code * @param order the order that you want * @param value the value that you want to set and return current value - * @brief send set command to the XBus servo + * @brief Send set command to the XBus servo + * This should NOT be called on interrupt handler when you + * setup the XBus servo. * @note this is only for TX only mode * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ @@ -212,9 +240,12 @@ private: -#define kRecieveBufferSize 64 +#define kRecieveBufferSize 32 +// MODSERIAL XBusPort; RawSerial XBusPort; + DigitalOut TxSwitch; + Timeout TxSwitchTimer; uint8_t txOnly; // 1 for TX only mode PinName txPin; // @@ -222,20 +253,21 @@ uint8_t maxServo; // max number of servos uint8_t* chPacketBuffer; // channel data packet buffer uint8_t* sendBuffer; // serial send buffer - uint8_t* sendBufferPointer; - uint8_t sendLength; + volatile uint8_t* sendBufferPointer; + volatile uint8_t sendLength; uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer - uint8_t recieveBufferPointer; + volatile int8_t recieveBufferPointer; uint8_t dirty; // - uint8_t serialWriteBusy; // + volatile uint8_t serialCommandBusy; // volatile uint8_t modifyServosNow; // + volatile uint8_t need2ReadData; uint8_t crc_table(uint8_t data, uint8_t crc); uint8_t crc8(uint8_t* buffer, uint8_t length); /** * @return data size for this order * @param order the order that you want to know - * @brief get the data size of this order + * @brief Get the data size of this order * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ uint8_t getDataSize(uint8_t order); @@ -243,33 +275,44 @@ /** * @param buffer data buffer to send * @param length data length on the buffer - * @brief start to send all packet data + * @brief Start to send all packet data * @author 2014/09/30 : Zak Sawa */ void write(uint8_t* buffer, uint8_t length); + /** - * @brief wait to send all packet data + * @brief Wait to send all packet data * @author 2014/09/30 : Zak Sawa */ void flush(void); + /** * @brief handler for Tx buffer empty * @author 2014/09/30 : Zak Sawa */ +// void TxIrqHandler(MODSERIAL_IRQ_INFO *q); void TxIrqHandler(void); + /** * @brief handler for Rx buffer full * @author 2014/09/30 : Zak Sawa */ +// void RxIrqHandler(MODSERIAL_IRQ_INFO *q); void RxIrqHandler(void); /** + * @brief handler for Tx switch + * @author 2014/10/29 : Zak Sawa + */ + void TxSwitchHandler(void); + + /** * @param command The commnad that you want to send * @param channelID The channel ID of the XBus servo that you want to set up * @param order The order that you want to set up * @param value The value that you want to set / get * @param valueSize The value size. 1 byte(char) or 2 byte(int) - * @brief This should NOT be called on the timer handler when you + * @brief This should NOT be called on interrupt handler when you * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */