Sungwoo Kim
for learning
Diff: main.cpp
- Revision:
- 138:a843f32ced33
- Parent:
- 137:ccf70b9b1705
- Child:
- 139:15621998925b
--- 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;