Kazutaka "Zak" Sawa / Mbed 2 deprecated mbed_XBus_MotionTest

Test program to play motion with XBus servos.

Dependencies:   XBusServo mbed

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@1:4b239aea14f4, 2014-11-05 (annotated)

Committer:
sawa
Date:
Wed Nov 05 07:36:23 2014 +0000
Revision:
1:4b239aea14f4
Parent:
0:f7f2ac58b1ad
Child:
2:ca608c24a14e
fix comment

Who changed what in which revision?

UserRevisionLine numberNew 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 1:4b239aea14f4 83 // play motion !
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 // init()
sawa 0:f7f2ac58b1ad 124 // initialize all setup
sawa 0:f7f2ac58b1ad 125 //=============================================================
sawa 0:f7f2ac58b1ad 126 XBusError init()
sawa 0:f7f2ac58b1ad 127 {
sawa 0:f7f2ac58b1ad 128 XBusError result;
sawa 0:f7f2ac58b1ad 129 uint16_t counter;
sawa 0:f7f2ac58b1ad 130
sawa 0:f7f2ac58b1ad 131 // initialize XBus
sawa 0:f7f2ac58b1ad 132 result = gXBus.start();
sawa 0:f7f2ac58b1ad 133 if (result != kXBusError_NoError) {
sawa 0:f7f2ac58b1ad 134 gXBus.stop();
sawa 0:f7f2ac58b1ad 135 return result;
sawa 0:f7f2ac58b1ad 136 }
sawa 0:f7f2ac58b1ad 137
sawa 0:f7f2ac58b1ad 138 // initialize XBus Servos
sawa 0:f7f2ac58b1ad 139 for (counter = 0; counter < kMaxServoNum; counter++)
sawa 0:f7f2ac58b1ad 140 gXBus.addServo(servoChannel[counter], kXbusServoNeutral);
sawa 0:f7f2ac58b1ad 141
sawa 0:f7f2ac58b1ad 142 return kXBusError_NoError;
sawa 0:f7f2ac58b1ad 143 }
sawa 0:f7f2ac58b1ad 144
sawa 0:f7f2ac58b1ad 145
sawa 0:f7f2ac58b1ad 146 //=============================================================
sawa 0:f7f2ac58b1ad 147 // main()
sawa 0:f7f2ac58b1ad 148 //
sawa 0:f7f2ac58b1ad 149 //=============================================================
sawa 0:f7f2ac58b1ad 150 int main()
sawa 0:f7f2ac58b1ad 151 {
sawa 0:f7f2ac58b1ad 152 if (init() != kXBusError_NoError)
sawa 0:f7f2ac58b1ad 153 return -1;
sawa 0:f7f2ac58b1ad 154
sawa 0:f7f2ac58b1ad 155 // start motion
sawa 0:f7f2ac58b1ad 156 gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
sawa 0:f7f2ac58b1ad 157
sawa 0:f7f2ac58b1ad 158 while(1) {
sawa 0:f7f2ac58b1ad 159 }
sawa 0:f7f2ac58b1ad 160
sawa 0:f7f2ac58b1ad 161
sawa 0:f7f2ac58b1ad 162 }