Diff: main.cpp

Revision:

241:c53d4c26b04a

Parent:

240:3ffd78cbd029

Child:

242:9f487eaa87b5

--- a/main.cpp	Wed Jul 20 07:50:48 2022 +0000
+++ b/main.cpp	Tue Jul 26 06:58:25 2022 +0000
@@ -270,7 +270,7 @@
         dac_1 = PRES_A_VREF / 3.3f;
         dac_2 = PRES_B_VREF / 3.3f;
     }
-
+    
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -356,9 +356,9 @@
         }
     }
     Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
-    int R_FET = 1.5f;
-    int R_Motor = 5.4f;
-    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
+//    int R_FET = 1.5f;
+//    int R_Motor = 5.4f;
+//    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
 }
 
 // Valve feedforward for SW valve
@@ -390,9 +390,9 @@
         }
     }
     Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
-    int R_FET = 1.5f;
-    int R_Motor = 5.4f;
-    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
+//    int R_FET = 1.5f;
+//    int R_Motor = 5.4f;
+//    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
 }
 
 // PWM duty vs. voltage output of L6205 in STM board
@@ -541,18 +541,26 @@
         LVDT_L = 0;
         LVDT_H = 1;
 
-        for (int ij = 0; ij<150; ij++) {
-
+        for (int ij = 0; ij<120; ij++) {
+            if (ij < 20) {
+                continue;
+            } else if (ij == 20) {
+//                LED = 1;
+            } else if (ij == 100) {
+                LVDT_H = 0;
+                LVDT_L = 0;
+            }
             ADC3->CR2  |= 0x40000000;
             LVDT_new = ((float)ADC3->DR - 2047.0f);
             if(DIR_VALVE_ENC < 0) LVDT_new = 0.0f - LVDT_new;
             LVDT_sum = LVDT_sum + LVDT_new;
         }
+//        LED = 0;
 
-        LVDT_H = 0;
-        LVDT_L = 1;
+//        LVDT_H = 0;
+//        LVDT_L = 0;
 
-        LVDT_new = (LVDT_sum + 2047.0f * 150.0f) * 0.01f*2.0f;
+        LVDT_new = (LVDT_sum + 2047.0f * 100.0f) * 0.01f;
 
         float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
         LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
@@ -570,10 +578,11 @@
 float FREQ_TMR2 = (float)FREQ_5k;
 float DT_TMR2 = (float)DT_5k;
 int cnt_trans = 0;
+int cnt_jitter = 0;
 extern "C" void TIM2_IRQHandler(void)
 {
     if (TIM2->SR & TIM_SR_UIF ) {
-
+        LED = 1;
         if(MODE_POS_FT_TRANS == 1) {
             if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
             alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
@@ -797,9 +806,9 @@
                     Vout.ref = 3000.0f * sin(2.0f*3.14159f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
                 } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
                     Vout.ref = 1000.0f*(ID_index_array[ID_index]);
-                    int R_FET = 1.5f;
-                    int R_Motor = 5.4f;
-                    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
+//                    int R_FET = 1.5f;
+//                    int R_Motor = 5.4f;
+//                    Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f;
                 } else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
                     VALVE_POS_TMP = 0;
                     data_num = 0;
@@ -1394,13 +1403,15 @@
 
         } else {            //////////////////////////sw valve
             // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
-            // L6205PD
-//            if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
-//            else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
-//            else V_out = 0.0f;
-            // IRSM836-45MA
-            if (V_out > 0 ) V_out = V_out*1.654f + 200.0f;
-            else if (V_out < 0) V_out = V_out*1.654f - 200.0f;
+
+            /////////////Jitter/////////////////////
+            V_out = V_out + 4000.0f*sin(2.0f*3.14159f*400.0f*((float) cnt_jitter)*0.0002f);     //4000, 500Hz
+            cnt_jitter++;
+            if(cnt_jitter > 50000000) cnt_jitter = 0;
+
+            //L6205D
+            if (V_out > 0 ) V_out = 800.0f + V_out*1.1275f;
+            else if (V_out < 0) V_out = -800.0f + V_out*1.1275f;
             else V_out = 0.0f;
         }
 
@@ -1417,6 +1428,7 @@
         } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
             V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
         }
+
         PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f);
 
         // Saturation of output voltage
@@ -1470,6 +1482,7 @@
             TMR2_COUNT_CAN_TX = 0;
         }
         TMR2_COUNT_CAN_TX++;
+        LED = 0;
     }
     TIM2->SR = 0x0;  // reset the status register
 }