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