Diff: main.cpp

Revision:

245:071785d74ad0

Parent:

244:e9c5ec04e378

Child:

246:d5742551f432

--- a/main.cpp	Fri Jun 17 06:06:53 2022 +0000
+++ b/main.cpp	Fri Jun 17 09:28:54 2022 +0000
@@ -270,18 +270,10 @@
         dac_1 = FORCE_VREF / 3.3f;
         dac_2 = 0.0f;
     } else if (SENSING_MODE == 1) {
-//        if (DIR_VALVE_ENC > 0) {
         dac_1 = PRES_A_VREF / 3.3f;
         dac_2 = PRES_B_VREF / 3.3f;
-//        } else {
-//            dac_1 = PRES_B_VREF / 3.3f;
-//            dac_2 = PRES_A_VREF / 3.3f;
-//        }
     }
 
-    dac_1 = 1.0f/3.3f;
-    dac_2 = 1.0f/3.3f;
-
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -471,8 +463,8 @@
 {
     if (TIM3->SR & TIM_SR_UIF ) {
 
-        if (LED > 0) LED = 0;
-        else LED = 1;
+//        if (LED > 0) LED = 0;
+//        else LED = 1;
 
         float PSEN1 = 0.0f;
         float PSEN2 = 0.0f;
@@ -496,10 +488,15 @@
 
         /////////////////////////Force or Pressure//////////////////////////////////////////////////////////////////
         if (SENSING_MODE == 0) {  // Force sensing
-        
+
             HAL_ADC_Start(&hadc1);
             HAL_ADC_PollForConversion(&hadc1, 1);
             PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
+
+            HAL_ADC_Start(&hadc1);
+            HAL_ADC_PollForConversion(&hadc1, 1);
+            PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
+
             force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N
 
         } else if (SENSING_MODE == 1) { // Pressure sensing
@@ -579,6 +576,7 @@
 float FREQ_TMR2 = (float)FREQ_5k;
 float DT_TMR2 = (float)DT_5k;
 int cnt_trans = 0;
+float nulling_test  = 0.0f;
 extern "C" void TIM2_IRQHandler(void)
 {
     if (TIM2->SR & TIM_SR_UIF ) {
@@ -666,9 +664,9 @@
             }
 
             case MODE_TORQUE_SENSOR_NULLING: {
-                static float FORCE_pulse_sum = 0.0;
-                static float PresA_pulse_sum = 0.0;
-                static float PresB_pulse_sum = 0.0;
+                static float FORCE_pulse_sum = 0.0f;
+                static float PresA_pulse_sum = 0.0f;
+                static float PresB_pulse_sum = 0.0f;
 
                 // DAC Voltage reference set
                 float VREF_TuningGain = -0.000003f;
@@ -679,8 +677,8 @@
                         if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
                             float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
                             FORCE_pulse_sum = 0.0f;
-
                             FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
+                            nulling_test = FORCE_VREF;
                             if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
                             if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
                             dac_1 = FORCE_VREF / 3.3f;
@@ -1426,19 +1424,17 @@
         ////////////////////////////////////////////////////////////////////////////
         //////////////////////  Data transmission through CAN //////////////////////
         ////////////////////////////////////////////////////////////////////////////
-    
+
 //        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
         if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) {
             // Position, Velocity, and Torque (ID:1200)
 
-            if (flag_data_request[0] == LOW) {
-                CAN_TX_POSITION_FT((int16_t) (cur.sen*1000.0f), (int16_t) (V_EXI), (int16_t) (valve_pos.sen));
-//                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//
-//                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-//                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-//                }
+            if (flag_data_request[0] == HIGH) {
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+                }
             }
 
             // Valve Position (ID:1300)
@@ -1446,7 +1442,8 @@
                 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));
+//                    CAN_TX_PWM((int16_t)(valve_pos.sen));
+                    CAN_TX_PWM((int16_t)(nulling_test));
                 }
             }