Diff: main.cpp

Revision:

20:806196fda269

Parent:

19:23b7c1ad8683

Child:

21:e5f1a43ea6f9

diff -r 23b7c1ad8683 -r 806196fda269 main.cpp
--- a/main.cpp	Mon Sep 09 06:29:56 2019 +0000
+++ b/main.cpp	Mon Sep 09 06:57:31 2019 +0000
@@ -180,6 +180,11 @@
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
     
+    // TMR5 init
+    Init_TMR5();
+    TIM5->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
+    
     // CAN
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
@@ -1588,7 +1593,7 @@
         //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
         //PWM_out = CUR_PWM;
 
-
+/*
 
         //CAN ----------------------------------------------------------------------
         //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
@@ -1651,7 +1656,7 @@
         }
         TMR3_COUNT_CAN_TX++;
 
-
+*/
 
 
         /*******************************************************
@@ -1728,6 +1733,74 @@
     
 }
 
+
+
+//unsigned long CNT_TMR5 = 0;
+//double FREQ_TMR5 = (double)FREQ_500;
+//double DT_TMR5 = (double)DT_500;
+extern "C" void TIM5_IRQHandler(void)
+{ 
+    if (TIM5->SR & TIM_SR_UIF ) {
+        
+        //CAN ----------------------------------------------------------------------
+        if (flag_data_request[0] == HIGH) {
+            //position+velocity
+            CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+            //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
+            //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
+            //pc.printf("can good");
+            //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
+        }
+
+        if (flag_data_request[1] == HIGH) {
+            //torque
+            //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
+            //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+            CAN_TX_TORQUE((int16_t) (cur.sen));
+            //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
+            //            CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
+        }
+
+        if (flag_data_request[2] == HIGH) {
+            //pressure A and B
+            //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+            //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
+            //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
+            //            CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
+
+        }
+
+        if (flag_data_request[3] == HIGH) {
+            //PWM
+            CAN_TX_PWM((int16_t) CUR_PWM);
+            //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
+            //            CAN_TX_PWM((int16_t) cnt_vel_findhome);
+            //            CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
+            //                        CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
+
+        }
+
+        if (flag_data_request[4] == HIGH) {
+            //valve position
+            CAN_TX_VALVE_POSITION((int16_t) (value));
+            //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
+            //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
+
+
+            //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
+            //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
+            //            CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
+            //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
+            //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
+        }
+
+    }
+    TIM5->SR = 0x0;  // reset the status register
+}
+
+
+
 void CurrentControl()
 {
     cur.err = cur.ref - cur.sen;