albatross
/
XBusServoTest
XBusServoの設定を行うプログラムです。
main.cpp@4:bde6d41517d3, 2017-01-11 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Jan 11 11:51:42 2017 +0000
- Revision:
- 4:bde6d41517d3
- Parent:
- 3:db5a2f683282
???????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
YusukeWakuta | 0:301100d3469a | 1 | #include "mbed.h" |
YusukeWakuta | 0:301100d3469a | 2 | #include "XBusServo.h" |
YusukeWakuta | 0:301100d3469a | 3 | #include <string> |
YusukeWakuta | 0:301100d3469a | 4 | |
YusukeWakuta | 0:301100d3469a | 5 | //#define DEBUGmain |
YusukeWakuta | 0:301100d3469a | 6 | |
YusukeWakuta | 0:301100d3469a | 7 | #ifdef DEBUGmain |
YusukeWakuta | 0:301100d3469a | 8 | #define DBG(fmt) printf(fmt) |
YusukeWakuta | 0:301100d3469a | 9 | #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__) |
YusukeWakuta | 0:301100d3469a | 10 | #else |
YusukeWakuta | 0:301100d3469a | 11 | #define DBG(...) |
YusukeWakuta | 0:301100d3469a | 12 | #define DBGF(...) |
YusukeWakuta | 0:301100d3469a | 13 | #endif |
YusukeWakuta | 0:301100d3469a | 14 | |
YusukeWakuta | 0:301100d3469a | 15 | // |
YusukeWakuta | 0:301100d3469a | 16 | // defines |
YusukeWakuta | 0:301100d3469a | 17 | // |
YusukeWakuta | 0:301100d3469a | 18 | #define kXBusTx p9 |
YusukeWakuta | 0:301100d3469a | 19 | #define kMaxServoNum 1 // 1 - 50 |
YusukeWakuta | 0:301100d3469a | 20 | |
YusukeWakuta | 0:301100d3469a | 21 | #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) |
YusukeWakuta | 0:301100d3469a | 22 | |
YusukeWakuta | 2:32aba462e4df | 23 | #define kMotionInterval 10 // flame / sec |
YusukeWakuta | 0:301100d3469a | 24 | #define kMotionMinMark 0x1249 |
YusukeWakuta | 0:301100d3469a | 25 | #define kMotionEndMark 0xED86 |
YusukeWakuta | 0:301100d3469a | 26 | |
YusukeWakuta | 0:301100d3469a | 27 | AnalogIn analog(p20); |
YusukeWakuta | 0:301100d3469a | 28 | Serial pc(USBTX,USBRX); |
YusukeWakuta | 2:32aba462e4df | 29 | DigitalOut Led(LED1); |
YusukeWakuta | 0:301100d3469a | 30 | |
YusukeWakuta | 0:301100d3469a | 31 | // |
YusukeWakuta | 0:301100d3469a | 32 | // typedefs |
YusukeWakuta | 0:301100d3469a | 33 | // |
YusukeWakuta | 0:301100d3469a | 34 | typedef enum poseDataType { |
YusukeWakuta | 0:301100d3469a | 35 | dataType_Pose, |
YusukeWakuta | 0:301100d3469a | 36 | numOfDataType |
YusukeWakuta | 0:301100d3469a | 37 | } poseDataType; |
YusukeWakuta | 0:301100d3469a | 38 | |
YusukeWakuta | 0:301100d3469a | 39 | |
YusukeWakuta | 0:301100d3469a | 40 | typedef struct poseRec { |
YusukeWakuta | 0:301100d3469a | 41 | poseDataType poseType; |
YusukeWakuta | 0:301100d3469a | 42 | uint16_t nextPoseIndex; |
YusukeWakuta | 0:301100d3469a | 43 | int32_t servoPosition[kMaxServoNum]; |
YusukeWakuta | 0:301100d3469a | 44 | uint16_t frameFromBeforePause; |
YusukeWakuta | 0:301100d3469a | 45 | } |
YusukeWakuta | 0:301100d3469a | 46 | poseRec; |
YusukeWakuta | 0:301100d3469a | 47 | |
YusukeWakuta | 0:301100d3469a | 48 | static const uint8_t servoChannel = 0x01; |
YusukeWakuta | 0:301100d3469a | 49 | |
YusukeWakuta | 0:301100d3469a | 50 | // |
YusukeWakuta | 0:301100d3469a | 51 | // global vars |
YusukeWakuta | 0:301100d3469a | 52 | // |
YusukeWakuta | 0:301100d3469a | 53 | XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum); |
YusukeWakuta | 0:301100d3469a | 54 | Ticker gTimer; |
YusukeWakuta | 0:301100d3469a | 55 | |
YusukeWakuta | 0:301100d3469a | 56 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 57 | // XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 58 | // play motion ! |
YusukeWakuta | 0:301100d3469a | 59 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 60 | void XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 61 | { |
YusukeWakuta | 0:301100d3469a | 62 | uint16_t value; |
YusukeWakuta | 0:301100d3469a | 63 | uint16_t diff = kMotionEndMark - kMotionMinMark; |
YusukeWakuta | 0:301100d3469a | 64 | value = (uint16_t)(diff * analog.read()) + kMotionMinMark; |
YusukeWakuta | 0:301100d3469a | 65 | gXBus.setServo(servoChannel, value); |
YusukeWakuta | 2:32aba462e4df | 66 | gXBus.sendChannelDataPacket(); |
YusukeWakuta | 0:301100d3469a | 67 | } |
YusukeWakuta | 0:301100d3469a | 68 | |
YusukeWakuta | 0:301100d3469a | 69 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 70 | // init() |
YusukeWakuta | 0:301100d3469a | 71 | // initialize all setup |
YusukeWakuta | 0:301100d3469a | 72 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 73 | XBusError init() |
YusukeWakuta | 0:301100d3469a | 74 | { |
YusukeWakuta | 0:301100d3469a | 75 | XBusError result; |
YusukeWakuta | 0:301100d3469a | 76 | |
YusukeWakuta | 0:301100d3469a | 77 | // initialize XBus |
YusukeWakuta | 0:301100d3469a | 78 | result = gXBus.start(); |
YusukeWakuta | 0:301100d3469a | 79 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 80 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 81 | return result; |
YusukeWakuta | 0:301100d3469a | 82 | } |
YusukeWakuta | 0:301100d3469a | 83 | |
YusukeWakuta | 0:301100d3469a | 84 | // initialize XBus Servos |
YusukeWakuta | 0:301100d3469a | 85 | result = gXBus.addServo(servoChannel, kXbusServoNeutral); |
YusukeWakuta | 0:301100d3469a | 86 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 87 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 88 | return result; |
YusukeWakuta | 0:301100d3469a | 89 | } |
YusukeWakuta | 0:301100d3469a | 90 | |
YusukeWakuta | 0:301100d3469a | 91 | return kXBusError_NoError; |
YusukeWakuta | 0:301100d3469a | 92 | } |
YusukeWakuta | 0:301100d3469a | 93 | |
YusukeWakuta | 0:301100d3469a | 94 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 95 | // main() |
YusukeWakuta | 0:301100d3469a | 96 | // |
YusukeWakuta | 0:301100d3469a | 97 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 98 | int main() |
YusukeWakuta | 0:301100d3469a | 99 | { |
YusukeWakuta | 3:db5a2f683282 | 100 | XBusError result; |
YusukeWakuta | 3:db5a2f683282 | 101 | |
YusukeWakuta | 2:32aba462e4df | 102 | if (init() != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 103 | return -1; |
YusukeWakuta | 2:32aba462e4df | 104 | Led = 1; |
YusukeWakuta | 2:32aba462e4df | 105 | wait(2); |
YusukeWakuta | 2:32aba462e4df | 106 | Led = 0; |
YusukeWakuta | 2:32aba462e4df | 107 | } |
YusukeWakuta | 0:301100d3469a | 108 | |
YusukeWakuta | 0:301100d3469a | 109 | // start motion |
YusukeWakuta | 0:301100d3469a | 110 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 0:301100d3469a | 111 | |
YusukeWakuta | 0:301100d3469a | 112 | while(1) { |
YusukeWakuta | 3:db5a2f683282 | 113 | //動作角度を180度幅に拡張 |
YusukeWakuta | 3:db5a2f683282 | 114 | int16_t value = 0x01; |
YusukeWakuta | 3:db5a2f683282 | 115 | int16_t angleParam = 0x0011; |
YusukeWakuta | 3:db5a2f683282 | 116 | result = gXBus.setCommand(kXBusOrder_1_Angle_180,&value); |
YusukeWakuta | 3:db5a2f683282 | 117 | result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&angleParam); |
YusukeWakuta | 3:db5a2f683282 | 118 | |
YusukeWakuta | 3:db5a2f683282 | 119 | //電源が入るとすぐに指示位置に移動 |
YusukeWakuta | 3:db5a2f683282 | 120 | int16_t quick = 0x00; |
YusukeWakuta | 3:db5a2f683282 | 121 | int16_t quickParam = 0x0012; |
YusukeWakuta | 3:db5a2f683282 | 122 | result = gXBus.setCommand(kXBusOrder_1_SlowStart,&quick); |
YusukeWakuta | 3:db5a2f683282 | 123 | result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&quickParam); |
YusukeWakuta | 3:db5a2f683282 | 124 | |
YusukeWakuta | 3:db5a2f683282 | 125 | //信号が途切れたらその位置に固定 |
YusukeWakuta | 3:db5a2f683282 | 126 | int16_t steer = 0x01; |
YusukeWakuta | 3:db5a2f683282 | 127 | int16_t steerParam = 0x0013; |
YusukeWakuta | 3:db5a2f683282 | 128 | result = gXBus.setCommand(kXBusOrder_1_StopMode,&steer); |
YusukeWakuta | 3:db5a2f683282 | 129 | result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&steerParam); |
YusukeWakuta | 4:bde6d41517d3 | 130 | |
YusukeWakuta | 4:bde6d41517d3 | 131 | //ROMから設定を削除する時は、PaParameterResetをsetCOmmandで書き込む |
YusukeWakuta | 3:db5a2f683282 | 132 | |
YusukeWakuta | 3:db5a2f683282 | 133 | wait(0.1); |
YusukeWakuta | 3:db5a2f683282 | 134 | |
YusukeWakuta | 0:301100d3469a | 135 | } |
YusukeWakuta | 0:301100d3469a | 136 | } |