Sungwoo Kim
distribution-201229
Diff: main.cpp
- Revision:
- 139:15621998925b
- Parent:
- 138:a843f32ced33
- Child:
- 140:cc5bc7fb0a16
--- 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++) {
// ;