Sungwoo Kim
/
HydraulicControlBoard_PostLIGHT_210420
LIGHT2
Diff: main.cpp
- Revision:
- 254:c53d4c26b04a
- Parent:
- 253:3ffd78cbd029
- Child:
- 255:9f487eaa87b5
diff -r 3ffd78cbd029 -r c53d4c26b04a main.cpp --- a/main.cpp Wed Jul 20 07:50:48 2022 +0000 +++ b/main.cpp Tue Jul 26 06:58:25 2022 +0000 @@ -270,7 +270,7 @@ dac_1 = PRES_A_VREF / 3.3f; dac_2 = PRES_B_VREF / 3.3f; } - + make_delay(); for (int i=0; i<50; i++) { @@ -356,9 +356,9 @@ } } 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*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } // Valve feedforward for SW valve @@ -390,9 +390,9 @@ } } 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*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } // PWM duty vs. voltage output of L6205 in STM board @@ -541,18 +541,26 @@ LVDT_L = 0; LVDT_H = 1; - for (int ij = 0; ij<150; ij++) { - + for (int ij = 0; ij<120; ij++) { + if (ij < 20) { + continue; + } else if (ij == 20) { +// LED = 1; + } else if (ij == 100) { + LVDT_H = 0; + LVDT_L = 0; + } ADC3->CR2 |= 0x40000000; LVDT_new = ((float)ADC3->DR - 2047.0f); if(DIR_VALVE_ENC < 0) LVDT_new = 0.0f - LVDT_new; LVDT_sum = LVDT_sum + LVDT_new; } +// LED = 0; - LVDT_H = 0; - LVDT_L = 1; +// LVDT_H = 0; +// LVDT_L = 0; - LVDT_new = (LVDT_sum + 2047.0f * 150.0f) * 0.01f*2.0f; + LVDT_new = (LVDT_sum + 2047.0f * 100.0f) * 0.01f; 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; @@ -570,10 +578,11 @@ float FREQ_TMR2 = (float)FREQ_5k; float DT_TMR2 = (float)DT_5k; int cnt_trans = 0; +int cnt_jitter = 0; extern "C" void TIM2_IRQHandler(void) { if (TIM2->SR & TIM_SR_UIF ) { - + LED = 1; if(MODE_POS_FT_TRANS == 1) { if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f; @@ -797,9 +806,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*2.0f; +// int R_FET = 1.5f; +// int R_Motor = 5.4f; +// Vout.ref = Vout.ref + Vout.ref/R_Motor*R_FET*2.0f; } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -1394,13 +1403,15 @@ } else { //////////////////////////sw valve // Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW - // L6205PD -// if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f; -// else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f; -// else V_out = 0.0f; - // IRSM836-45MA - if (V_out > 0 ) V_out = V_out*1.654f + 200.0f; - else if (V_out < 0) V_out = V_out*1.654f - 200.0f; + + /////////////Jitter///////////////////// + V_out = V_out + 4000.0f*sin(2.0f*3.14159f*400.0f*((float) cnt_jitter)*0.0002f); //4000, 500Hz + cnt_jitter++; + if(cnt_jitter > 50000000) cnt_jitter = 0; + + //L6205D + if (V_out > 0 ) V_out = 800.0f + V_out*1.1275f; + else if (V_out < 0) V_out = -800.0f + V_out*1.1275f; else V_out = 0.0f; } @@ -1417,6 +1428,7 @@ } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) { V_out = -VALVE_VOLTAGE_LIMIT*1000.0f; } + PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Saturation of output voltage @@ -1470,6 +1482,7 @@ TMR2_COUNT_CAN_TX = 0; } TMR2_COUNT_CAN_TX++; + LED = 0; } TIM2->SR = 0x0; // reset the status register }