albatross
/
XBusServoTest
XBusServoの設定を行うプログラムです。
main.cpp@0:301100d3469a, 2017-01-04 (annotated)
- Committer:
- YusukeWakuta
- Date:
- Wed Jan 04 09:10:05 2017 +0000
- Revision:
- 0:301100d3469a
- Child:
- 1:fee82ed64991
- Child:
- 2:32aba462e4df
????????
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 | 0:301100d3469a | 23 | #define kMotionInterval 30 // 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 | 0:301100d3469a | 29 | |
YusukeWakuta | 0:301100d3469a | 30 | // |
YusukeWakuta | 0:301100d3469a | 31 | // typedefs |
YusukeWakuta | 0:301100d3469a | 32 | // |
YusukeWakuta | 0:301100d3469a | 33 | typedef enum poseDataType { |
YusukeWakuta | 0:301100d3469a | 34 | dataType_Pose, |
YusukeWakuta | 0:301100d3469a | 35 | numOfDataType |
YusukeWakuta | 0:301100d3469a | 36 | } poseDataType; |
YusukeWakuta | 0:301100d3469a | 37 | |
YusukeWakuta | 0:301100d3469a | 38 | |
YusukeWakuta | 0:301100d3469a | 39 | typedef struct poseRec { |
YusukeWakuta | 0:301100d3469a | 40 | poseDataType poseType; |
YusukeWakuta | 0:301100d3469a | 41 | uint16_t nextPoseIndex; |
YusukeWakuta | 0:301100d3469a | 42 | int32_t servoPosition[kMaxServoNum]; |
YusukeWakuta | 0:301100d3469a | 43 | uint16_t frameFromBeforePause; |
YusukeWakuta | 0:301100d3469a | 44 | } |
YusukeWakuta | 0:301100d3469a | 45 | poseRec; |
YusukeWakuta | 0:301100d3469a | 46 | |
YusukeWakuta | 0:301100d3469a | 47 | static const uint8_t servoChannel = 0x01; |
YusukeWakuta | 0:301100d3469a | 48 | |
YusukeWakuta | 0:301100d3469a | 49 | // |
YusukeWakuta | 0:301100d3469a | 50 | // global vars |
YusukeWakuta | 0:301100d3469a | 51 | // |
YusukeWakuta | 0:301100d3469a | 52 | XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum); |
YusukeWakuta | 0:301100d3469a | 53 | Ticker gTimer; |
YusukeWakuta | 0:301100d3469a | 54 | |
YusukeWakuta | 0:301100d3469a | 55 | string Deci2Hex(int val,bool lower = false){ |
YusukeWakuta | 0:301100d3469a | 56 | string str = "0x"; |
YusukeWakuta | 0:301100d3469a | 57 | char answer[10]; |
YusukeWakuta | 0:301100d3469a | 58 | char henkan[17] = "0123456789ABCDEF"; |
YusukeWakuta | 0:301100d3469a | 59 | int i = 0,j = 0; |
YusukeWakuta | 0:301100d3469a | 60 | while(val > 0){ |
YusukeWakuta | 0:301100d3469a | 61 | j = val % 16; |
YusukeWakuta | 0:301100d3469a | 62 | val = val / 16; |
YusukeWakuta | 0:301100d3469a | 63 | answer[i] = henkan[j]; |
YusukeWakuta | 0:301100d3469a | 64 | i++; |
YusukeWakuta | 0:301100d3469a | 65 | } |
YusukeWakuta | 0:301100d3469a | 66 | i--; |
YusukeWakuta | 0:301100d3469a | 67 | for(j = i;j >= 0;j--){ |
YusukeWakuta | 0:301100d3469a | 68 | str += answer[j]; |
YusukeWakuta | 0:301100d3469a | 69 | } |
YusukeWakuta | 0:301100d3469a | 70 | return str; |
YusukeWakuta | 0:301100d3469a | 71 | } |
YusukeWakuta | 0:301100d3469a | 72 | |
YusukeWakuta | 0:301100d3469a | 73 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 74 | // XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 75 | // play motion ! |
YusukeWakuta | 0:301100d3469a | 76 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 77 | void XbusIntervalHandler() |
YusukeWakuta | 0:301100d3469a | 78 | { |
YusukeWakuta | 0:301100d3469a | 79 | uint16_t value; |
YusukeWakuta | 0:301100d3469a | 80 | uint16_t diff = kMotionEndMark - kMotionMinMark; |
YusukeWakuta | 0:301100d3469a | 81 | // send current data |
YusukeWakuta | 0:301100d3469a | 82 | gXBus.sendChannelDataPacket(); |
YusukeWakuta | 0:301100d3469a | 83 | value = (uint16_t)(diff * analog.read()) + kMotionMinMark; |
YusukeWakuta | 0:301100d3469a | 84 | gXBus.setServo(servoChannel, value); |
YusukeWakuta | 0:301100d3469a | 85 | } |
YusukeWakuta | 0:301100d3469a | 86 | |
YusukeWakuta | 0:301100d3469a | 87 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 88 | // init() |
YusukeWakuta | 0:301100d3469a | 89 | // initialize all setup |
YusukeWakuta | 0:301100d3469a | 90 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 91 | XBusError init() |
YusukeWakuta | 0:301100d3469a | 92 | { |
YusukeWakuta | 0:301100d3469a | 93 | XBusError result; |
YusukeWakuta | 0:301100d3469a | 94 | |
YusukeWakuta | 0:301100d3469a | 95 | // initialize XBus |
YusukeWakuta | 0:301100d3469a | 96 | result = gXBus.start(); |
YusukeWakuta | 0:301100d3469a | 97 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 98 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 99 | return result; |
YusukeWakuta | 0:301100d3469a | 100 | } |
YusukeWakuta | 0:301100d3469a | 101 | |
YusukeWakuta | 0:301100d3469a | 102 | // initialize XBus Servos |
YusukeWakuta | 0:301100d3469a | 103 | result = gXBus.addServo(servoChannel, kXbusServoNeutral); |
YusukeWakuta | 0:301100d3469a | 104 | if (result != kXBusError_NoError) { |
YusukeWakuta | 0:301100d3469a | 105 | gXBus.stop(); |
YusukeWakuta | 0:301100d3469a | 106 | return result; |
YusukeWakuta | 0:301100d3469a | 107 | } |
YusukeWakuta | 0:301100d3469a | 108 | |
YusukeWakuta | 0:301100d3469a | 109 | return kXBusError_NoError; |
YusukeWakuta | 0:301100d3469a | 110 | } |
YusukeWakuta | 0:301100d3469a | 111 | |
YusukeWakuta | 0:301100d3469a | 112 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 113 | // main() |
YusukeWakuta | 0:301100d3469a | 114 | // |
YusukeWakuta | 0:301100d3469a | 115 | //============================================================= |
YusukeWakuta | 0:301100d3469a | 116 | int main() |
YusukeWakuta | 0:301100d3469a | 117 | { |
YusukeWakuta | 0:301100d3469a | 118 | if (init() != kXBusError_NoError) |
YusukeWakuta | 0:301100d3469a | 119 | return -1; |
YusukeWakuta | 0:301100d3469a | 120 | |
YusukeWakuta | 0:301100d3469a | 121 | // start motion |
YusukeWakuta | 0:301100d3469a | 122 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
YusukeWakuta | 0:301100d3469a | 123 | |
YusukeWakuta | 0:301100d3469a | 124 | while(1) { |
YusukeWakuta | 0:301100d3469a | 125 | } |
YusukeWakuta | 0:301100d3469a | 126 | } |