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上でのみ動作確認しています。
Diff: main.cpp
- Revision:
- 0:f7f2ac58b1ad
- Child:
- 1:4b239aea14f4
diff -r 000000000000 -r f7f2ac58b1ad main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Nov 05 06:40:28 2014 +0000 @@ -0,0 +1,163 @@ +/* main.cpp file + * + * for testing mbed motion player with XBusServo.cpp + * + * Copyright (c) 2014-2014 JR PROPO + * Released under the MIT License: http://mbed.org/license/mit + * + * by Zak Sawa + */ + +#include "mbed.h" +#include "XBusServo.h" + +//#define DEBUGmain + +#ifdef DEBUGmain +#define DBG(fmt) printf(fmt) +#define DBGF(fmt, ...) printf(fmt, __VA_ARGS__) +#else +#define DBG(...) +#define DBGF(...) +#endif + +// +// defines +// +#define kXBusTx PTE0 // PTD3 p13 +#define kXBusRx PTE1 // PTD2 p14 +#define kXBusSwitch D10 +#define kMaxServoNum 4 // 1 - 50 + +#define kMaxServoPause (sizeof(motionData) / sizeof(pauseRec)) + +#define kMotionInterval 30 // flame / sec +#define kMotionEndMark 0xFFFF + +// +// typedefs +// +typedef enum poseDataType { + dataType_Pose, + numOfDataType +} poseDataType; + + +typedef struct poseRec { + poseDataType poseType; + uint16_t nextPoseIndex; + int32_t servoPosition[kMaxServoNum]; + uint16_t frameFromBeforePause; +} +poseRec; + + +// +// sample motion data +// +static const uint8_t servoChannel[kMaxServoNum] = {0x01, 0x02, 0x03, 0x04}; + +static const poseRec motionData[] = { +// data type nextPoseIndex servoPosition frameFromBeforePause + {dataType_Pose, 1, {0x7FFF, 0x7FFF, 0x7FFF, 0x7FFF}, 10}, + {dataType_Pose, 2, {0x5FFF, 0x8FFF, 0xAFFF, 0x4FFF}, 30}, + {dataType_Pose, 3, {0x9FFF, 0x5FFF, 0x6FFF, 0xAFFF}, 10}, + {dataType_Pose, 4, {0x9FFF, 0xEFFF, 0x7FFF, 0x1FFF}, 20}, + {dataType_Pose, kMotionEndMark, {0x2FFF, 0x5FFF, 0x8FFF, 0x5FFF}, 30} +}; + + +// +// global vars +// +XBusServo gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum); +Ticker gTimer; +volatile uint16_t gCurrentFlame = 0; +volatile uint16_t gCurrentIndex = 0; +volatile uint16_t gBeforeIndex = 0; +volatile uint16_t gCurrentServoPos[kMaxServoNum]; + + +//============================================================= +// XbusIntervalHandler() +// 14mSec interval handler +//============================================================= +void XbusIntervalHandler() +{ + uint16_t counter; + + // send current data + gXBus.sendChannelDataPacket(); + if (gCurrentIndex == kMotionEndMark) + return; + + // make next data + gCurrentFlame++; + + if (gCurrentFlame == motionData[gCurrentIndex].frameFromBeforePause) { + gBeforeIndex = gCurrentIndex; + gCurrentIndex = motionData[gCurrentIndex].nextPoseIndex; + gCurrentFlame = 0; + } + + if (gCurrentIndex == kMotionEndMark) + return; + + if (gCurrentIndex != gBeforeIndex) + for (counter = 0; counter < kMaxServoNum; counter++) { + uint16_t value; + int32_t temp; + + value = motionData[gBeforeIndex].servoPosition[counter]; + temp = motionData[gCurrentIndex].servoPosition[counter] - motionData[gBeforeIndex].servoPosition[counter]; + temp *= gCurrentFlame; + temp /= motionData[gCurrentIndex].frameFromBeforePause; + value += temp; + + gXBus.setServo(servoChannel[counter], value); + } + +} + + +//============================================================= +// init() +// initialize all setup +//============================================================= +XBusError init() +{ + XBusError result; + uint16_t counter; + + // initialize XBus + result = gXBus.start(); + if (result != kXBusError_NoError) { + gXBus.stop(); + return result; + } + + // initialize XBus Servos + for (counter = 0; counter < kMaxServoNum; counter++) + gXBus.addServo(servoChannel[counter], kXbusServoNeutral); + + return kXBusError_NoError; +} + + +//============================================================= +// main() +// +//============================================================= +int main() +{ + if (init() != kXBusError_NoError) + return -1; + + // start motion + gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval); + + while(1) { + } + + +}