![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
2011
Diff: main.cpp
- Revision:
- 238:91d3ec3a087e
- Parent:
- 237:f69cd0553be0
- Child:
- 239:a08f301231f1
diff -r f69cd0553be0 -r 91d3ec3a087e main.cpp --- a/main.cpp Wed Jun 29 00:19:21 2022 +0000 +++ b/main.cpp Fri Jul 01 05:37:08 2022 +0000 @@ -356,24 +356,21 @@ } } Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + int R_FET = 1.5f; + int R_Motor = 5.4f; + Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET; } // Valve feedforward for SW valve void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS) { int i = 0; - float valve_pos_ref_dz = 0.0f; if(REF_VALVE_POS > VALVE_MAX_POS) { REF_VALVE_POS = VALVE_MAX_POS; } else if(REF_VALVE_POS < VALVE_MIN_POS) { REF_VALVE_POS = VALVE_MIN_POS; } - if (REF_VALVE_POS >= 0) { - valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER; - } else { - valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER; - } - valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen); + valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen); valve_pos_err_diff = valve_pos_err - valve_pos_err_old; valve_pos_err_old = valve_pos_err; valve_pos_err_sum += valve_pos_err; @@ -383,16 +380,19 @@ VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff; for(i=0; i<24; i++) { - if(valve_pos_ref_dz >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) valve_pos_ref_dz <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { + if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) { if(i==0) { - VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i]); + VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]); } else { - VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1]; + VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1]; } break; } } Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + int R_FET = 1.5f; + int R_Motor = 5.4f; + Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET; } // PWM duty vs. voltage output of L6205 in STM board @@ -544,19 +544,20 @@ for (int ij = 0; ij<150; ij++) { ADC3->CR2 |= 0x40000000; - LVDT_new = ((float)ADC3->DR); + LVDT_new = ((float)ADC3->DR - 2047.0f); + if(DIR_VALVE_ENC < 0) LVDT_new = 0.0f - LVDT_new; LVDT_sum = LVDT_sum + LVDT_new; } LVDT_H = 0; LVDT_L = 1; - LVDT_new = LVDT_sum * 0.01f*2.0f; + LVDT_new = (LVDT_sum + 2047.0f * 150.0f) * 0.01f*2.0f; float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f)); LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new; valve_pos.sen = LVDT_LPF; - if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen; + CNT_TMR4++; } @@ -796,6 +797,9 @@ Vout.ref = 3000.0f * sin(2.0f*3.14159f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { Vout.ref = 1000.0f*(ID_index_array[ID_index]); + int R_FET = 1.5f; + int R_Motor = 5.4f; + Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET; } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -898,7 +902,8 @@ } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; +// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 2048 / DZ_index; if(valve_pos_raw.ref <= VALVE_MIN_POS) { valve_pos_raw.ref = VALVE_MIN_POS; } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { @@ -920,7 +925,8 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) { +// if(DZ_index >= 128) { + if(DZ_index >= 4096) { FIRST_DZ = valve_pos_raw.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; @@ -934,7 +940,8 @@ } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; +// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 2048 / DZ_index; if(valve_pos_raw.ref <= VALVE_MIN_POS) { valve_pos_raw.ref = VALVE_MIN_POS; } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { @@ -958,7 +965,8 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index * 2; - if(DZ_index >= 128) { +// if(DZ_index >= 128) { + if(DZ_index >= 4096) { SECOND_DZ = valve_pos_raw.ref; VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; @@ -979,7 +987,8 @@ } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; +// valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; + valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 2048 / DZ_index; if(valve_pos_raw.ref <= VALVE_MIN_POS) { valve_pos_raw.ref = VALVE_MIN_POS; } else if(valve_pos_raw.ref >= VALVE_MAX_POS) { @@ -1000,7 +1009,8 @@ } VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) { +// if(DZ_index >= 128) { + if(DZ_index >= 4096) { FIRST_DZ = valve_pos_raw.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; @@ -1014,7 +1024,8 @@ } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) { START_POS = pos.sen; } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) { - valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; +// valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; + valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 2048 / DZ_index; if(valve_pos_raw.ref <= VALVE_MIN_POS) { valve_pos_raw.ref = VALVE_MIN_POS; } else if(valve_pos_raw.ref > VALVE_MAX_POS) { @@ -1038,7 +1049,8 @@ VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) { +// if(DZ_index >= 128) { + if(DZ_index >= 4096) { SECOND_DZ = valve_pos_raw.ref; VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ)); first_check = 0; @@ -1285,12 +1297,12 @@ valve_pos_pulse = -10000.0f; } } -// if (valve_pos_pulse >= 0) { -// valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; -// } else { -// valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; -// } - VALVE_POS_CONTROL_DZ(valve_pos_pulse); + if (valve_pos_pulse >= 0) { + valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; + } else { + valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; + } + VALVE_POS_CONTROL_DZ(valve_pos.ref); V_out = Vout.ref; } break; @@ -1387,6 +1399,7 @@ //////////////////////////////////////////////////////////////////// /////////////////// PWM Command /////////////////////////////////// //////////////////////////////////////////////////////////////////// + if(DIR_VALVE<0) { V_out = -V_out; } @@ -1438,7 +1451,6 @@ valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); } CAN_TX_PWM((int16_t)(valve_pos_can)); -// CAN_TX_PWM((int16_t)(valve_pos.sen)); } }