Diff: main.cpp

Revision:

2:32aba462e4df

Parent:

0:301100d3469a

Child:

3:db5a2f683282

--- a/main.cpp	Wed Jan 04 09:10:05 2017 +0000
+++ b/main.cpp	Wed Jan 11 10:01:47 2017 +0000
@@ -20,12 +20,13 @@
 
 #define kMaxServoPause     (sizeof(motionData) / sizeof(pauseRec))
 
-#define kMotionInterval     30      // flame / sec
+#define kMotionInterval     10      // flame / sec
 #define kMotionMinMark      0x1249
 #define kMotionEndMark      0xED86
 
 AnalogIn analog(p20);
 Serial pc(USBTX,USBRX);
+DigitalOut Led(LED1);
 
 //
 // typedefs
@@ -52,24 +53,6 @@
 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 !
@@ -78,10 +61,9 @@
 {
     uint16_t value;
     uint16_t diff  = kMotionEndMark - kMotionMinMark;
-    // send current data
-    gXBus.sendChannelDataPacket();
     value = (uint16_t)(diff * analog.read()) + kMotionMinMark;
     gXBus.setServo(servoChannel, value);
+    gXBus.sendChannelDataPacket();
 }
 
 //=============================================================
@@ -106,6 +88,15 @@
         return result;
     }
 
+    int16_t index = 0x1D;
+    int16_t value = 0x01;
+  //  int16_t Angle = 0x0011;
+
+    result = gXBus.setCommand(kXBusOrder_1_Angle_180,&value);
+   // result = gXBus.setCommand(kX);
+
+ //   result = gXBus.setCommand(kXBusOrder_2_ParamWrite,&index);
+
     return kXBusError_NoError;
 }
 
@@ -115,8 +106,12 @@
 //=============================================================
 int main()
 {
-    if (init() != kXBusError_NoError)
+    if (init() != kXBusError_NoError) {
         return -1;
+        Led = 1;
+        wait(2);
+        Led = 0;
+    }
 
     // start motion
     gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);