rainbow

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Comitter:
Lightvalve
Date:
Fri Jun 24 05:55:29 2022 +0000
Parent:
247:bfdf0f479a38
Child:
249:f69cd0553be0
Commit message:
220624_2

Changed in this revision

CAN/function_CAN.cpp Show annotated file Show diff for this revision Revisions of this file
INIT_HW/INIT_HW.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/CAN/function_CAN.cpp	Fri Jun 24 00:18:20 2022 +0000
+++ b/CAN/function_CAN.cpp	Fri Jun 24 05:55:29 2022 +0000
@@ -732,11 +732,11 @@
             valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000)
         } else { //SW Valve
             if(temp_ref_valve_pos >= 0) {
-//                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
-                valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
+                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+//                valve_pos.ref = (double)temp_ref_valve_pos;
             } else {
-//                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
-                valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
+                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+//                valve_pos.ref = (double)temp_ref_valve_pos;
             }
         }
     } else if(address==CID_RX_REF_PWM) {
@@ -1089,8 +1089,8 @@
     CANMessage temp_msg;
      int16_t temp_valve_min_pos = 0;
      int16_t temp_valve_max_pos = 0;
-     temp_valve_min_pos = (int16_t) ((float) VALVE_MIN_POS * 0.1f);
-     temp_valve_max_pos = (int16_t) ((float) VALVE_MAX_POS * 0.1f);
+     temp_valve_min_pos = (int16_t) VALVE_MIN_POS;
+     temp_valve_max_pos = (int16_t) VALVE_MAX_POS;
 
     temp_msg.id = CID_TX_INFO;
     temp_msg.len = 5;
@@ -1246,9 +1246,9 @@
     int16_t valve_pos_vs_pwm;
 
     if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_ELECTRIC_CENTER) {
-        valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER);
+        valve_pos_vs_pwm = (int16_t) (10000.0f*((float)VALVE_POS_VS_PWM[canindex]-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER));
     } else {
-        valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER);
+        valve_pos_vs_pwm = (int16_t) (-10000.0f*((float)VALVE_POS_VS_PWM[canindex]-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER));
     }
 
     int16_t PWM_VALVE_ID;
--- a/INIT_HW/INIT_HW.cpp	Fri Jun 24 00:18:20 2022 +0000
+++ b/INIT_HW/INIT_HW.cpp	Fri Jun 24 05:55:29 2022 +0000
@@ -225,7 +225,7 @@
     TIM4->CR1 |= TIM_CR1_UDIS;
     TIM4->EGR |= TIM_EGR_UG;
 
-    TIM4->PSC = 10-1;                                            // 10 prescaler, timer counts up in sync with the peripheral clock
+    TIM4->PSC = 20-1;                                            // 10 prescaler, timer counts up in sync with the peripheral clock
     TIM4->ARR = 1;
     TIM4->CNT = 0;
     TIM4->SR = 0; 
--- 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));
                 }
             }