Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
rainbow
Diff: main.cpp
- Revision:
- 139:15621998925b
- Parent:
- 138:a843f32ced33
- Child:
- 140:cc5bc7fb0a16
diff -r a843f32ced33 -r 15621998925b main.cpp --- a/main.cpp Wed Oct 07 01:07:09 2020 +0000 +++ b/main.cpp Thu Oct 08 00:29:29 2020 +0000 @@ -1,4 +1,4 @@ -//201007-1 +//201008-1 #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -1571,7 +1571,8 @@ K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D); D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D); - torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] +// torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N] + torq_ref = torq.ref; // torque feedback torq.err = torq_ref - torq.sen; //[N] @@ -1654,7 +1655,7 @@ } if(I_GAIN_JOINT_TORQUE != 0) { - double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; + double Ka = 2.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f; if(valve_pos.ref>VALVE_MAX_POS) { double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; valve_pos_rem = valve_pos_rem * Ka; @@ -1689,15 +1690,15 @@ case MODE_JOINT_ADAPTIVE_BACKSTEPPING: { -// float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 -// float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 + float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 + float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 - float Va = (1256.6f + Amm * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 - float Vb = (1256.6f + Amm * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 +// float Va = (1256.6f + Amm * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3 +// float Vb = (1256.6f + Amm * 39.5f) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3 V = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f - float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 137076 + float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076 float g3_prime = 0.0f; if (torq.sen > Amm*(Ps-Pt)*0.000001f){ @@ -1707,13 +1708,14 @@ } else { if ((value-VALVE_CENTER) > 0) { g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); +// g3_prime = sqrt(Ps-Pt); } else { g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); +// g3_prime = sqrt(Ps-Pt); } } float tau = 0.01f; - float K_valve = 0.00004f; -// float K_valve = 0.4f; + float K_valve = 0.04f; float x_v = 0.0f; //x_v : -1~1 if(value>=VALVE_CENTER) { @@ -1724,7 +1726,7 @@ float f4 = -x_v/tau; float g4 = K_valve/tau; - float torq_ref_dot = torq.ref_diff * (float) TMR_FREQ_5k; + float torq_ref_dot = torq.ref_diff * 500.0f; pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] @@ -1733,20 +1735,29 @@ torq.err = torq.ref - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] - float k3 = 10.0f; - float k4 = 10000.0f; + float k3 = 3000.0f; + float k4 = 1000.0f; float rho3 = 1.0f; - float rho4 = 0.0001f; - float x_4_des = (-f3 + torq_ref_dot - k3*torq.err)/(gamma_hat*g3_prime); + float rho4 = 50000.0f; +// float rho4 = 100000.0f; + float x_4_des = (-f3 + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime); if (x_4_des > 1) x_4_des = 1; else if (x_4_des < -1) x_4_des = -1; + + if (x_4_des > 0) { + valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER; + } else { + valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER; + } + + float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k; x_4_des_old = x_4_des; - V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*torq.err)/g4; + V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4; - float rho_gamma = 1.0f; - float gamma_hat_dot = rho3*torq.err/rho_gamma*((-f3+torq_ref_dot-k3*torq.err)/gamma_hat + g3_prime*(x_v-x_4_des)); + float rho_gamma = 100.0f; + float gamma_hat_dot = rho3*(-torq.err)/rho_gamma*((-f3+torq_ref_dot-k3*(-torq.err))/gamma_hat + g3_prime*(x_v-x_4_des)); gamma_hat = gamma_hat + gamma_hat_dot / (float) TMR_FREQ_5k; break; } @@ -1938,7 +1949,7 @@ } else { t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); } - CAN_TX_PRES((int16_t) (t_value), (int16_t) (V_out)); // 1400 + CAN_TX_PRES((int16_t) (t_value), (int16_t) (torq.ref)); // 1400 } //If it doesn't rest, below can can not work. @@ -1946,10 +1957,10 @@ ; } - if (flag_data_request[3] == HIGH) { + if (flag_data_request[3] == LOW) { //PWM //CAN_TX_PWM((int16_t) value); //1500 - CAN_TX_PWM((int16_t) input_NN[0] * 100.0f); //1500 + CAN_TX_PWM((int16_t) gamma_hat); //1500 } //for (i = 0; i < 10000; i++) { // ;