Diff: main.cpp

Revision:

248:544011ad9c85

Parent:

247:bfdf0f479a38

Child:

249:f69cd0553be0

diff -r bfdf0f479a38 -r 544011ad9c85 main.cpp
--- a/main.cpp	Fri Jun 24 00:18:20 2022 +0000
+++ b/main.cpp	Fri Jun 24 05:55:29 2022 +0000
@@ -198,7 +198,6 @@
     enc.format(8,0);
     enc.frequency(5000000); //10M
     enc_cs = 0;
-
     make_delay();
 
     // spi _ enc
@@ -360,21 +359,19 @@
 }
 
 // Valve feedforward for SW valve
-void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS_10000)
+void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS)
 {
     int i = 0;
     float valve_pos_ref_dz = 0.0f;
-    if(REF_VALVE_POS_10000 > 10000.0f) {
-        REF_VALVE_POS_10000 = 10000.0f;
-    } else if(REF_VALVE_POS_10000 < -10000.0f) {
-        REF_VALVE_POS_10000 = -10000.0f;
+    if(REF_VALVE_POS > VALVE_MAX_POS) {
+        REF_VALVE_POS = VALVE_MAX_POS;
+    } else if(REF_VALVE_POS < VALVE_MIN_POS) {
+        REF_VALVE_POS = VALVE_MIN_POS;
     }
-    if (REF_VALVE_POS_10000 >= 0) {
-//        valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
-        valve_pos_ref_dz = REF_VALVE_POS_10000;
+    if (REF_VALVE_POS >= 0) {
+        valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER;
     } else {
-//        valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
-        valve_pos_ref_dz = REF_VALVE_POS_10000;
+        valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER;
     }
     valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen);
     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
@@ -547,7 +544,7 @@
         for (int ij = 0; ij<150; ij++) {
 
             ADC3->CR2  |= 0x40000000;
-            LVDT_new = ((float)ADC3->DR) - 2047.5f;
+            LVDT_new = ((float)ADC3->DR);
             LVDT_sum = LVDT_sum + LVDT_new;
         }
 
@@ -836,8 +833,6 @@
                     ID_index = 0;
                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
                 }
-
-
                 break;
             }
             case MODE_DDV_DEADZONE_AND_CENTER: {
@@ -1076,7 +1071,7 @@
                     valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
-                VALVE_POS_CONTROL(valve_pos_raw.ref);
+                VALVE_POS_CONTROL_DZ(valve_pos_raw.ref);
 
                 ref_array[cnt_step_test] = valve_pos_ref;
                 if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) {
@@ -1111,7 +1106,7 @@
                     valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
                 }
 
-                VALVE_POS_CONTROL(valve_pos_raw.ref);
+                VALVE_POS_CONTROL_DZ(valve_pos_raw.ref);
 
                 ref_array[cnt_freq_test] = valve_pos_ref;
 //                if(value>=(float) VALVE_ELECTRIC_CENTER) {
@@ -1436,7 +1431,13 @@
                 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
                     CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
                 } else {
-                    CAN_TX_PWM((int16_t)(valve_pos.sen));
+                    float valve_pos_can = 0.0f;
+                    if(valve_pos.sen >= VALVE_ELECTRIC_CENTER) {
+                        valve_pos_can = 10000.0f*((float)valve_pos.sen-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
+                    } else {
+                        valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
+                    }
+                    CAN_TX_PWM((int16_t)(valve_pos_can));
                 }
             }