Test program to play motion with XBus servos.
This is just a working sample. This code will play motion with XBus servos. This is not finished :-). Tested only on KL25Z.
これは単なる動作サンプルです。 XBusサーボを使ってモーションの再生を行います。 まだまだ未完です(^_^;) KL25Z上でのみ動作確認しています。
main.cpp@0:f7f2ac58b1ad, 2014-11-05 (annotated)
- Committer:
- sawa
- Date:
- Wed Nov 05 06:40:28 2014 +0000
- Revision:
- 0:f7f2ac58b1ad
- Child:
- 1:4b239aea14f4
Test program to play motion with XBus servos.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sawa | 0:f7f2ac58b1ad | 1 | /* main.cpp file |
sawa | 0:f7f2ac58b1ad | 2 | * |
sawa | 0:f7f2ac58b1ad | 3 | * for testing mbed motion player with XBusServo.cpp |
sawa | 0:f7f2ac58b1ad | 4 | * |
sawa | 0:f7f2ac58b1ad | 5 | * Copyright (c) 2014-2014 JR PROPO |
sawa | 0:f7f2ac58b1ad | 6 | * Released under the MIT License: http://mbed.org/license/mit |
sawa | 0:f7f2ac58b1ad | 7 | * |
sawa | 0:f7f2ac58b1ad | 8 | * by Zak Sawa |
sawa | 0:f7f2ac58b1ad | 9 | */ |
sawa | 0:f7f2ac58b1ad | 10 | |
sawa | 0:f7f2ac58b1ad | 11 | #include "mbed.h" |
sawa | 0:f7f2ac58b1ad | 12 | #include "XBusServo.h" |
sawa | 0:f7f2ac58b1ad | 13 | |
sawa | 0:f7f2ac58b1ad | 14 | //#define DEBUGmain |
sawa | 0:f7f2ac58b1ad | 15 | |
sawa | 0:f7f2ac58b1ad | 16 | #ifdef DEBUGmain |
sawa | 0:f7f2ac58b1ad | 17 | #define DBG(fmt) printf(fmt) |
sawa | 0:f7f2ac58b1ad | 18 | #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__) |
sawa | 0:f7f2ac58b1ad | 19 | #else |
sawa | 0:f7f2ac58b1ad | 20 | #define DBG(...) |
sawa | 0:f7f2ac58b1ad | 21 | #define DBGF(...) |
sawa | 0:f7f2ac58b1ad | 22 | #endif |
sawa | 0:f7f2ac58b1ad | 23 | |
sawa | 0:f7f2ac58b1ad | 24 | // |
sawa | 0:f7f2ac58b1ad | 25 | // defines |
sawa | 0:f7f2ac58b1ad | 26 | // |
sawa | 0:f7f2ac58b1ad | 27 | #define kXBusTx PTE0 // PTD3 p13 |
sawa | 0:f7f2ac58b1ad | 28 | #define kXBusRx PTE1 // PTD2 p14 |
sawa | 0:f7f2ac58b1ad | 29 | #define kXBusSwitch D10 |
sawa | 0:f7f2ac58b1ad | 30 | #define kMaxServoNum 4 // 1 - 50 |
sawa | 0:f7f2ac58b1ad | 31 | |
sawa | 0:f7f2ac58b1ad | 32 | #define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) |
sawa | 0:f7f2ac58b1ad | 33 | |
sawa | 0:f7f2ac58b1ad | 34 | #define kMotionInterval 30 // flame / sec |
sawa | 0:f7f2ac58b1ad | 35 | #define kMotionEndMark 0xFFFF |
sawa | 0:f7f2ac58b1ad | 36 | |
sawa | 0:f7f2ac58b1ad | 37 | // |
sawa | 0:f7f2ac58b1ad | 38 | // typedefs |
sawa | 0:f7f2ac58b1ad | 39 | // |
sawa | 0:f7f2ac58b1ad | 40 | typedef enum poseDataType { |
sawa | 0:f7f2ac58b1ad | 41 | dataType_Pose, |
sawa | 0:f7f2ac58b1ad | 42 | numOfDataType |
sawa | 0:f7f2ac58b1ad | 43 | } poseDataType; |
sawa | 0:f7f2ac58b1ad | 44 | |
sawa | 0:f7f2ac58b1ad | 45 | |
sawa | 0:f7f2ac58b1ad | 46 | typedef struct poseRec { |
sawa | 0:f7f2ac58b1ad | 47 | poseDataType poseType; |
sawa | 0:f7f2ac58b1ad | 48 | uint16_t nextPoseIndex; |
sawa | 0:f7f2ac58b1ad | 49 | int32_t servoPosition[kMaxServoNum]; |
sawa | 0:f7f2ac58b1ad | 50 | uint16_t frameFromBeforePause; |
sawa | 0:f7f2ac58b1ad | 51 | } |
sawa | 0:f7f2ac58b1ad | 52 | poseRec; |
sawa | 0:f7f2ac58b1ad | 53 | |
sawa | 0:f7f2ac58b1ad | 54 | |
sawa | 0:f7f2ac58b1ad | 55 | // |
sawa | 0:f7f2ac58b1ad | 56 | // sample motion data |
sawa | 0:f7f2ac58b1ad | 57 | // |
sawa | 0:f7f2ac58b1ad | 58 | static const uint8_t servoChannel[kMaxServoNum] = {0x01, 0x02, 0x03, 0x04}; |
sawa | 0:f7f2ac58b1ad | 59 | |
sawa | 0:f7f2ac58b1ad | 60 | static const poseRec motionData[] = { |
sawa | 0:f7f2ac58b1ad | 61 | // data type nextPoseIndex servoPosition frameFromBeforePause |
sawa | 0:f7f2ac58b1ad | 62 | {dataType_Pose, 1, {0x7FFF, 0x7FFF, 0x7FFF, 0x7FFF}, 10}, |
sawa | 0:f7f2ac58b1ad | 63 | {dataType_Pose, 2, {0x5FFF, 0x8FFF, 0xAFFF, 0x4FFF}, 30}, |
sawa | 0:f7f2ac58b1ad | 64 | {dataType_Pose, 3, {0x9FFF, 0x5FFF, 0x6FFF, 0xAFFF}, 10}, |
sawa | 0:f7f2ac58b1ad | 65 | {dataType_Pose, 4, {0x9FFF, 0xEFFF, 0x7FFF, 0x1FFF}, 20}, |
sawa | 0:f7f2ac58b1ad | 66 | {dataType_Pose, kMotionEndMark, {0x2FFF, 0x5FFF, 0x8FFF, 0x5FFF}, 30} |
sawa | 0:f7f2ac58b1ad | 67 | }; |
sawa | 0:f7f2ac58b1ad | 68 | |
sawa | 0:f7f2ac58b1ad | 69 | |
sawa | 0:f7f2ac58b1ad | 70 | // |
sawa | 0:f7f2ac58b1ad | 71 | // global vars |
sawa | 0:f7f2ac58b1ad | 72 | // |
sawa | 0:f7f2ac58b1ad | 73 | XBusServo gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum); |
sawa | 0:f7f2ac58b1ad | 74 | Ticker gTimer; |
sawa | 0:f7f2ac58b1ad | 75 | volatile uint16_t gCurrentFlame = 0; |
sawa | 0:f7f2ac58b1ad | 76 | volatile uint16_t gCurrentIndex = 0; |
sawa | 0:f7f2ac58b1ad | 77 | volatile uint16_t gBeforeIndex = 0; |
sawa | 0:f7f2ac58b1ad | 78 | volatile uint16_t gCurrentServoPos[kMaxServoNum]; |
sawa | 0:f7f2ac58b1ad | 79 | |
sawa | 0:f7f2ac58b1ad | 80 | |
sawa | 0:f7f2ac58b1ad | 81 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 82 | // XbusIntervalHandler() |
sawa | 0:f7f2ac58b1ad | 83 | // 14mSec interval handler |
sawa | 0:f7f2ac58b1ad | 84 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 85 | void XbusIntervalHandler() |
sawa | 0:f7f2ac58b1ad | 86 | { |
sawa | 0:f7f2ac58b1ad | 87 | uint16_t counter; |
sawa | 0:f7f2ac58b1ad | 88 | |
sawa | 0:f7f2ac58b1ad | 89 | // send current data |
sawa | 0:f7f2ac58b1ad | 90 | gXBus.sendChannelDataPacket(); |
sawa | 0:f7f2ac58b1ad | 91 | if (gCurrentIndex == kMotionEndMark) |
sawa | 0:f7f2ac58b1ad | 92 | return; |
sawa | 0:f7f2ac58b1ad | 93 | |
sawa | 0:f7f2ac58b1ad | 94 | // make next data |
sawa | 0:f7f2ac58b1ad | 95 | gCurrentFlame++; |
sawa | 0:f7f2ac58b1ad | 96 | |
sawa | 0:f7f2ac58b1ad | 97 | if (gCurrentFlame == motionData[gCurrentIndex].frameFromBeforePause) { |
sawa | 0:f7f2ac58b1ad | 98 | gBeforeIndex = gCurrentIndex; |
sawa | 0:f7f2ac58b1ad | 99 | gCurrentIndex = motionData[gCurrentIndex].nextPoseIndex; |
sawa | 0:f7f2ac58b1ad | 100 | gCurrentFlame = 0; |
sawa | 0:f7f2ac58b1ad | 101 | } |
sawa | 0:f7f2ac58b1ad | 102 | |
sawa | 0:f7f2ac58b1ad | 103 | if (gCurrentIndex == kMotionEndMark) |
sawa | 0:f7f2ac58b1ad | 104 | return; |
sawa | 0:f7f2ac58b1ad | 105 | |
sawa | 0:f7f2ac58b1ad | 106 | if (gCurrentIndex != gBeforeIndex) |
sawa | 0:f7f2ac58b1ad | 107 | for (counter = 0; counter < kMaxServoNum; counter++) { |
sawa | 0:f7f2ac58b1ad | 108 | uint16_t value; |
sawa | 0:f7f2ac58b1ad | 109 | int32_t temp; |
sawa | 0:f7f2ac58b1ad | 110 | |
sawa | 0:f7f2ac58b1ad | 111 | value = motionData[gBeforeIndex].servoPosition[counter]; |
sawa | 0:f7f2ac58b1ad | 112 | temp = motionData[gCurrentIndex].servoPosition[counter] - motionData[gBeforeIndex].servoPosition[counter]; |
sawa | 0:f7f2ac58b1ad | 113 | temp *= gCurrentFlame; |
sawa | 0:f7f2ac58b1ad | 114 | temp /= motionData[gCurrentIndex].frameFromBeforePause; |
sawa | 0:f7f2ac58b1ad | 115 | value += temp; |
sawa | 0:f7f2ac58b1ad | 116 | |
sawa | 0:f7f2ac58b1ad | 117 | gXBus.setServo(servoChannel[counter], value); |
sawa | 0:f7f2ac58b1ad | 118 | } |
sawa | 0:f7f2ac58b1ad | 119 | |
sawa | 0:f7f2ac58b1ad | 120 | } |
sawa | 0:f7f2ac58b1ad | 121 | |
sawa | 0:f7f2ac58b1ad | 122 | |
sawa | 0:f7f2ac58b1ad | 123 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 124 | // init() |
sawa | 0:f7f2ac58b1ad | 125 | // initialize all setup |
sawa | 0:f7f2ac58b1ad | 126 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 127 | XBusError init() |
sawa | 0:f7f2ac58b1ad | 128 | { |
sawa | 0:f7f2ac58b1ad | 129 | XBusError result; |
sawa | 0:f7f2ac58b1ad | 130 | uint16_t counter; |
sawa | 0:f7f2ac58b1ad | 131 | |
sawa | 0:f7f2ac58b1ad | 132 | // initialize XBus |
sawa | 0:f7f2ac58b1ad | 133 | result = gXBus.start(); |
sawa | 0:f7f2ac58b1ad | 134 | if (result != kXBusError_NoError) { |
sawa | 0:f7f2ac58b1ad | 135 | gXBus.stop(); |
sawa | 0:f7f2ac58b1ad | 136 | return result; |
sawa | 0:f7f2ac58b1ad | 137 | } |
sawa | 0:f7f2ac58b1ad | 138 | |
sawa | 0:f7f2ac58b1ad | 139 | // initialize XBus Servos |
sawa | 0:f7f2ac58b1ad | 140 | for (counter = 0; counter < kMaxServoNum; counter++) |
sawa | 0:f7f2ac58b1ad | 141 | gXBus.addServo(servoChannel[counter], kXbusServoNeutral); |
sawa | 0:f7f2ac58b1ad | 142 | |
sawa | 0:f7f2ac58b1ad | 143 | return kXBusError_NoError; |
sawa | 0:f7f2ac58b1ad | 144 | } |
sawa | 0:f7f2ac58b1ad | 145 | |
sawa | 0:f7f2ac58b1ad | 146 | |
sawa | 0:f7f2ac58b1ad | 147 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 148 | // main() |
sawa | 0:f7f2ac58b1ad | 149 | // |
sawa | 0:f7f2ac58b1ad | 150 | //============================================================= |
sawa | 0:f7f2ac58b1ad | 151 | int main() |
sawa | 0:f7f2ac58b1ad | 152 | { |
sawa | 0:f7f2ac58b1ad | 153 | if (init() != kXBusError_NoError) |
sawa | 0:f7f2ac58b1ad | 154 | return -1; |
sawa | 0:f7f2ac58b1ad | 155 | |
sawa | 0:f7f2ac58b1ad | 156 | // start motion |
sawa | 0:f7f2ac58b1ad | 157 | gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); |
sawa | 0:f7f2ac58b1ad | 158 | |
sawa | 0:f7f2ac58b1ad | 159 | while(1) { |
sawa | 0:f7f2ac58b1ad | 160 | } |
sawa | 0:f7f2ac58b1ad | 161 | |
sawa | 0:f7f2ac58b1ad | 162 | |
sawa | 0:f7f2ac58b1ad | 163 | } |