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.h
- Committer:
- sawa
- Date:
- 2014-11-05
- Revision:
- 18:75ddf12d93b6
- Parent:
- 17:3ffb2e3e3bec
File content as of revision 18:75ddf12d93b6:
/** * @file XBusServo.h * @brief this header file will contain all required * definitions and basic utilities functions * for controling XBus servo. * @author Zak Sawa * @note Copyright (c) 2014-2014 JR PROPO * @note Released under the MIT License: http://mbed.org/license/mit */ #ifndef XBusServo_h #define XBusServo_h #include "mbed.h" /// standard interval time for XBus servo #define kXBusStandardInterval 14 // mSec /// angle data for XBus servo #define kXbusServoMinimum 0x1249 // 900uSec -60 or -90 degree #define kXbusServoNeutral 0x7FFF // 1500uSec 0 degree #define kXbusServoMaximum 0xEDB6 // 2100uSec 60 or 90 degree /// maximum number of XBus servo #define kXBusMaxServoNum 50 /// convert ServoID & SubID to real Channel ID #define ChannelID(ServoID, SubID) ((ServoID & 0x3F) | ((SubID << 6) & 0xC0)) //! 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, kXBusOrder_2_Version = 0x04, // only for get kXBusOrder_2_Product = 0x05, // only for get kXBusOrder_1_Unsupported = 0x06, // only for status kXBusOrder_2_Reset = 0x07, // only for set kXBusOrder_2_ParamWrite = 0x08, // only for set kXBusOrder_2_Reverse = 0x10, kXBusOrder_2_Neutral = 0x11, kXBusOrder_2_H_Travel = 0x12, kXBusOrder_2_L_Travel = 0x13, kXBusOrder_2_H_Limit = 0x14, kXBusOrder_2_L_Limit = 0x15, kXBusOrder_1_P_Gain = 0x16, kXBusOrder_1_I_Gain = 0x17, kXBusOrder_1_D_Gain = 0x18, kXBusOrder_1_DeadBand = 0x19, kXBusOrder_2_PowerOffset = 0x1A, kXBusOrder_1_AlarmLevel = 0x1B, kXBusOrder_2_AlarmDelay = 0x1C, kXBusOrder_1_Angle_180 = 0x1D, kXBusOrder_1_SlowStart = 0x1E, kXBusOrder_1_StopMode = 0x1F, kXBusOrder_2_CurrentPos = 0x20, // only for get kXBusOrder_1_CurrentPow = 0x21, // only for get kXBusOrder_1_SpeedLimit = 0x22, kXBusOrder_2_MaxInteger = 0x23, } XBusOrder; /** * @brief Library class for XBus servo * @note 2014/09/02 : move from Arduino lib by Sawa */ 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 * add XBus servo at the beginning of your sketch * * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusServo(PinName tx, PinName rx, PinName sw, uint8_t maxServoNum); public: /** * @return XBus error code * @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 * This should NOT be called on interrupt handler. * @author 2014/10/07 : Zak Sawa */ void stop(); /** * @return XBus error code * @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 * 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 * 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 * 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); /** * @brief This should be called on the timer handler when you * use the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ void sendChannelDataPacket(void); /** * @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 * 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 * 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 * 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); /** * @return XBus error code * @param newChannelID new channel ID for 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 * 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 setCommand(uint8_t order, int16_t* value); private: #define kRecieveBufferSize 32 RawSerial XBusPort; DigitalOut TxSwitch; Timeout TxSwitchTimer; uint8_t txOnly; // 1 for TX only mode PinName txPin; // uint8_t numOfServo; // number of servos uint8_t maxServo; // max number of servos uint8_t* chPacketBuffer; // channel data packet buffer uint8_t* sendBuffer; // serial send buffer volatile uint8_t* sendBufferPointer; volatile uint8_t sendLength; volatile uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer volatile int8_t recieveBufferPointer; uint8_t dirty; // 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 * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ uint8_t getDataSize(uint8_t order); /** * @param buffer data buffer to send * @param length data length on the buffer * @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 * @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 interrupt handler when you * setup the XBus servo. * @author 2014/09/02 : move from Arduino lib by Zak Sawa */ XBusError sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize); }; #endif // of XBusServo_h