Sungwoo Kim
/
HydraulicControlBoard_LIGHT
For LIGHT
Diff: main.cpp
- Revision:
- 65:443a5f0e8ee1
- Parent:
- 64:dc5b5c258579
- Child:
- 66:d385a08132d7
diff -r dc5b5c258579 -r 443a5f0e8ee1 main.cpp --- a/main.cpp Mon May 18 00:36:59 2020 +0000 +++ b/main.cpp Tue May 19 01:36:41 2020 +0000 @@ -636,18 +636,67 @@ if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f; else pos.ref = pos.ref - 12.0f; + + + + 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_home_pos - pos.sen; // float VALVE_POS_RAW_POS_FB = 0.0f; // VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f; // valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER; // VALVE_POS_CONTROL(valve_pos.ref); - CONTROL_MODE = MODE_JOINT_CONTROL; + cnt_findhome_wait = 0; +// CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; } else { ENC_SET(HOMEPOS_OFFSET); +// for(int i_hp=0; i_hp<100; i_hp++){ +// ENC_SET(HOMEPOS_OFFSET); +// } // ENC_SET_ZERO(); INIT_REF_POS = HOMEPOS_OFFSET; REF_POSITION = 0; @@ -655,16 +704,19 @@ FINDHOME_POSITION = 0; FINDHOME_POSITION_OLD = 0; FINDHOME_VELOCITY = 0; - cnt_findhome = 0; - cnt_vel_findhome = 0; - FINDHOME_STAGE = FINDHOME_ZEROPOSE; + + if(cnt_findhome_wait > 10) { + cnt_findhome = 0; + cnt_vel_findhome = 0; + 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; + + cnt_findhome_wait = cnt_findhome_wait + 1; //FINDHOME_STAGE = FINDHOME_INIT; //CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;