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@3:9a10ad81d04e, 2014-10-08 (annotated)
- Committer:
- sawa
- Date:
- Wed Oct 08 03:25:28 2014 +0000
- Revision:
- 3:9a10ad81d04e
- Parent:
- 2:4aca5ffce457
- Child:
- 4:d88f498d259b
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sawa | 3:9a10ad81d04e | 1 | /** |
sawa | 3:9a10ad81d04e | 2 | * @file XBusServo.h |
sawa | 3:9a10ad81d04e | 3 | * @brief this header file will contain all required |
sawa | 3:9a10ad81d04e | 4 | * definitions and basic utilities functions. |
sawa | 3:9a10ad81d04e | 5 | * @author Zak Sawa |
sawa | 3:9a10ad81d04e | 6 | * @note Copyright (c) 2014-2014 JR PROPO |
sawa | 3:9a10ad81d04e | 7 | * @note Released under the MIT License: http://mbed.org/license/mit |
sawa | 0:381d475cfd6c | 8 | */ |
sawa | 0:381d475cfd6c | 9 | |
sawa | 0:381d475cfd6c | 10 | #ifndef XBusServo_h |
sawa | 0:381d475cfd6c | 11 | #define XBusServo_h |
sawa | 0:381d475cfd6c | 12 | |
sawa | 0:381d475cfd6c | 13 | #include "mbed.h" |
sawa | 0:381d475cfd6c | 14 | |
sawa | 1:bd80d3e8f3a3 | 15 | // standard interval time for XBus servo |
sawa | 1:bd80d3e8f3a3 | 16 | #define kXBusStandardInterval 14 // mSec |
sawa | 0:381d475cfd6c | 17 | |
sawa | 1:bd80d3e8f3a3 | 18 | // angle data for XBus servo |
sawa | 1:bd80d3e8f3a3 | 19 | #define kXbusServoMinimum 0x1249 // 900uSec -60 or -90 degree |
sawa | 1:bd80d3e8f3a3 | 20 | #define kXbusServoNeutral 0x7FFF // 1500uSec 0 degree |
sawa | 1:bd80d3e8f3a3 | 21 | #define kXbusServoMaximum 0xEDB6 // 2100uSec 60 or 90 degree |
sawa | 1:bd80d3e8f3a3 | 22 | |
sawa | 1:bd80d3e8f3a3 | 23 | // maximum number of XBus servo |
sawa | 0:381d475cfd6c | 24 | #define kXBusMaxServoNum 50 |
sawa | 0:381d475cfd6c | 25 | |
sawa | 0:381d475cfd6c | 26 | // XBus Get/Set/Status command order |
sawa | 0:381d475cfd6c | 27 | typedef enum { |
sawa | 0:381d475cfd6c | 28 | kXBusOrder_1_Mode = 0x01, |
sawa | 0:381d475cfd6c | 29 | kXBusOrder_1_ID = 0x03, |
sawa | 0:381d475cfd6c | 30 | kXBusOrder_2_Version = 0x04, // only for get |
sawa | 0:381d475cfd6c | 31 | kXBusOrder_2_Product = 0x05, // only for get |
sawa | 0:381d475cfd6c | 32 | kXBusOrder_1_Unsupported = 0x06, // only for status |
sawa | 0:381d475cfd6c | 33 | kXBusOrder_2_Reset = 0x07, // only for set |
sawa | 0:381d475cfd6c | 34 | kXBusOrder_2_ParamWrite = 0x08, // only for set |
sawa | 0:381d475cfd6c | 35 | |
sawa | 0:381d475cfd6c | 36 | kXBusOrder_2_Reverse = 0x10, |
sawa | 0:381d475cfd6c | 37 | kXBusOrder_2_Neutral = 0x11, |
sawa | 0:381d475cfd6c | 38 | kXBusOrder_2_H_Travel = 0x12, |
sawa | 0:381d475cfd6c | 39 | kXBusOrder_2_L_Travel = 0x13, |
sawa | 0:381d475cfd6c | 40 | kXBusOrder_2_H_Limit = 0x14, |
sawa | 0:381d475cfd6c | 41 | kXBusOrder_2_L_Limit = 0x15, |
sawa | 0:381d475cfd6c | 42 | kXBusOrder_1_P_Gain = 0x16, |
sawa | 0:381d475cfd6c | 43 | kXBusOrder_1_I_Gain = 0x17, |
sawa | 0:381d475cfd6c | 44 | kXBusOrder_1_D_Gain = 0x18, |
sawa | 0:381d475cfd6c | 45 | kXBusOrder_1_DeadBand = 0x19, |
sawa | 0:381d475cfd6c | 46 | kXBusOrder_2_PowerOffset = 0x1A, |
sawa | 0:381d475cfd6c | 47 | kXBusOrder_1_AlarmLevel = 0x1B, |
sawa | 0:381d475cfd6c | 48 | kXBusOrder_2_AlarmDelay = 0x1C, |
sawa | 0:381d475cfd6c | 49 | kXBusOrder_1_Angle_180 = 0x1D, |
sawa | 0:381d475cfd6c | 50 | kXBusOrder_1_SlowStart = 0x1E, |
sawa | 0:381d475cfd6c | 51 | kXBusOrder_1_StopMode = 0x1F, |
sawa | 0:381d475cfd6c | 52 | kXBusOrder_2_CurrentPos = 0x20, // only for get |
sawa | 0:381d475cfd6c | 53 | kXBusOrder_1_CurrentPow = 0x21, // only for get |
sawa | 0:381d475cfd6c | 54 | kXBusOrder_1_SpeedLimit = 0x22, |
sawa | 0:381d475cfd6c | 55 | kXBusOrder_2_MaxInteger = 0x23, |
sawa | 0:381d475cfd6c | 56 | } XBusOrder; |
sawa | 0:381d475cfd6c | 57 | |
sawa | 0:381d475cfd6c | 58 | |
sawa | 0:381d475cfd6c | 59 | // XBus parameter index |
sawa | 0:381d475cfd6c | 60 | typedef enum { |
sawa | 0:381d475cfd6c | 61 | kParamIdx_Unused0 = 0x0000, |
sawa | 0:381d475cfd6c | 62 | kParamIdx_AllData_wID = 0x0001, |
sawa | 0:381d475cfd6c | 63 | kParamIdx_AllData_woID = 0x0002, |
sawa | 0:381d475cfd6c | 64 | kParamIdx_ServoID = 0x0003, |
sawa | 0:381d475cfd6c | 65 | kParamIdx_Reversed = 0x0004, |
sawa | 0:381d475cfd6c | 66 | kParamIdx_NeutralOffset = 0x0005, |
sawa | 0:381d475cfd6c | 67 | kParamIdx_TravelHigh = 0x0006, |
sawa | 0:381d475cfd6c | 68 | kParamIdx_TravelLow = 0x0007, |
sawa | 0:381d475cfd6c | 69 | kParamIdx_LimitHigh = 0x0008, |
sawa | 0:381d475cfd6c | 70 | kParamIdx_LimitLow = 0x0009, |
sawa | 0:381d475cfd6c | 71 | kParamIdx_PGainDiff = 0x000A, |
sawa | 0:381d475cfd6c | 72 | kParamIdx_IGainDiff = 0x000B, |
sawa | 0:381d475cfd6c | 73 | kParamIdx_DGainDiff = 0x000C, |
sawa | 0:381d475cfd6c | 74 | kParamIdx_DeadBandDiff = 0x000D, |
sawa | 0:381d475cfd6c | 75 | kParamIdx_PWOffsetDiff = 0x000E, |
sawa | 0:381d475cfd6c | 76 | kParamIdx_AlarmLevel = 0x000F, |
sawa | 0:381d475cfd6c | 77 | kParamIdx_AlarmDelay = 0x0010, |
sawa | 0:381d475cfd6c | 78 | kParamIdx_Angle_180 = 0x0011, |
sawa | 0:381d475cfd6c | 79 | kParamIdx_SlowStart = 0x0012, |
sawa | 0:381d475cfd6c | 80 | kParamIdx_StopMode = 0x0013, |
sawa | 0:381d475cfd6c | 81 | kParamIdx_SpeedLimit = 0x0014, |
sawa | 0:381d475cfd6c | 82 | kParamIdx_MaxIntegerDiff = 0x0015, |
sawa | 0:381d475cfd6c | 83 | } XBusParamIdx; |
sawa | 0:381d475cfd6c | 84 | |
sawa | 0:381d475cfd6c | 85 | |
sawa | 0:381d475cfd6c | 86 | // XBus error code |
sawa | 0:381d475cfd6c | 87 | typedef enum { |
sawa | 0:381d475cfd6c | 88 | kXBusError_NoError = 0x0000, |
sawa | 0:381d475cfd6c | 89 | kXBusError_CRCError, |
sawa | 0:381d475cfd6c | 90 | kXBusError_ServoNumOverflow, |
sawa | 0:381d475cfd6c | 91 | kXBusError_ServoNumIsZero, |
sawa | 0:381d475cfd6c | 92 | kXBusError_AddWithSameID, |
sawa | 0:381d475cfd6c | 93 | kXBusError_IDNotFound, |
sawa | 0:381d475cfd6c | 94 | kXBusError_Unsupported, |
sawa | 0:381d475cfd6c | 95 | kXBusError_OnlyForTxOnlyMode, |
sawa | 0:381d475cfd6c | 96 | kXBusError_OnlyForNormalMode, |
sawa | 0:381d475cfd6c | 97 | |
sawa | 0:381d475cfd6c | 98 | kXBusError_NumOfError, |
sawa | 0:381d475cfd6c | 99 | } XBusError; |
sawa | 0:381d475cfd6c | 100 | |
sawa | 0:381d475cfd6c | 101 | |
sawa | 0:381d475cfd6c | 102 | |
sawa | 2:4aca5ffce457 | 103 | class XBusServo // : private RawSerial |
sawa | 0:381d475cfd6c | 104 | { |
sawa | 0:381d475cfd6c | 105 | public: |
sawa | 3:9a10ad81d04e | 106 | /** XBusServo::XBusServo |
sawa | 3:9a10ad81d04e | 107 | @param tx pin name for tx |
sawa | 3:9a10ad81d04e | 108 | @param rx pin name for rx |
sawa | 3:9a10ad81d04e | 109 | @param maxServoNum max number of servo that you want to connect. |
sawa | 3:9a10ad81d04e | 110 | (limit 50) |
sawa | 3:9a10ad81d04e | 111 | this does just to resetve the buffer. you need to |
sawa | 3:9a10ad81d04e | 112 | add XBus servo at the beginning of your sketch |
sawa | 2:4aca5ffce457 | 113 | |
sawa | 3:9a10ad81d04e | 114 | @bref Constructor |
sawa | 3:9a10ad81d04e | 115 | |
sawa | 3:9a10ad81d04e | 116 | 2014/09/02 : move from Arduino lib by Sawa |
sawa | 3:9a10ad81d04e | 117 | */ |
sawa | 0:381d475cfd6c | 118 | XBusServo(PinName tx, PinName rx, uint8_t maxServoNum); |
sawa | 0:381d475cfd6c | 119 | |
sawa | 0:381d475cfd6c | 120 | public: |
sawa | 1:bd80d3e8f3a3 | 121 | XBusError start(); |
sawa | 1:bd80d3e8f3a3 | 122 | void stop(); |
sawa | 1:bd80d3e8f3a3 | 123 | |
sawa | 0:381d475cfd6c | 124 | XBusError addServo(uint8_t channelID, uint16_t initValue); |
sawa | 0:381d475cfd6c | 125 | XBusError removeServo(uint8_t channelID); |
sawa | 0:381d475cfd6c | 126 | XBusError setServo(uint8_t channelID, uint16_t value); |
sawa | 0:381d475cfd6c | 127 | |
sawa | 0:381d475cfd6c | 128 | void sendChannelDataPacket(void); |
sawa | 0:381d475cfd6c | 129 | |
sawa | 0:381d475cfd6c | 130 | XBusError setChannelID(uint8_t oldChannelID, uint8_t newChannelID); |
sawa | 0:381d475cfd6c | 131 | XBusError setCommand(uint8_t channelID, uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 132 | XBusError getCommand(uint8_t channelID, uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 133 | |
sawa | 0:381d475cfd6c | 134 | XBusError setChannelID(uint8_t newChannelID); |
sawa | 0:381d475cfd6c | 135 | XBusError setCommand(uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 136 | |
sawa | 0:381d475cfd6c | 137 | |
sawa | 0:381d475cfd6c | 138 | private: |
sawa | 1:bd80d3e8f3a3 | 139 | #define kRecieveBufferSize 64 |
sawa | 1:bd80d3e8f3a3 | 140 | |
sawa | 1:bd80d3e8f3a3 | 141 | RawSerial XBusPort; |
sawa | 1:bd80d3e8f3a3 | 142 | |
sawa | 0:381d475cfd6c | 143 | uint8_t txOnly; // 1 for TX only mode |
sawa | 0:381d475cfd6c | 144 | PinName txPin; // |
sawa | 0:381d475cfd6c | 145 | uint8_t numOfServo; // number of servos |
sawa | 0:381d475cfd6c | 146 | uint8_t maxServo; // max number of servos |
sawa | 0:381d475cfd6c | 147 | uint8_t* chPacketBuffer; // channel data packet buffer |
sawa | 0:381d475cfd6c | 148 | uint8_t* sendBuffer; // serial send buffer |
sawa | 0:381d475cfd6c | 149 | uint8_t* sendBufferPointer; |
sawa | 0:381d475cfd6c | 150 | uint8_t sendLength; |
sawa | 0:381d475cfd6c | 151 | uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer |
sawa | 0:381d475cfd6c | 152 | uint8_t recieveBufferPointer; |
sawa | 0:381d475cfd6c | 153 | uint8_t dirty; // |
sawa | 0:381d475cfd6c | 154 | uint8_t serialWriteBusy; // |
sawa | 0:381d475cfd6c | 155 | volatile uint8_t modifyServosNow; // |
sawa | 0:381d475cfd6c | 156 | |
sawa | 0:381d475cfd6c | 157 | uint8_t crc_table(uint8_t data, uint8_t crc); |
sawa | 0:381d475cfd6c | 158 | uint8_t crc8(uint8_t* buffer, uint8_t length); |
sawa | 0:381d475cfd6c | 159 | uint8_t getDataSize(uint8_t order); |
sawa | 0:381d475cfd6c | 160 | |
sawa | 0:381d475cfd6c | 161 | void write(uint8_t* buffer, uint8_t length); |
sawa | 0:381d475cfd6c | 162 | void flush(void); |
sawa | 0:381d475cfd6c | 163 | void TxIrqHandler(void); |
sawa | 0:381d475cfd6c | 164 | void RxIrqHandler(void); |
sawa | 0:381d475cfd6c | 165 | |
sawa | 0:381d475cfd6c | 166 | XBusError sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize); |
sawa | 0:381d475cfd6c | 167 | }; |
sawa | 0:381d475cfd6c | 168 | |
sawa | 0:381d475cfd6c | 169 | |
sawa | 0:381d475cfd6c | 170 | #endif // of XBusServo_h |
sawa | 0:381d475cfd6c | 171 |