![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 63:376a20d2fbfd
- Parent:
- 62:456f87e3124e
- Child:
- 64:368bcd2a230a
--- 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++) { // ;