albatross / Mbed 2 deprecated XBusServoTest

XBusServoの設定を行うプログラムです。

Dependencies:   XBusServo mbed

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?

UserRevisionLine numberNew 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 }