Diff: main.cpp

Branch:

mpu????????

Revision:

77:ca4ab599ba2b

Parent:

74:8ccd04302a7f

Child:

78:e272e65f5a0e

--- a/main.cpp	Sat Dec 23 05:30:55 2017 +0000
+++ b/main.cpp	Wed Feb 28 03:36:22 2018 +0000
@@ -53,7 +53,7 @@
 #define print2pc(flag,fmt,...) do{if(flag){pc.printf(fmt,__VA_ARGS__);}}while(0)
 #define SENDING_DATA_DEBUG_FLAG debugflag[0].flag
 #define GETTING_DATA_DEBUG_FLAG debugflag[1].flag
-#define SERVO_DONFIG_DEBUG_FLAG debugflag[2].flag
+#define SERVO_CONFIG_DEBUG_FLAG debugflag[2].flag
 #define DEBUG_FLAG debugflag[3].flag
 
 struct flaglist{
@@ -80,10 +80,10 @@
 DigitalOut led1(LED1);
 AnalogIn setDeg10(p20);
 AnalogIn setDeg1(p19);
-DigitalIn IsRPin(p13,PullDown);
-DigitalIn InSetModePin(p14,PullDown);
-DigitalIn EDstatePin(p15,PullDown);
-DigitalIn TrimMaxDegPin(p16,PullDown);
+DigitalIn IsRPin(p16,PullDown);
+DigitalIn InSetModePin(p15,PullDown);
+DigitalIn EDstatePin(p14,PullDown);
+DigitalIn TrimMaxDegPin(p13,PullDown);
 DigitalOut servoOff(p17);
 DigitalOut led2(LED2);
 DigitalOut led3(LED3);
@@ -375,29 +375,34 @@
     drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
     eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
     print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff));
+    print2pc(SERVO_CONFIG_DEBUG_FLAG,"eTr:%f   dTr:%f   eMD:%f   dMD:%f\n\r",eruronTrim,drugTrim,eruronMoveDeg,drugMoveDeg);
     print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer);
 }
 
-void setTrim()
+void setTrim(float initEruronState, float initDrugState, float initPinState)
 {
-    led2 =  1;
+    led2 = 1;
+    led4 = 0;
+    if(eruronTrim < 0 || drugTrim < 0)led2 = 0;
     if(EDstatePin) {
-        eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+        eruronTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
         eruronServo.pulsewidth(calcPulse(eruronTrim));
     } else {
-        drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+        drugTrim = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
         drugServo.pulsewidth(calcPulse(drugTrim));
     }
     //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
-    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+    pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
 }
 
-void setMaxDeg()
+void setMaxDeg(float initEruronState, float initDrugState, float initPinState)
 {
     led4 = 1;
-    float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
-    float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+    led2 = 0;
+    float eruronTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initEruronState;
+    float drugTemp = -initPinState + (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10 + initDrugState;
+    if(eruronTemp < 0 || drugTemp < 0)led4 = 0;
     if(EDstatePin) {
         eruronMoveDeg = eruronTemp-eruronTrim;
         eruronServo.pulsewidth(calcPulse(eruronTemp));
@@ -405,7 +410,7 @@
         drugMoveDeg = drugTemp-drugTrim;
         drugServo.pulsewidth(calcPulse(drugTemp));
     }
-    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+    pc.printf("eTr:%f   dTr:%f   ",eruronTrim,drugTrim);
     pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
     wait_us(10);
 }
@@ -420,11 +425,14 @@
 
     while(1) {
         if(InSetModePin){
+            float initEruronState = eruronTrim;
+            float initDrugState = drugTrim;
+            float initPinState = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
             do{
                 if(TrimMaxDegPin)
-                    setTrim();
+                    setTrim(initEruronState,initDrugState,initPinState);
                 else
-                    setMaxDeg();
+                    setMaxDeg(initEruronState,initDrugState,initPinState);
             }while(InSetModePin);
             writeConfig();
         }