Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_210420
LIGHT2
Diff: main.cpp
- Revision:
- 260:bbb74caca589
- Parent:
- 259:5da820cc9a1a
- Child:
- 261:10f0e04fdb0b
- Child:
- 262:a9507d8a4674
diff -r 5da820cc9a1a -r bbb74caca589 main.cpp --- a/main.cpp Fri Jul 09 02:07:56 2021 +0000 +++ b/main.cpp Sat Jul 10 04:39:20 2021 +0000 @@ -467,10 +467,10 @@ pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f); // unit : Nm - } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N force.UpdateSen(force_new,FREQ_TMR4,1000.0f); // unit : N } @@ -749,7 +749,7 @@ vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s // position control command =============================================================================================================================================== - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s // L when P-gain = 100, f_cut = 10Hz } else { @@ -762,13 +762,13 @@ K_LPF = (1.0f-alpha_SpringDamper) * K_LPF + alpha_SpringDamper * K_SPRING; D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + alpha_SpringDamper * D_DAMPER; - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode - float torq_ref_act = torq.ref + K_SPRING * pos.err + D_DAMPER * vel.err; // unit : Nm + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode + float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm torq.err = torq_ref_act - torq.sen; torq.err_int += torq.err/((float)TMR_FREQ_5k); 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 + float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * 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); @@ -777,7 +777,7 @@ // velocity feedforward command ======================================================================================================================================== - if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s } else { temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s