Kim GiJeong
/
HydraulicControlBoard_LIGHT_GJ
eeprom_test
Diff: main.cpp
- Revision:
- 63:a3c7f31742c9
- Parent:
- 62:851cf7b7aa7a
- Child:
- 64:dc5b5c258579
--- a/main.cpp Tue May 12 06:23:22 2020 +0000 +++ b/main.cpp Fri May 15 03:25:15 2020 +0000 @@ -76,6 +76,8 @@ extern int CID_TX_VOUT; extern int CID_TX_VALVE_POSITION; +float I_REF_fil_DZ = 0.0f; + // ============================================================================= // ============================================================================= // ============================================================================= @@ -1300,7 +1302,8 @@ case MODE_JOINT_CONTROL: { 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] + vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] +// vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] //K & D Low Pass Filter @@ -1311,7 +1314,7 @@ 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] + torq.err = torq_ref - torq.sen; //[N] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N] if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -1423,14 +1426,9 @@ //////////////////////////////////////////////////////////////////////////// //////////////////////////// CURRENT CONTROL ////////////////////////////// //////////////////////////////////////////////////////////////////////////// + if (CURRENT_CONTROL_MODE) { - double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz - I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; - - I_ERR = I_REF_fil - cur.sen; - I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; - - + // Moog Valve Current Control Gain double R_model = 500.0f; // ohm double L_model = 1.2f; @@ -1446,15 +1444,34 @@ KP_I = 1.0f * L_model*w0; KI_I = 0.08f * R_model*w0; } + + double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz + I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; + + FLAG_VALVE_DEADZONE = 1; + + if (FLAG_VALVE_DEADZONE) { + if (I_REF_fil > 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_PLUS / 1000.0f; // unit: mA + else if (I_REF_fil < 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_MINUS / 1000.0f; // unit: mA + else I_REF_fil_DZ = I_REF_fil + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f / 1000.0f; // unit: mA + } else { + I_REF_fil_DZ = I_REF_fil; + } + + I_ERR = I_REF_fil_DZ - cur.sen; + I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; + + + double FF_gain = 1.0f; VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV - I_REF_fil_diff = I_REF_fil - I_REF_fil_old; - I_REF_fil_old = I_REF_fil; -// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV - VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV + I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; + I_REF_fil_old = I_REF_fil_DZ; +// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV + VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV double Ka = 3.0f / KP_I; @@ -1479,15 +1496,25 @@ ///////////////// Dead Zone Cancellation & Linearization ////////////////// //////////////////////////////////////////////////////////////////////////// // Dead Zone Cancellation (Mechanical Valve dead-zone) - if (FLAG_VALVE_DEADZONE) { - if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV - else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV +// if (FLAG_VALVE_DEADZONE) { +// if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV +// else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV +// VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV +// } else { +// VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; +// } - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV - - } else { + // VALVE_DEADZONE_PLUS : Current[mA] to start extraction + // VALVE_DEADZONE_MINUS : Current[mA] to start contraction +// double R_model = 500.0f; +// FLAG_VALVE_DEADZONE = true; +// if (FLAG_VALVE_DEADZONE) { +// if (VALVE_PWM_RAW > 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * R_model / 1000.0f; // unit: mV +// else if (VALVE_PWM_RAW < 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * R_model / 1000.0f; // unit: mV +// else VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f* R_model / 1000.0f; +// } else { VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; - } +// } // Output Voltage Linearization double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV @@ -1546,7 +1573,7 @@ } } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen)); + CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); } else if (SENSING_MODE == 1) { CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); } @@ -1567,8 +1594,8 @@ } else { t_value = V_out; } - 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_fil_DZ * 1000.0f)); //1300 + CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300 }