Diff: main.cpp

Revision:

53:4d66fb1c5dd9

Parent:

52:8ea76864368a

--- a/main.cpp	Wed Feb 19 05:48:57 2020 +0000
+++ b/main.cpp	Tue Feb 25 07:39:17 2020 +0000
@@ -192,7 +192,7 @@
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
 //    NVIC_SetPriority(TIM2_IRQn, 3);
-    NVIC_SetPriority(TIM4_IRQn, 4);
+    NVIC_SetPriority(TIM4_IRQn, 3);
 
     //can.reset();
     can.filter(msg.id, 0xFFFFF000, CANStandard);
@@ -405,11 +405,12 @@
 int j =0;
 //unsigned long CNT_TMR3 = 0;
 //float FREQ_TMR3 = (float)FREQ_5k;
-float FREQ_TMR3 = (float)FREQ_1k;
+float FREQ_TMR3 = (float)FREQ_5k;
 float DT_TMR3 = (float)DT_5k;
 //float DT_TMR3 = (float)DT_1k;
 int cnt_trans = 0;
 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
+int canfreq = CAN_FREQUENCY;
 
 extern "C" void TIM3_IRQHandler(void)
 {
@@ -455,10 +456,14 @@
 
                 float VALVE_POS_RAW_FORCE_FB = 0.0f;
 
+                
+                double torq_ref = 0.0f;
+                //if(TMR3_COUNT_TEST % (int) (50) == 0){
                 pos.err = pos.ref - pos.sen; //[pulse]
                 vel.err = vel.ref - vel.sen; //[pulse/s]
-                double torq_ref = 0.0f;
                 torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
+                    //torq_ref_logging = torq_ref
+                //}
 
                 // torque feedback
                 torq.err = torq_ref - torq.sen; //[pulse]
@@ -493,9 +498,12 @@
                 }    
             
                 VALVE_POS_CONTROL(valve_pos.ref);
-
+                
+                //TMR3_COUNT_TEST++;
+                
                 break;
             }
+            
 
             case MODE_VALVE_OPEN_LOOP: {
                 V_out = (float) Vout.ref;
@@ -1634,57 +1642,61 @@
 
 
         
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k / CAN_FREQ) == 0) {
-
-        // Position, Velocity, and Torque (ID:1200)
-        if (flag_data_request[0] == HIGH) {
-            if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
-                if (SENSING_MODE == 0) {
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
-                } else if (SENSING_MODE == 1) {
-                    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
-                }
-            } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
-                if (SENSING_MODE == 0) {
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
-                } else if (SENSING_MODE == 1) {
-                    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/canfreq) == 0) {
+//        if (TMR2_COUNT_CAN_TX % 10 == 0) {
+            // Position, Velocity, and Torque (ID:1200)
+            if (flag_data_request[0] == HIGH) {
+                if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+                    }
+                } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+                    if (SENSING_MODE == 0) {
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
+                    } else if (SENSING_MODE == 1) {
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+                    }
                 }
             }
-        }
-        if (flag_data_request[1] == HIGH) {
-            //valve position
-            double t_value = 0;
-            if(value>=DDV_CENTER) {
-                t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
-            } else {
-                t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+            if (flag_data_request[1] == HIGH) {
+                //valve position
+                double t_value = 0;
+                if(value>=DDV_CENTER) {
+                    t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+                } else {
+                    t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+                }
+                CAN_TX_TORQUE((int16_t) (t_value));
             }
-            CAN_TX_TORQUE((int16_t) (t_value));
-        }
 
-        if (flag_data_request[2] == HIGH) {
-            //pressure A and B
-            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-        }
+            if (flag_data_request[2] == HIGH) {
+                //pressure A and B
+                CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            }
 
-        if (flag_data_request[3] == HIGH) {
-            //PWM
-            CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
-        }
+            if (flag_data_request[3] == HIGH) {
+                //PWM
+                CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
+            }
 
-        if (flag_data_request[4] == HIGH) {
-            //valve position
-            CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
-        }
+            if (flag_data_request[4] == HIGH) {
+                //valve position
+                CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
+            }
 
-        // Others : Reference position, Reference FT, PWM, Current  (ID:1300)
+            // Others : Reference position, Reference FT, PWM, Current  (ID:1300)
 //        if (flag_data_request[1] == HIGH) {
 //            CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
 //        }
+            //if (flag_delay_test == 1){
+                //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
+            //}
 
-        TMR2_COUNT_CAN_TX = 0;
-    }
+            TMR2_COUNT_CAN_TX = 0;
+        }
+        TMR2_COUNT_CAN_TX++;