Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_
rainbow
Diff: main.cpp
- Revision:
- 138:a843f32ced33
- Parent:
- 137:ccf70b9b1705
- Child:
- 139:15621998925b
diff -r ccf70b9b1705 -r a843f32ced33 main.cpp --- a/main.cpp Tue Oct 06 12:06:45 2020 +0000 +++ b/main.cpp Wed Oct 07 01:07:09 2020 +0000 @@ -1,4 +1,4 @@ -//200927-1 +//201007-1 #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -109,7 +109,7 @@ MODE_JOINT_CONTROL, //2 MODE_VALVE_OPEN_LOOP, //3 - MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4 + MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4 MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5 MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6 @@ -1560,68 +1560,7 @@ case MODE_JOINT_CONTROL: { -// 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 - 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 g3_prime = 0.0f; - if (torq.sen > Amm*(Ps-Pt)*0.000001f){ - g3_prime = 1.0f; - } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { - g3_prime = -1.0f; - } else { - if ((value-VALVE_CENTER) > 0) { - g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); - } else { - g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); - } - } - float tau = 0.01f; - float K_valve = 0.00004f; -// float K_valve = 0.4f; - - float x_v = 0.0f; //x_v : -1~1 - if(value>=VALVE_CENTER) { - x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - } else { - x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - } - float f4 = -x_v/tau; - float g4 = K_valve/tau; - - float torq_ref_dot = torq.ref_diff * (float) TMR_FREQ_5k; - - 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] - pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] - - 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 rho3 = 1.0f; - float rho4 = 0.0001f; - 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; - 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; - - 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)); - gamma_hat = gamma_hat + gamma_hat_dot / (float) TMR_FREQ_5k; - - - /* + double torq_ref = 0.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] @@ -1739,10 +1678,6 @@ torq_ref_past = torq_ref; - */ - - - break; } @@ -1750,6 +1685,71 @@ V_out = (float) Vout.ref; break; } + + 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 * 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 g3_prime = 0.0f; + if (torq.sen > Amm*(Ps-Pt)*0.000001f){ + g3_prime = 1.0f; + } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) { + g3_prime = -1.0f; + } else { + if ((value-VALVE_CENTER) > 0) { + g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f); + } else { + g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f); + } + } + float tau = 0.01f; + float K_valve = 0.00004f; +// float K_valve = 0.4f; + + float x_v = 0.0f; //x_v : -1~1 + if(value>=VALVE_CENTER) { + x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + } else { + x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + } + float f4 = -x_v/tau; + float g4 = K_valve/tau; + + float torq_ref_dot = torq.ref_diff * (float) TMR_FREQ_5k; + + 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] + pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] + + 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 rho3 = 1.0f; + float rho4 = 0.0001f; + 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; + 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; + + 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)); + gamma_hat = gamma_hat + gamma_hat_dot / (float) TMR_FREQ_5k; + break; + } default: break;