Diff: main.cpp

Revision:

260:bbb74caca589

Parent:

259:5da820cc9a1a

Child:

261:10f0e04fdb0b

Child:

262:a9507d8a4674

--- 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