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
- Committer:
- sawa
- Date:
- 2014-11-05
- Revision:
- 0:f7f2ac58b1ad
- Child:
- 1:4b239aea14f4
File content as of revision 0:f7f2ac58b1ad:
/* 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) { } }