Diff: main.cpp

Branch:

XBus???

Revision:

31:5d22ebe5f705

Parent:

30:00041540e23c

Child:

32:b03557a08efa

--- a/main.cpp	Fri Mar 10 08:12:54 2017 +0000
+++ b/main.cpp	Fri Mar 10 10:38:17 2017 +0000
@@ -19,7 +19,7 @@
 
 #define ERURON_MOVE_DEG_INI_L -30
 #define DRUG_MOVE_DEG_INI_L -80
-#define ERURON_TRIM_INI_L 113  
+#define ERURON_TRIM_INI_L 113
 #define DRUG_TRIM_INI_L 110
 
 #define kMaxServoNum        1       // 1 - 50
@@ -62,6 +62,7 @@
 bool SERVO_FLAG;
 bool INA_FLAG;
 bool MPU_FLAG;
+uint16_t XbusValue;
 
 char gyro_c[6] = {0,0,0,0,0,0};
 
@@ -175,16 +176,15 @@
 
 void XbusIntervalHandler()
 {
-    uint16_t value;
     uint16_t diff  = kMotionEndMark - kMotionMinMark;
-    value = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
-    gXBus.setServo(servoChannel, value);
+    XbusValue = (uint16_t)(diff * (eruronTrim  + eruronMoveDeg * (eruronfloat-1)) + kMotionMinMark);
+    //pc.printf("%f",value);
+    gXBus.setServo(servoChannel, XbusValue);
     gXBus.sendChannelDataPacket();
 }
 
 void WriteServo()
 {
-    pc.printf("%d",controlValues[4]);
     drugServo.pulsewidth(calcPulse( drugTrim + drugMoveDeg * controlValues[4]));
 }
 
@@ -223,7 +223,7 @@
 {
     init();
     XBusError result;
-    
+
     setTrimPin.mode(PullDown);
     setMaxDegPin.mode(PullDown);
     EDstatePin.mode(PullDown);
@@ -240,8 +240,8 @@
             setMaxDeg();
         }
         //  pc.printf("eT:%f\n\r",eruronTrim);
-        pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-        pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
+       // pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+        //pc.printf("eMD:%f   dMD:%f    ef:%f\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
         led4 = 0;
 
         debugLED = 0;
@@ -250,6 +250,7 @@
         WriteServo();
         updateDatas();
 //      pc.printf("%6d,%6d,%6d\n\r",gyro[0],gyro[1],gyro[2]);
+        pc.printf("%f\n\r",XbusValue);
         led3 = !led3;
         wait(WAIT_LOOP_TIME);
     }