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@0:381d475cfd6c, 2014-10-02 (annotated)
- Committer:
- sawa
- Date:
- Thu Oct 02 08:46:53 2014 +0000
- Revision:
- 0:381d475cfd6c
- Child:
- 1:bd80d3e8f3a3
proto 0.01
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sawa | 0:381d475cfd6c | 1 | /* XBusServo.h file |
sawa | 0:381d475cfd6c | 2 | * |
sawa | 0:381d475cfd6c | 3 | * for mbed |
sawa | 0:381d475cfd6c | 4 | * |
sawa | 0:381d475cfd6c | 5 | * Copyright (c) 2013-2014 JR PROPO |
sawa | 0:381d475cfd6c | 6 | * by Zak Sawa |
sawa | 0:381d475cfd6c | 7 | */ |
sawa | 0:381d475cfd6c | 8 | |
sawa | 0:381d475cfd6c | 9 | #ifndef XBusServo_h |
sawa | 0:381d475cfd6c | 10 | #define XBusServo_h |
sawa | 0:381d475cfd6c | 11 | |
sawa | 0:381d475cfd6c | 12 | #include "mbed.h" |
sawa | 0:381d475cfd6c | 13 | #include "gpio_api.h" |
sawa | 0:381d475cfd6c | 14 | |
sawa | 0:381d475cfd6c | 15 | |
sawa | 0:381d475cfd6c | 16 | #define kXBusInterval 14 // mSec |
sawa | 0:381d475cfd6c | 17 | #define kXbusServoNeutral 0x7FFF // 1500uSec |
sawa | 0:381d475cfd6c | 18 | #define kXBusMaxServoNum 50 |
sawa | 0:381d475cfd6c | 19 | |
sawa | 0:381d475cfd6c | 20 | // XBus Get/Set/Status command order |
sawa | 0:381d475cfd6c | 21 | typedef enum { |
sawa | 0:381d475cfd6c | 22 | kXBusOrder_1_Mode = 0x01, |
sawa | 0:381d475cfd6c | 23 | kXBusOrder_1_ID = 0x03, |
sawa | 0:381d475cfd6c | 24 | kXBusOrder_2_Version = 0x04, // only for get |
sawa | 0:381d475cfd6c | 25 | kXBusOrder_2_Product = 0x05, // only for get |
sawa | 0:381d475cfd6c | 26 | kXBusOrder_1_Unsupported = 0x06, // only for status |
sawa | 0:381d475cfd6c | 27 | kXBusOrder_2_Reset = 0x07, // only for set |
sawa | 0:381d475cfd6c | 28 | kXBusOrder_2_ParamWrite = 0x08, // only for set |
sawa | 0:381d475cfd6c | 29 | |
sawa | 0:381d475cfd6c | 30 | kXBusOrder_2_Reverse = 0x10, |
sawa | 0:381d475cfd6c | 31 | kXBusOrder_2_Neutral = 0x11, |
sawa | 0:381d475cfd6c | 32 | kXBusOrder_2_H_Travel = 0x12, |
sawa | 0:381d475cfd6c | 33 | kXBusOrder_2_L_Travel = 0x13, |
sawa | 0:381d475cfd6c | 34 | kXBusOrder_2_H_Limit = 0x14, |
sawa | 0:381d475cfd6c | 35 | kXBusOrder_2_L_Limit = 0x15, |
sawa | 0:381d475cfd6c | 36 | kXBusOrder_1_P_Gain = 0x16, |
sawa | 0:381d475cfd6c | 37 | kXBusOrder_1_I_Gain = 0x17, |
sawa | 0:381d475cfd6c | 38 | kXBusOrder_1_D_Gain = 0x18, |
sawa | 0:381d475cfd6c | 39 | kXBusOrder_1_DeadBand = 0x19, |
sawa | 0:381d475cfd6c | 40 | kXBusOrder_2_PowerOffset = 0x1A, |
sawa | 0:381d475cfd6c | 41 | kXBusOrder_1_AlarmLevel = 0x1B, |
sawa | 0:381d475cfd6c | 42 | kXBusOrder_2_AlarmDelay = 0x1C, |
sawa | 0:381d475cfd6c | 43 | kXBusOrder_1_Angle_180 = 0x1D, |
sawa | 0:381d475cfd6c | 44 | kXBusOrder_1_SlowStart = 0x1E, |
sawa | 0:381d475cfd6c | 45 | kXBusOrder_1_StopMode = 0x1F, |
sawa | 0:381d475cfd6c | 46 | kXBusOrder_2_CurrentPos = 0x20, // only for get |
sawa | 0:381d475cfd6c | 47 | kXBusOrder_1_CurrentPow = 0x21, // only for get |
sawa | 0:381d475cfd6c | 48 | kXBusOrder_1_SpeedLimit = 0x22, |
sawa | 0:381d475cfd6c | 49 | kXBusOrder_2_MaxInteger = 0x23, |
sawa | 0:381d475cfd6c | 50 | } XBusOrder; |
sawa | 0:381d475cfd6c | 51 | |
sawa | 0:381d475cfd6c | 52 | |
sawa | 0:381d475cfd6c | 53 | // XBus parameter index |
sawa | 0:381d475cfd6c | 54 | typedef enum { |
sawa | 0:381d475cfd6c | 55 | kParamIdx_Unused0 = 0x0000, |
sawa | 0:381d475cfd6c | 56 | kParamIdx_AllData_wID = 0x0001, |
sawa | 0:381d475cfd6c | 57 | kParamIdx_AllData_woID = 0x0002, |
sawa | 0:381d475cfd6c | 58 | kParamIdx_ServoID = 0x0003, |
sawa | 0:381d475cfd6c | 59 | kParamIdx_Reversed = 0x0004, |
sawa | 0:381d475cfd6c | 60 | kParamIdx_NeutralOffset = 0x0005, |
sawa | 0:381d475cfd6c | 61 | kParamIdx_TravelHigh = 0x0006, |
sawa | 0:381d475cfd6c | 62 | kParamIdx_TravelLow = 0x0007, |
sawa | 0:381d475cfd6c | 63 | kParamIdx_LimitHigh = 0x0008, |
sawa | 0:381d475cfd6c | 64 | kParamIdx_LimitLow = 0x0009, |
sawa | 0:381d475cfd6c | 65 | kParamIdx_PGainDiff = 0x000A, |
sawa | 0:381d475cfd6c | 66 | kParamIdx_IGainDiff = 0x000B, |
sawa | 0:381d475cfd6c | 67 | kParamIdx_DGainDiff = 0x000C, |
sawa | 0:381d475cfd6c | 68 | kParamIdx_DeadBandDiff = 0x000D, |
sawa | 0:381d475cfd6c | 69 | kParamIdx_PWOffsetDiff = 0x000E, |
sawa | 0:381d475cfd6c | 70 | kParamIdx_AlarmLevel = 0x000F, |
sawa | 0:381d475cfd6c | 71 | kParamIdx_AlarmDelay = 0x0010, |
sawa | 0:381d475cfd6c | 72 | kParamIdx_Angle_180 = 0x0011, |
sawa | 0:381d475cfd6c | 73 | kParamIdx_SlowStart = 0x0012, |
sawa | 0:381d475cfd6c | 74 | kParamIdx_StopMode = 0x0013, |
sawa | 0:381d475cfd6c | 75 | kParamIdx_SpeedLimit = 0x0014, |
sawa | 0:381d475cfd6c | 76 | kParamIdx_MaxIntegerDiff = 0x0015, |
sawa | 0:381d475cfd6c | 77 | } XBusParamIdx; |
sawa | 0:381d475cfd6c | 78 | |
sawa | 0:381d475cfd6c | 79 | |
sawa | 0:381d475cfd6c | 80 | // XBus error code |
sawa | 0:381d475cfd6c | 81 | typedef enum { |
sawa | 0:381d475cfd6c | 82 | kXBusError_NoError = 0x0000, |
sawa | 0:381d475cfd6c | 83 | kXBusError_CRCError, |
sawa | 0:381d475cfd6c | 84 | kXBusError_ServoNumOverflow, |
sawa | 0:381d475cfd6c | 85 | kXBusError_ServoNumIsZero, |
sawa | 0:381d475cfd6c | 86 | kXBusError_AddWithSameID, |
sawa | 0:381d475cfd6c | 87 | kXBusError_IDNotFound, |
sawa | 0:381d475cfd6c | 88 | kXBusError_Unsupported, |
sawa | 0:381d475cfd6c | 89 | kXBusError_OnlyForTxOnlyMode, |
sawa | 0:381d475cfd6c | 90 | kXBusError_OnlyForNormalMode, |
sawa | 0:381d475cfd6c | 91 | |
sawa | 0:381d475cfd6c | 92 | kXBusError_NumOfError, |
sawa | 0:381d475cfd6c | 93 | } XBusError; |
sawa | 0:381d475cfd6c | 94 | |
sawa | 0:381d475cfd6c | 95 | |
sawa | 0:381d475cfd6c | 96 | |
sawa | 0:381d475cfd6c | 97 | class XBusServo : private RawSerial //, private DigitalInOut |
sawa | 0:381d475cfd6c | 98 | { |
sawa | 0:381d475cfd6c | 99 | public: |
sawa | 0:381d475cfd6c | 100 | XBusServo(PinName tx, PinName rx, uint8_t maxServoNum); |
sawa | 0:381d475cfd6c | 101 | virtual ~XBusServo(); |
sawa | 0:381d475cfd6c | 102 | |
sawa | 0:381d475cfd6c | 103 | public: |
sawa | 0:381d475cfd6c | 104 | XBusError addServo(uint8_t channelID, uint16_t initValue); |
sawa | 0:381d475cfd6c | 105 | XBusError removeServo(uint8_t channelID); |
sawa | 0:381d475cfd6c | 106 | XBusError setServo(uint8_t channelID, uint16_t value); |
sawa | 0:381d475cfd6c | 107 | |
sawa | 0:381d475cfd6c | 108 | void sendChannelDataPacket(void); |
sawa | 0:381d475cfd6c | 109 | |
sawa | 0:381d475cfd6c | 110 | XBusError setChannelID(uint8_t oldChannelID, uint8_t newChannelID); |
sawa | 0:381d475cfd6c | 111 | XBusError setCommand(uint8_t channelID, uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 112 | XBusError getCommand(uint8_t channelID, uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 113 | |
sawa | 0:381d475cfd6c | 114 | XBusError setChannelID(uint8_t newChannelID); |
sawa | 0:381d475cfd6c | 115 | XBusError setCommand(uint8_t order, int16_t* value); |
sawa | 0:381d475cfd6c | 116 | |
sawa | 0:381d475cfd6c | 117 | |
sawa | 0:381d475cfd6c | 118 | private: |
sawa | 0:381d475cfd6c | 119 | #define kRecieveBufferSize 64 |
sawa | 0:381d475cfd6c | 120 | |
sawa | 0:381d475cfd6c | 121 | uint8_t txOnly; // 1 for TX only mode |
sawa | 0:381d475cfd6c | 122 | PinName txPin; // |
sawa | 0:381d475cfd6c | 123 | uint8_t numOfServo; // number of servos |
sawa | 0:381d475cfd6c | 124 | uint8_t maxServo; // max number of servos |
sawa | 0:381d475cfd6c | 125 | uint8_t* chPacketBuffer; // channel data packet buffer |
sawa | 0:381d475cfd6c | 126 | uint8_t* sendBuffer; // serial send buffer |
sawa | 0:381d475cfd6c | 127 | uint8_t* sendBufferPointer; |
sawa | 0:381d475cfd6c | 128 | uint8_t sendLength; |
sawa | 0:381d475cfd6c | 129 | uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer |
sawa | 0:381d475cfd6c | 130 | uint8_t recieveBufferPointer; |
sawa | 0:381d475cfd6c | 131 | uint8_t dirty; // |
sawa | 0:381d475cfd6c | 132 | uint8_t serialWriteBusy; // |
sawa | 0:381d475cfd6c | 133 | volatile uint8_t modifyServosNow; // |
sawa | 0:381d475cfd6c | 134 | |
sawa | 0:381d475cfd6c | 135 | uint8_t crc_table(uint8_t data, uint8_t crc); |
sawa | 0:381d475cfd6c | 136 | uint8_t crc8(uint8_t* buffer, uint8_t length); |
sawa | 0:381d475cfd6c | 137 | uint8_t getDataSize(uint8_t order); |
sawa | 0:381d475cfd6c | 138 | |
sawa | 0:381d475cfd6c | 139 | void write(uint8_t* buffer, uint8_t length); |
sawa | 0:381d475cfd6c | 140 | void flush(void); |
sawa | 0:381d475cfd6c | 141 | void TxIrqHandler(void); |
sawa | 0:381d475cfd6c | 142 | void RxIrqHandler(void); |
sawa | 0:381d475cfd6c | 143 | |
sawa | 0:381d475cfd6c | 144 | XBusError sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize); |
sawa | 0:381d475cfd6c | 145 | }; |
sawa | 0:381d475cfd6c | 146 | |
sawa | 0:381d475cfd6c | 147 | |
sawa | 0:381d475cfd6c | 148 | #endif // of XBusServo_h |
sawa | 0:381d475cfd6c | 149 |