Diff: main.cpp

Revision:

1:fee82ed64991

Parent:

0:301100d3469a

--- a/main.cpp	Wed Jan 04 09:10:05 2017 +0000
+++ b/main.cpp	Sat Jan 07 09:41:37 2017 +0000
@@ -26,25 +26,22 @@
 
 AnalogIn analog(p20);
 Serial pc(USBTX,USBRX);
+DigitalOut Led(LED1);
 
 //
-// typedefs
+// global vars
 //
-typedef enum poseDataType {
-    dataType_Pose,
-    numOfDataType
-} poseDataType;
-
+unsigned char           gOK2SendPacket = true;
+unsigned int            gCurrentPos = kXbusServoNeutral;
+long int            gCurrentValue = 0;
+unsigned char           gCurrentID = 1;
+unsigned char           gCurrentSubID = 0;
+uint8_t Angle  = 0x1D;
+int16_t AngleValue = 0x01;
+static const uint8_t        servoChannel = 0x01;
 
-typedef struct poseRec {
-    poseDataType            poseType;
-    uint16_t                nextPoseIndex;
-    int32_t                 servoPosition[kMaxServoNum];
-    uint16_t                frameFromBeforePause;
-}
-poseRec;
-
-static const uint8_t        servoChannel = 0x01;
+uint16_t value;
+uint16_t diff  = kMotionEndMark - kMotionMinMark;
 
 //
 // global vars
@@ -52,39 +49,51 @@
 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);
+    if(gOK2SendPacket) {
+        value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
+        gXBus.setServo(servoChannel, value);
+
+        // send current data
+        gXBus.sendChannelDataPacket();
+    }
 }
 
 //=============================================================
+// setCurrentValue()
+//  set current value to the servo for temp
+//=============================================================
+void setCurrentValue()
+{
+    XBusError       result;
+    for(;;) {
+        result = gXBus.setCommand(Angle, &AngleValue);
+        wait_ms(10);
+        if (result == kXBusError_NoError)
+            break;
+    }
+}
+
+//=============================================================
+// writeCurrentValue()
+//  write current value to the servo
+//=============================================================
+void writeCurrentValue()
+{
+    XBusError       result;
+    for (;;) {
+        result = gXBus.setCommand(Angle, &AngleValue);
+        wait_ms(10);
+        if (result == kXBusError_NoError)
+            break;
+    }
+}
+//=============================================================
 // init()
 //  initialize all setup
 //=============================================================
@@ -105,14 +114,9 @@
         gXBus.stop();
         return result;
     }
-
     return kXBusError_NoError;
 }
 
-//=============================================================
-// main()
-//
-//=============================================================
 int main()
 {
     if (init() != kXBusError_NoError)
@@ -122,5 +126,12 @@
     gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
 
     while(1) {
+        gOK2SendPacket = false;
+      //  setCurrentValue();
+       // writeCurrentValue();
+
+        value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
+        gXBus.setServo(servoChannel,value );
+        gOK2SendPacket = true;
     }
 }