Diff: main.cpp

Revision:

43:b084e5f5d0d5

Parent:

42:1cf66990ccab

Child:

44:fe7d5cfd2eea

--- a/main.cpp	Thu Dec 26 00:19:14 2019 +0000
+++ b/main.cpp	Tue Jan 07 08:39:22 2020 +0000
@@ -206,8 +206,10 @@
     make_delay();
 
     //DAC init
-    dac_1 = PRES_A_VREF / 3.3f;
-    dac_2 = PRES_B_VREF / 3.3f;
+//    dac_1 = PRES_A_VREF / 3.3f;
+//    dac_2 = PRES_B_VREF / 3.3f;
+    dac_1 = TORQUE_VREF / 3.3f;
+    dac_2 = 0.0f;
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -412,7 +414,7 @@
 //        float pres_A_new = ((float)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
         float pres_A_new = ((float)ADC1->DR);
         pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
-        torq.sen = - (pres_A.sen-2048.0f)*10000.0f / 2048.0f; //N
+        torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
         
 
         //Pressure sensor 1B
@@ -833,13 +835,11 @@
                         CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
                         CUR_TORQUE_sum = 0;
 
-                        TORQUE_VREF += 0.0001f * (TORQUE_NULL - CUR_TORQUE_mean);
+                        TORQUE_VREF += 0.0003 * (0.0f - CUR_TORQUE_mean);
 
                         if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
                         if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-
-                        ROM_RESET_DATA();
-
+                        
                         //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
                         dac_1 = TORQUE_VREF / 3.3f;
                     }
@@ -859,6 +859,53 @@
                 }
                 TMR3_COUNT_TORQUE_NULL++;
                 break;
+                
+                
+                
+                     //           // DAC Voltage reference set
+//                if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
+//                    CUR_PRES_A_sum += pres_A.sen;
+//                    CUR_PRES_B_sum += pres_B.sen;
+//
+//                    if (TMR3_COUNT_PRES_NULL % 10 == 0) {
+//                        CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
+//                        CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
+//                        CUR_PRES_A_sum = 0;
+//                        CUR_PRES_B_sum = 0;
+//
+//                        float VREF_NullingGain = 0.0003f;
+//                        PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
+//                        PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+//
+//                        if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
+//                        if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
+//                        if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
+//                        if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
+//
+//                        dac_1 = PRES_A_VREF / 3.3f;
+//                        dac_2 = PRES_B_VREF / 3.3f;
+//                    }
+//                } else {
+//                    CONTROL_MODE = MODE_NO_ACT;
+//                    TMR3_COUNT_PRES_NULL = 0;
+//                    CUR_PRES_A_sum = 0;
+//                    CUR_PRES_B_sum = 0;
+//                    CUR_PRES_A_mean = 0;
+//                    CUR_PRES_B_mean = 0;
+//
+//                    ROM_RESET_DATA();
+//
+//                    dac_1 = PRES_A_VREF / 3.3f;
+//                    dac_2 = PRES_B_VREF / 3.3f;
+//                    //pc.printf("nulling end");
+//                }
+//                TMR3_COUNT_PRES_NULL++;
+//                break;
+                
+                
+                
+                
+                
             }
 
             case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
@@ -1677,7 +1724,7 @@
         if (flag_data_request[0] == HIGH) {
             //position+velocity
             //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) torq.sen);
+            CAN_TX_POSITION((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
         }
 
         if (flag_data_request[1] == HIGH) {