albatross
/
XBusServoTest
XBusServoの設定を行うプログラムです。
main.cpp@2:32aba462e4df, 2017-01-11 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Jan 11 10:01:47 2017 +0000
- Revision:
- 2:32aba462e4df
- Parent:
- 0:301100d3469a
- Child:
- 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 | 2:32aba462e4df | 91 | int16_t index = 0x1D; |
YusukeWakuta | 2:32aba462e4df | 92 | int16_t value = 0x01; |
YusukeWakuta | 2:32aba462e4df | 93 | // int16_t Angle = 0x0011; |
YusukeWakuta | 2:32aba462e4df | 94 | |
YusukeWakuta | 2:32aba462e4df | 95 | result = gXBus.setCommand(kXBusOrder_1_Angle_180,&value); |
YusukeWakuta | 2:32aba462e4df | 96 | // result = gXBus.setCommand(kX); |
YusukeWakuta | 2:32aba462e4df | 97 | |
YusukeWakuta | 2:32aba462e4df | 98 | // result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&index); |
YusukeWakuta | 2:32aba462e4df | 99 | |
YusukeWakuta | 0:301100d3469a | 100 | return kXBusError_NoError; |
YusukeWakuta | 0:301100d3469a | 101 | } |
YusukeWakuta | 0:301100d3469a | 102 | |
YusukeWakuta | 0:301100d3469a | 103 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 104 | // main() |
YusukeWakuta | 0:301100d3469a | 105 | // |
YusukeWakuta | 0:301100d3469a | 106 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 107 | int main() |
YusukeWakuta | 0:301100d3469a | 108 | { |
YusukeWakuta | 2:32aba462e4df | 109 | if (init() != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 110 | return -1; |
YusukeWakuta | 2:32aba462e4df | 111 | Led = 1; |
YusukeWakuta | 2:32aba462e4df | 112 | wait(2); |
YusukeWakuta | 2:32aba462e4df | 113 | Led = 0; |
YusukeWakuta | 2:32aba462e4df | 114 | } |
YusukeWakuta | 0:301100d3469a | 115 | |
YusukeWakuta | 0:301100d3469a | 116 | // start motion |
YusukeWakuta | 0:301100d3469a | 117 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 0:301100d3469a | 118 | |
YusukeWakuta | 0:301100d3469a | 119 | while(1) { |
YusukeWakuta | 0:301100d3469a | 120 | } |
YusukeWakuta | 0:301100d3469a | 121 | } |