Diff: main.cpp

Branch:

mpu????????

Revision:

68:ae3c3500074c

Parent:

67:94fe2180f39a

Child:

69:073116d31a56

--- a/main.cpp	Sat Jul 15 02:45:03 2017 +0000
+++ b/main.cpp	Fri Sep 29 08:13:16 2017 +0000
@@ -46,12 +46,12 @@
 PwmOut drugServo(p22);
 PwmOut eruronServo(p23);
 DigitalOut led1(LED1);
-AnalogIn drugAna(p20);
-AnalogIn eruronAna(p19);
-DigitalIn IsRPin(p11);
-DigitalIn setTrimPin(p12);
-DigitalIn EDstatePin(p14);
-DigitalIn setMaxDegPin(p15);
+AnalogIn eruronAna(p20);
+AnalogIn drugAna(p19);
+DigitalIn IsRPin(p11,PullDown);
+DigitalIn InSetModePin(p12,PullDown);
+DigitalIn EDstatePin(p14,PullDown);
+DigitalIn TrimMaxDegPin(p15,PullDown);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
@@ -317,7 +317,7 @@
 //    pc.printf("\n\r");
     drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
     eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
-//    pc.printf("ef:%10f  %5f\n\r",eruronfloat,drugInput);
+    pc.printf("ef:%10f  %5f\n\r",eruronfloat,drugInput);
 }
 
 void setTrim()
@@ -344,8 +344,8 @@
         eruronMoveDeg = eruronTemp-eruronTrim;
         eruronServo.pulsewidth(calcPulse(eruronTemp));
     } else {
+        drugMoveDeg = drugTemp-drugTrim;
         drugServo.pulsewidth(calcPulse(drugTemp));
-        drugMoveDeg = drugTemp-drugTrim;
     }
     pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f    ef:%d\n\r",eruronMoveDeg,drugMoveDeg,eruronfloat);
@@ -355,32 +355,24 @@
 int main()
 {
     init();
-
-    setTrimPin.mode(PullDown);
-    setMaxDegPin.mode(PullDown);
-    EDstatePin.mode(PullDown);
-    IsRPin.mode(PullDown);
     Thread mpu_thread(&mpuProcessing);
 
     // start motion
 //    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
 
     while(1) {
-        if(setTrimPin) {
-            do {
-                setTrim();
-            } while(setTrimPin);
-            writeConfig();
-        }
-        if(setMaxDegPin) {
-            do {
-                setMaxDeg();
-            } while(setMaxDegPin);
+        if(InSetModePin){
+            do{
+                if(TrimMaxDegPin)
+                    setTrim();
+                else
+                    setMaxDeg();
+            }while(InSetModePin);
             writeConfig();
         }
         led4 = 0;
         led2 = 0;
-        //receiveDatas();
+        receiveDatas();
 //        sendDatas();
         WriteServo();
         updateDatas();