Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Rainbow_v1_2_copy1

2011

Dependencies:   mbed FastPWM

Diff: main.cpp

Revision:
233:2ec0de8590d3
Parent:
232:2976cf1b4252
Child:
234:bd00f763acb1
--- a/main.cpp	Mon May 31 07:19:49 2021 +0000
+++ b/main.cpp	Wed Jun 02 03:14:34 2021 +0000
@@ -495,14 +495,16 @@
             if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
             alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
             cnt_trans++;
-            torq.err_sum = 0;
+            torq.err_int = 0.0f;
+            force.err_int = 0.0f;
             if((float)cnt_trans * DT_TMR3 > 3.0f)
                 MODE_POS_FT_TRANS = 2;
         } else if(MODE_POS_FT_TRANS == 3) {
             if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0;
             alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
             cnt_trans++;
-            torq.err_sum = 0;
+            torq.err_int = 0.0f;
+            force.err_int = 0.0f;
             if((float) cnt_trans * DT_TMR3 > 3.0f )
                 MODE_POS_FT_TRANS = 0;
         } else if(MODE_POS_FT_TRANS == 2) {
@@ -799,12 +801,14 @@
                         float I_rem = I_REF - I_MAX;
                         I_REF = I_MAX;
                         float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]  
-                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
+//                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
+                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
                     } else if (I_REF < -I_MAX) {
                         double I_rem = I_REF - (-I_MAX);
                         I_REF = -I_MAX;
                         float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_d*I_rem) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]  
-                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!
+/*                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE)*(float)TMR_FREQ_5k; // Need to Check!*/
+                        torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
                     }
                 }
                 break;