Diff: main.cpp

Revision:

0:f7f2ac58b1ad

Child:

1:4b239aea14f4

--- /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) {
+    }
+
+
+}