Sungwoo Kim
asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 63:376a20d2fbfd
- Parent:
- 62:456f87e3124e
- Child:
- 64:368bcd2a230a
diff -r 456f87e3124e -r 376a20d2fbfd main.cpp
--- a/main.cpp Tue Apr 28 09:19:26 2020 +0000
+++ b/main.cpp Thu May 07 03:52:28 2020 +0000
@@ -139,11 +139,11 @@
*** Initialization
*********************************/
HAL_Init();
- SystemClock_Config();
+// SystemClock_Config();
- LED = 1;
- pc.baud(9600);
+// LED = 1;
+// pc.baud(9600);
// i2c init
i2c.frequency(400 * 1000); // 0.4 mHz
@@ -221,7 +221,7 @@
*************************************/
while(1) {
if(timer_while==1000 && OPERATING_MODE==5) {
- //if(timer_while==1000) {
+// if(timer_while==1000) {
//i2c
read_field(i2c_slave_addr1);
if(DIR_VALVE_ENC < 0) value = 1023 - value;
@@ -614,7 +614,7 @@
vel.ref = 0.0f;
FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
- int cnt_check_enc = (TMR_FREQ_5k/500);
+ int cnt_check_enc = (TMR_FREQ_5k/20);
if(cnt_findhome%cnt_check_enc == 0) {
FINDHOME_POSITION = pos.sen;
FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
@@ -630,8 +630,8 @@
if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
//REFERENCE_MODE = MODE_REF_NO_ACT;
- if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 8.0f;
- else pos.ref = pos.ref - 8.0f;
+ if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
+ else pos.ref = pos.ref - 12.0f;
// pos.err = pos.ref_home_pos - pos.sen;
// float VALVE_POS_RAW_POS_FB = 0.0f;
@@ -645,6 +645,7 @@
} else {
ENC_SET(HOMEPOS_OFFSET);
+// ENC_SET_ZERO();
INIT_REF_POS = HOMEPOS_OFFSET;
REF_POSITION = 0;
REF_VELOCITY = 0;
@@ -653,17 +654,83 @@
FINDHOME_VELOCITY = 0;
cnt_findhome = 0;
cnt_vel_findhome = 0;
- FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+// FINDHOME_STAGE = FINDHOME_ZEROPOSE;
+
+
+ cnt_findhome = 0;
+ pos.ref = 0.0f;
+ vel.ref = 0.0f;
+ pos.ref_home_pos = 0.0f;
+ vel.ref_home_pos = 0.0f;
+ FINDHOME_STAGE = FINDHOME_INIT;
+ CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
+
+
}
} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
int T_move = 2*TMR_FREQ_5k;
pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
+ //pos.ref = 0.0f;
vel.ref = 0.0f;
-
+
// input for position control
+
+// CONTROL_MODE = MODE_JOINT_CONTROL;
+ alpha_trans = 0.0f;
+
+
+
+
- CONTROL_MODE = MODE_JOINT_CONTROL;
- alpha_trans = 0.0f;
+ 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]
+ pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
+
+ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
+
+ double I_REF_POS = 0.0f;
+ double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
+ double I_REF_VC = 0.0f; // I_REF for velocity compensation
+
+ double temp_vel_pos = 0.0f;
+ double temp_vel_torq = 0.0f;
+ double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
+
+ if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
+ temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
+ // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
+ } else if ((OPERATING_MODE && 0x01) == 1) {
+ temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
+ // L when P-gain = 100, f_cut = 10Hz L feedforward velocity
+ }
+ if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
+ else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
+
+ I_REF = I_REF_POS;
+
+
+
+ } else {
+ float VALVE_POS_RAW_FORCE_FB = 0.0f;
+
+ VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
+
+ if (VALVE_POS_RAW_FORCE_FB >= 0) {
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
+ } else {
+ valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
+ }
+
+ VALVE_POS_CONTROL(valve_pos.ref);
+
+ V_out = (float) Vout.ref;
+
+ }
+
+
+
+
// pos.err = pos.ref - (float)pos.sen;
// float VALVE_POS_RAW_POS_FB = 0.0f;
// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
@@ -1240,7 +1307,13 @@
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_ref = torq.ref + (K_SPRING * pos.err * 0.01f - D_DAMPER * vel.sen * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
+
+ //K & D Low Pass Filter
+ float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz
+ 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]
// torque feedback
torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE) - torq.sen; //[N]
@@ -1285,6 +1358,23 @@
// Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm;
I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB);
+
+ // Anti-windup for FT
+ if (I_GAIN_JOINT_TORQUE != 0) {
+ double I_MAX = 10.0f; // Maximum Current : 10mV
+ double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f);
+ if (I_REF > I_MAX) {
+ double I_rem = I_REF - I_MAX;
+ I_rem = Ka*I_rem;
+ I_REF = I_MAX;
+ torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
+ } else if (I_REF < -I_MAX) {
+ double I_rem = I_REF - (-I_MAX);
+ I_rem = Ka*I_rem;
+ I_REF = -I_MAX;
+ torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
+ }
+ }
} else {
float VALVE_POS_RAW_FORCE_FB = 0.0f;
@@ -1470,11 +1560,11 @@
if (flag_data_request[1] == HIGH) {
//valve position
double t_value = 0;
- if(valve_pos.ref>=(float) VALVE_CENTER) {
- t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
- } else {
- t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
- }
+// if(valve_pos.ref>=(float) VALVE_CENTER) {
+// t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+// } else {
+// t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+// }
if(OPERATING_MODE==5) {
t_value = (double) value;
} else if(CURRENT_CONTROL_MODE==1) {
@@ -1482,8 +1572,9 @@
} else {
t_value = V_out;
}
- //CAN_TX_TORQUE((int16_t) (value), (int16_t) (V_out)); //1300
- CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
+ CAN_TX_TORQUE((int16_t) (t_value)); //1300
+ //CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
+ //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300
}
@@ -1499,7 +1590,7 @@
if (flag_data_request[3] == HIGH) {
//PWM
- CAN_TX_PWM((int16_t) cur.sen); //1500
+ CAN_TX_PWM((int16_t) value); //1500
}
//for (i = 0; i < 10000; i++) {
// ;