Diff: main.cpp

Branch:

mpu????????

Revision:

75:4b6f1b976bec

Parent:

74:8ccd04302a7f

--- a/main.cpp	Sat Dec 23 05:30:55 2017 +0000
+++ b/main.cpp	Wed Feb 07 13:11:57 2018 +0000
@@ -8,6 +8,11 @@
 * -----
 *
 ******************************************
+*LED1: blink in receiveDatas
+*LED2: light in setTrim
+*LED3: blink in main loop
+*LED4: light in setMaxDeg
+******************************************
 **magic character of debug**
 *
 *(0)s:sending datas: mpu, servoV
@@ -74,16 +79,16 @@
 CANMessage recmsg;
 Serial pc(USBTX,USBRX);
 I2C ina226_i2c(p28,p27);
-INA226 VCmonitor(ina226_i2c);
+INA226 VCmonitor(ina226_i2c,0x4A,400000);
 PwmOut drugServo(p23);
 PwmOut eruronServo(p25);
 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);
@@ -362,7 +367,7 @@
 
 double calcPulse(float analog)
 {
-    return (0.0006 + (analog)*(0.00240-0.00060) );
+    return (0.6 + (analog)*(2.40-0.60) );
 }
 
 void WriteServo()
@@ -372,59 +377,78 @@
 //    }
 //    pc.printf(" : %f",eruronfloat);
 //    pc.printf("\n\r");
-    drugServo.pulsewidth(calcPulse(drugTrim + drugMoveDeg *drugInput));
-    eruronServo.pulsewidth(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
+    drugServo.pulsewidth_ms(calcPulse(drugTrim + drugMoveDeg *drugInput));
+    eruronServo.pulsewidth_ms(calcPulse( eruronTrim + eruronMoveDeg * eruronfloat));
     print2pc(GETTING_DATA_DEBUG_FLAG,"ef:%5.2f di:%d so:%d\n\r",eruronfloat,drugInput,int(servoOff));
     print2pc(DEBUG_FLAG,"servoOffVer:%d\n\r",servoOffVer);
 }
 
-void setTrim()
+void setTrim(float baseinp)
 {
+    float inp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10 - baseinp;
     led2 =  1;
     if(EDstatePin) {
-        eruronTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
-        eruronServo.pulsewidth(calcPulse(eruronTrim));
+        eruronTrim = eruronServo.read();
+        
+        eruronServo.pulsewidth_ms(calcPulse(inp));
     } else {
-        drugTrim = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
-        drugServo.pulsewidth(calcPulse(drugTrim));
+        drugTrim = drugServo.read();
+        drugServo.pulsewidth_ms(calcPulse(inp));
     }
-    //pc.printf("eruronTrim:%f    drugTrim:%f\n\r",eruronTrim,drugTrim);
-    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
 }
 
-void setMaxDeg()
+void setMaxDeg(float baseinp)
 {
+    float inp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10 - baseinp;
     led4 = 1;
-    float eruronTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
-    float drugTemp = (int)(setDeg10.read()*9)/10.0 + setDeg1.read()/10;
     if(EDstatePin) {
+        float eruronTemp = eruronServo.read();
         eruronMoveDeg = eruronTemp-eruronTrim;
-        eruronServo.pulsewidth(calcPulse(eruronTemp));
+        eruronServo.pulsewidth_ms(calcPulse(inp));
     } else {
+        float drugTemp = drugServo.read();
         drugMoveDeg = drugTemp-drugTrim;
-        drugServo.pulsewidth(calcPulse(drugTemp));
+        drugServo.pulsewidth_ms(calcPulse(inp));
     }
-    pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
-    pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
     wait_us(10);
 }
+void setServoConfig(PwmOut servo, float* trim, float* movedeg, float base){
+    
+    float neji = (setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+    printf("\nneji:%f\n\r",neji);
+    
+    if(TrimMaxDegPin){
+        servo.write(base + neji);
+        *trim = base + neji;
+        led2 = !led2;
+    }else{
+        servo.write(base + neji);
+        *movedeg = base + neji - *trim;
+        led4 = !led4;
+    }
+}
 
 int main()
 {
     init();
     Thread mpu_thread(&mpuProcessing);
 
-//    start motion
-//    gTimer.attach_us(&XbusIntervalHandler, 1000000 / kMotionInterval);
-
     while(1) {
         if(InSetModePin){
+            float baseinp = (setDeg10.read()*9)/10.0 + setDeg1.read()/10;
+            float eruronIni = eruronServo.read();
+            float drugIni = drugServo.read();
+            
             do{
-                if(TrimMaxDegPin)
-                    setTrim();
-                else
-                    setMaxDeg();
+                if(EDstatePin){
+                    setServoConfig(eruronServo, &eruronTrim, &eruronMoveDeg, eruronIni - baseinp);
+                }else{
+                    setServoConfig(drugServo, &drugTrim, &drugMoveDeg, drugIni - baseinp);
+                }
+                
+                pc.printf("eruronTrim:%f    drugTrim:%f    ",eruronTrim,drugTrim);
+                pc.printf("eMD:%f   dMD:%f\n\r",eruronMoveDeg,drugMoveDeg);
+                
             }while(InSetModePin);
             writeConfig();
         }