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-10-08
- Revision:
- 2:4aca5ffce457
- Parent:
- 1:bd80d3e8f3a3
- Child:
- 3:9a10ad81d04e
File content as of revision 2:4aca5ffce457:
/** @file * XBusServo.h file * * for mbed * * Copyright (c) 2014-2014 JR PROPO * Released under the MIT License: http://mbed.org/license/mit * * by Zak Sawa */ #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 // 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; // 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; class XBusServo // : private RawSerial { public: /** XBusServo::XBusServo @param tx pin name for tx @param rx pin name for rx @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 @bref Constructor 2014/09/02 : move from Arduino lib by Sawa */ XBusServo(PinName tx, PinName rx, uint8_t maxServoNum); public: XBusError start(); void stop(); XBusError addServo(uint8_t channelID, uint16_t initValue); XBusError removeServo(uint8_t channelID); XBusError setServo(uint8_t channelID, uint16_t value); void sendChannelDataPacket(void); XBusError setChannelID(uint8_t oldChannelID, uint8_t newChannelID); XBusError setCommand(uint8_t channelID, uint8_t order, int16_t* value); XBusError getCommand(uint8_t channelID, uint8_t order, int16_t* value); XBusError setChannelID(uint8_t newChannelID); XBusError setCommand(uint8_t order, int16_t* value); private: #define kRecieveBufferSize 64 RawSerial XBusPort; 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 uint8_t* sendBufferPointer; uint8_t sendLength; uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer uint8_t recieveBufferPointer; uint8_t dirty; // uint8_t serialWriteBusy; // volatile uint8_t modifyServosNow; // uint8_t crc_table(uint8_t data, uint8_t crc); uint8_t crc8(uint8_t* buffer, uint8_t length); uint8_t getDataSize(uint8_t order); void write(uint8_t* buffer, uint8_t length); void flush(void); void TxIrqHandler(void); void RxIrqHandler(void); XBusError sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize); }; #endif // of XBusServo_h