main.cpp

00001 /* main.cpp file
00002  *
00003  * for testing mbed motion player with XBusServo.cpp
00004  *
00005  * Copyright (c) 2014-2014 JR PROPO
00006  * Released under the MIT License: http://mbed.org/license/mit
00007  *
00008  * by Zak Sawa
00009  */
00010 
00011 #include "mbed.h"
00012 #include "XBusServo.h"
00013 
00014 //#define DEBUGmain
00015 
00016 #ifdef DEBUGmain
00017 #define DBG(fmt) printf(fmt)
00018 #define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
00019 #else
00020 #define DBG(...)
00021 #define DBGF(...)
00022 #endif
00023 
00024 //
00025 // defines
00026 //
00027 #define kXBusTx             PTE0    // PTD3 p13
00028 #define kXBusRx             PTE1    // PTD2 p14
00029 #define kXBusSwitch         D10
00030 #define kMaxServoNum        4       // 1 - 50
00031 
00032 #define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
00033 
00034 #define kMotionInterval     30      // flame / sec
00035 #define kMotionEndMark      0xFFFF
00036 
00037 //
00038 // typedefs
00039 //
00040 typedef enum poseDataType {
00041     dataType_Pose,
00042     numOfDataType
00043 } poseDataType;
00044 
00045 
00046 typedef struct poseRec {
00047     poseDataType            poseType;
00048     uint16_t                nextPoseIndex;
00049     int32_t                 servoPosition[kMaxServoNum];
00050     uint16_t                frameFromBeforePause;
00051 }
00052 poseRec;
00053 
00054 
00055 //
00056 // sample motion data
00057 //
00058 static const uint8_t        servoChannel[kMaxServoNum] = {0x01, 0x02, 0x03, 0x04};
00059 
00060 static const poseRec        motionData[] = {
00061 //      data type        nextPoseIndex         servoPosition                 frameFromBeforePause
00062     {dataType_Pose,         1,              {0x7FFF, 0x7FFF, 0x7FFF, 0x7FFF},           10},
00063     {dataType_Pose,         2,              {0x5FFF, 0x8FFF, 0xAFFF, 0x4FFF},           30},
00064     {dataType_Pose,         3,              {0x9FFF, 0x5FFF, 0x6FFF, 0xAFFF},           10},
00065     {dataType_Pose,         4,              {0x9FFF, 0xEFFF, 0x7FFF, 0x1FFF},           20},
00066     {dataType_Pose,         kMotionEndMark, {0x2FFF, 0x5FFF, 0x8FFF, 0x5FFF},           30}
00067 };
00068 
00069 
00070 //
00071 // global vars
00072 //
00073 XBusServo                       gXBus(kXBusTx, kXBusRx, kXBusSwitch, kMaxServoNum);
00074 Ticker                          gTimer;
00075 volatile uint16_t               gCurrentFlame = 0;
00076 volatile uint16_t               gCurrentIndex = 0;
00077 volatile uint16_t               gBeforeIndex = 0;
00078 volatile uint16_t               gCurrentServoPos[kMaxServoNum];
00079 
00080 
00081 //=============================================================
00082 // XbusIntervalHandler()
00083 // play motion !
00084 //=============================================================
00085 void XbusIntervalHandler()
00086 {
00087     uint16_t            counter;
00088 
00089     // send current data
00090     gXBus.sendChannelDataPacket();
00091     if (gCurrentIndex == kMotionEndMark)
00092         return;
00093 
00094     // make next data
00095     gCurrentFlame++;
00096 
00097     if (gCurrentFlame == motionData[gCurrentIndex].frameFromBeforePause) {
00098         gBeforeIndex = gCurrentIndex;
00099         gCurrentIndex = motionData[gCurrentIndex].nextPoseIndex;
00100         gCurrentFlame = 0;
00101     }
00102 
00103     if (gCurrentIndex == kMotionEndMark)
00104         return;
00105 
00106     if (gCurrentIndex != gBeforeIndex)
00107         for (counter = 0; counter < kMaxServoNum; counter++) {
00108             uint16_t            value;
00109             int32_t             temp;
00110 
00111             value = motionData[gBeforeIndex].servoPosition[counter];
00112             temp = motionData[gCurrentIndex].servoPosition[counter] - motionData[gBeforeIndex].servoPosition[counter];
00113             temp *= gCurrentFlame;
00114             temp /= motionData[gCurrentIndex].frameFromBeforePause;
00115             value += temp;
00116 
00117             gXBus.setServo(servoChannel[counter], value);
00118         }
00119 }
00120 
00121 
00122 //=============================================================
00123 // init()
00124 //  initialize all setup
00125 //=============================================================
00126 XBusError init()
00127 {
00128     XBusError           result;
00129     uint16_t            counter;
00130 
00131     // initialize XBus
00132     result = gXBus.start();
00133     if (result != kXBusError_NoError) {
00134         gXBus.stop();
00135         return result;
00136     }
00137 
00138     // initialize XBus Servos
00139     for (counter = 0; counter < kMaxServoNum; counter++) {
00140         result = gXBus.addServo(servoChannel[counter], kXbusServoNeutral);
00141         if (result != kXBusError_NoError) {
00142             gXBus.stop();
00143             return result;
00144         }
00145     }
00146 
00147     return kXBusError_NoError;
00148 }
00149 
00150 
00151 //=============================================================
00152 // main()
00153 //
00154 //=============================================================
00155 int main()
00156 {
00157     if (init() != kXBusError_NoError)
00158         return -1;
00159 
00160     // start motion
00161     gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
00162 
00163     while(1) {
00164     }
00165 
00166 
00167 }