Diff: main.cpp

Revision:

242:b235d67d25ba

Parent:

241:27eca07c9591

Child:

243:30896263bd8b

--- a/main.cpp	Tue Apr 05 09:07:14 2022 +0000
+++ b/main.cpp	Thu Apr 07 06:18:38 2022 +0000
@@ -59,10 +59,10 @@
 // CAN ///////////////////////////////////////////
 CAN can(PB_8, PB_9, 1000000);
 CANMessage msg;
-void onMsgReceived()
-{
-    CAN_RX_HANDLER();
-}
+//void onMsgReceived()
+//{
+//    CAN_RX_HANDLER();
+//}
 
 // Variables ///////////////////////////////////////////
 State pos;
@@ -263,7 +263,9 @@
     make_delay();
 
     //can.reset();
-    can.filter(msg.id, 0xFFFFF000, CANStandard);
+//    can.filter(msg.id, 0xFFFFF000, CANStandard);
+//    can.filter(0x050, 0xFFFFFFFF, CANStandard);
+    can.filter(0b100000000, 0b100000010, CANStandard);  //CAN ID 100~400번대 통과하게
 
     // TMR3 init
     Init_TMR3();
@@ -406,11 +408,13 @@
         REF_VALVE_POS_10000 = -10000.0f;
     }
     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/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
+        valve_pos_ref_dz = REF_VALVE_POS_10000;
     } 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/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
+        valve_pos_ref_dz = REF_VALVE_POS_10000;
     }
-    valve_pos_err = (float) (valve_pos_ref_dz - value);
+    valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen);
     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
     valve_pos_err_old = valve_pos_err;
     valve_pos_err_sum += valve_pos_err;
@@ -515,6 +519,7 @@
         float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
         LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
         valve_pos.sen = LVDT_LPF;
+        if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
 
     }
     TIM3->SR = 0x0;  // reset the status register
@@ -823,7 +828,8 @@
                     data_num = 0;
                 } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
                     data_num = data_num + 1;
-                    VALVE_POS_TMP = VALVE_POS_TMP + value;
+//                    VALVE_POS_TMP = VALVE_POS_TMP + value;
+                    VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
                 } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
                     Vout.ref = 0.0f;
                 } else {
@@ -1529,8 +1535,8 @@
         //////////////////////  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/CAN_FREQ) == 0) {
+
 
             // Position, Velocity, and Torque (ID:1200)
 //            if (flag_data_request[0] == LOW) {
@@ -1541,7 +1547,7 @@
 
 //                } 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));
-                    CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen));
+            CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen));
 //                }
 //            }