Diff: main.cpp

Revision:

236:c16774af92ed

Parent:

235:c831718303c9

Child:

237:5da820cc9a1a

--- a/main.cpp	Mon Jun 14 10:36:29 2021 +0000
+++ b/main.cpp	Wed Jul 07 10:09:11 2021 +0000
@@ -488,6 +488,7 @@
 int cnt_trans = 0;
 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
 int can_rest =0;
+float force_ref_act_can = 0.0f;
 
 extern "C" void TIM3_IRQHandler(void)
 {
@@ -768,6 +769,7 @@
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
                 } else {
                     float force_ref_act = force.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : N
+                    force_ref_act_can = force_ref_act;
                     force.err = force_ref_act - force.sen;
                     force.err_int += force.err/((float)TMR_FREQ_5k);
                     temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
@@ -1039,7 +1041,7 @@
 
             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
             if (flag_data_request[2] == HIGH) {
-                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (PRES_SUPPLY_NOM));
+                CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (PRES_SUPPLY), (int16_t) (force_ref_act_can*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
             }
 
             TMR2_COUNT_CAN_TX = 0;