Diff: main.cpp

Revision:

0:301100d3469a

Child:

1:fee82ed64991

Child:

2:32aba462e4df

diff -r 000000000000 -r 301100d3469a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jan 04 09:10:05 2017 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include "XBusServo.h"
+#include <string>
+
+//#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             p9
+#define kMaxServoNum        1       // 1 - 50
+
+#define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
+
+#define kMotionInterval     30      // flame / sec
+#define kMotionMinMark      0x1249
+#define kMotionEndMark      0xED86
+
+AnalogIn analog(p20);
+Serial pc(USBTX,USBRX);
+
+//
+// typedefs
+//
+typedef enum poseDataType {
+    dataType_Pose,
+    numOfDataType
+} poseDataType;
+
+
+typedef struct poseRec {
+    poseDataType            poseType;
+    uint16_t                nextPoseIndex;
+    int32_t                 servoPosition[kMaxServoNum];
+    uint16_t                frameFromBeforePause;
+}
+poseRec;
+
+static const uint8_t        servoChannel = 0x01;
+
+//
+// global vars
+//
+XBusServo gXBus(kXBusTx, NC, NC, kMaxServoNum);
+Ticker gTimer;
+
+string Deci2Hex(int val,bool lower = false){
+    string str = "0x";
+    char answer[10];
+    char henkan[17] = "0123456789ABCDEF"; 
+    int i = 0,j = 0;
+    while(val > 0){
+        j = val % 16;
+        val = val / 16;
+        answer[i] = henkan[j];
+        i++;
+    }
+    i--;
+    for(j = i;j >= 0;j--){
+        str += answer[j];
+    }
+    return str;
+}
+
+//=============================================================
+// XbusIntervalHandler()
+// play motion !
+//=============================================================
+void XbusIntervalHandler()
+{
+    uint16_t value;
+    uint16_t diff  = kMotionEndMark - kMotionMinMark;
+    // send current data
+    gXBus.sendChannelDataPacket();
+    value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
+    gXBus.setServo(servoChannel, value);
+}
+
+//=============================================================
+// init()
+//  initialize all setup
+//=============================================================
+XBusError init()
+{
+    XBusError result;
+
+    // initialize XBus
+    result = gXBus.start();
+    if (result != kXBusError_NoError) {
+        gXBus.stop();
+        return result;
+    }
+
+    // initialize XBus Servos
+    result = gXBus.addServo(servoChannel, kXbusServoNeutral);
+    if (result != kXBusError_NoError) {
+        gXBus.stop();
+        return result;
+    }
+
+    return kXBusError_NoError;
+}
+
+//=============================================================
+// main()
+//
+//=============================================================
+int main()
+{
+    if (init() != kXBusError_NoError)
+        return -1;
+
+    // start motion
+    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
+
+    while(1) {
+    }
+}