Kazutaka "Zak" Sawa / XBusServo

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@12:0e9d5941fc22, 2014-10-08 (annotated)

Committer:
sawa
Date:
Wed Oct 08 06:08:44 2014 +0000
Revision:
12:0e9d5941fc22
Parent:
11:becaec517aee
Child:
13:5e2aa53353a8
doxygen test

Who changed what in which revision?

UserRevisionLine numberNew 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 5:7246a46379f7 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 5:7246a46379f7 15 /// standard interval time for XBus servo
sawa 1:bd80d3e8f3a3 16 #define kXBusStandardInterval 14 // mSec
sawa 0:381d475cfd6c 17
sawa 5:7246a46379f7 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 5:7246a46379f7 23 /// maximum number of XBus servo
sawa 0:381d475cfd6c 24 #define kXBusMaxServoNum 50
sawa 0:381d475cfd6c 25
sawa 5:7246a46379f7 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 5:7246a46379f7 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 5:7246a46379f7 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 9:9248ea2cb4b5 102 /**
sawa 12:0e9d5941fc22 103 @brief Library class for XBus servo
sawa 8:66fb48d08e97 104 @note 2014/09/02 : move from Arduino lib by Sawa
sawa 8:66fb48d08e97 105 */
sawa 2:4aca5ffce457 106 class XBusServo // : private RawSerial
sawa 0:381d475cfd6c 107 {
sawa 0:381d475cfd6c 108 public:
sawa 7:04d294709f7a 109 /**
sawa 7:04d294709f7a 110 @param tx pin name for tx
sawa 7:04d294709f7a 111 @param rx pin name for rx
sawa 7:04d294709f7a 112 @param maxServoNum max number of servo that you want to connect.
sawa 7:04d294709f7a 113 (limit 50)
sawa 7:04d294709f7a 114 this does just to resetve the buffer. you need to
sawa 7:04d294709f7a 115 add XBus servo at the beginning of your sketch
sawa 7:04d294709f7a 116
sawa 7:04d294709f7a 117 @note 2014/09/02 : move from Arduino lib by Sawa
sawa 9:9248ea2cb4b5 118 */
sawa 0:381d475cfd6c 119 XBusServo(PinName tx, PinName rx, uint8_t maxServoNum);
sawa 0:381d475cfd6c 120
sawa 0:381d475cfd6c 121 public:
sawa 10:0cf9b0f582fb 122 /**
sawa 10:0cf9b0f582fb 123 @return XBus error code
sawa 12:0e9d5941fc22 124 @brief start function to use XBus
sawa 10:0cf9b0f582fb 125 @note 2014/09/02 : move from Arduino lib by Sawa
sawa 10:0cf9b0f582fb 126 */
sawa 1:bd80d3e8f3a3 127 XBusError start();
sawa 10:0cf9b0f582fb 128 /**
sawa 12:0e9d5941fc22 129 @brief stop function to use XBus
sawa 10:0cf9b0f582fb 130 @note 2014/09/02 : move from Arduino lib by Sawa
sawa 10:0cf9b0f582fb 131 */
sawa 1:bd80d3e8f3a3 132 void stop();
sawa 1:bd80d3e8f3a3 133
sawa 0:381d475cfd6c 134 XBusError addServo(uint8_t channelID, uint16_t initValue);
sawa 0:381d475cfd6c 135 XBusError removeServo(uint8_t channelID);
sawa 0:381d475cfd6c 136 XBusError setServo(uint8_t channelID, uint16_t value);
sawa 0:381d475cfd6c 137
sawa 0:381d475cfd6c 138 void sendChannelDataPacket(void);
sawa 0:381d475cfd6c 139
sawa 0:381d475cfd6c 140 XBusError setChannelID(uint8_t oldChannelID, uint8_t newChannelID);
sawa 0:381d475cfd6c 141 XBusError setCommand(uint8_t channelID, uint8_t order, int16_t* value);
sawa 0:381d475cfd6c 142 XBusError getCommand(uint8_t channelID, uint8_t order, int16_t* value);
sawa 0:381d475cfd6c 143
sawa 0:381d475cfd6c 144 XBusError setChannelID(uint8_t newChannelID);
sawa 0:381d475cfd6c 145 XBusError setCommand(uint8_t order, int16_t* value);
sawa 0:381d475cfd6c 146
sawa 0:381d475cfd6c 147
sawa 0:381d475cfd6c 148 private:
sawa 1:bd80d3e8f3a3 149 #define kRecieveBufferSize 64
sawa 1:bd80d3e8f3a3 150
sawa 1:bd80d3e8f3a3 151 RawSerial XBusPort;
sawa 1:bd80d3e8f3a3 152
sawa 0:381d475cfd6c 153 uint8_t txOnly; // 1 for TX only mode
sawa 0:381d475cfd6c 154 PinName txPin; //
sawa 0:381d475cfd6c 155 uint8_t numOfServo; // number of servos
sawa 0:381d475cfd6c 156 uint8_t maxServo; // max number of servos
sawa 0:381d475cfd6c 157 uint8_t* chPacketBuffer; // channel data packet buffer
sawa 0:381d475cfd6c 158 uint8_t* sendBuffer; // serial send buffer
sawa 0:381d475cfd6c 159 uint8_t* sendBufferPointer;
sawa 0:381d475cfd6c 160 uint8_t sendLength;
sawa 0:381d475cfd6c 161 uint8_t recieveBuffer[kRecieveBufferSize]; // serial recieve buffer
sawa 0:381d475cfd6c 162 uint8_t recieveBufferPointer;
sawa 0:381d475cfd6c 163 uint8_t dirty; //
sawa 0:381d475cfd6c 164 uint8_t serialWriteBusy; //
sawa 0:381d475cfd6c 165 volatile uint8_t modifyServosNow; //
sawa 0:381d475cfd6c 166
sawa 0:381d475cfd6c 167 uint8_t crc_table(uint8_t data, uint8_t crc);
sawa 0:381d475cfd6c 168 uint8_t crc8(uint8_t* buffer, uint8_t length);
sawa 0:381d475cfd6c 169 uint8_t getDataSize(uint8_t order);
sawa 0:381d475cfd6c 170
sawa 0:381d475cfd6c 171 void write(uint8_t* buffer, uint8_t length);
sawa 0:381d475cfd6c 172 void flush(void);
sawa 0:381d475cfd6c 173 void TxIrqHandler(void);
sawa 0:381d475cfd6c 174 void RxIrqHandler(void);
sawa 0:381d475cfd6c 175
sawa 0:381d475cfd6c 176 XBusError sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize);
sawa 0:381d475cfd6c 177 };
sawa 0:381d475cfd6c 178
sawa 0:381d475cfd6c 179
sawa 0:381d475cfd6c 180 #endif // of XBusServo_h
sawa 0:381d475cfd6c 181