![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 5:a4319f79457b
- Parent:
- 2:a1c0a37df760
- Child:
- 6:df07d3491e3a
--- a/main.cpp Tue Aug 20 07:57:10 2019 +0000 +++ b/main.cpp Tue Aug 20 10:12:00 2019 +0000 @@ -11,7 +11,7 @@ DigitalOut check_2(PC_3); AnalogOut dac_1(PA_4); AnalogOut dac_2(PA_5); -AnalogIn adc3(PC_1); +//AnalogIn adc3(PC_1); // pwm double dtc_v=0.0; @@ -37,11 +37,30 @@ CANMessage msg; // Variables +double cur = 0.0; +double cur_ref = 0.0; +double cur_ref_old = 0.0; +double cur_ref_diff = 0.0; +double cur_err = 0.0; +double cur_err_int = 0.0; +double cur_err_old = 0.0; +double cur_err_diff = 0.0; +double pos = 0.0; +double pos_ref = 0.0; -double a1; -double PWM_out; -double temp_cur=0.0; +double vel; +double vel_ref; + +double pres_A; +double pres_B; + +double V_out=0.0; +double V_rem=0.0; // for anti-windup +double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV + +double PWM_out=0.0; + int main() { /******************************************************* @@ -69,7 +88,7 @@ enc.frequency(5000000); //5M // ADC init -// Init_ADC(); + Init_ADC(); // Pwm init Init_PWM(); @@ -119,6 +138,7 @@ unsigned long CNT_TMR4 = 0; double FREQ_TMR4 = (double)FREQ_20k; +double DT_TMR4 = (double)DT_20k; extern "C" void TIM4_IRQHandler(void) { if ( TIM4->SR & TIM_SR_UIF ) { @@ -137,16 +157,15 @@ //// read_field(i2c_slave_addr1); //ADC - // ADC1->CR2 |= 0x40000000; // adc _ 12bit + ADC3->CR2 |= 0x40000000; // adc _ 12bit // a1=ADC1->DR; // a1=ADC2->DR; - // int raw_cur = ADC3->DR; - double raw_cur=(double)adc3*4096.0; - - double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*100.0)); // f_cutoff : 500Hz - double new_a1 = (double)raw_cur; - a1=a1*(1.0-alpha_update_cur)+new_a1*(alpha_update_cur); -// a1=new_a1; +// int raw_cur = ADC3->DR; + while((ADC3->SR & 0b10)); + + double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz + double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA + cur=cur*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur); } //DAC @@ -154,21 +173,56 @@ // dac_2 = ADC2->DR; /******************************************************* - *** Valve Control + *** Valve Current Control ********************************************************/ - // PWM_out : -1.0~1.0(full duty) - PWM_out = -0.0104; - double t = (double)CNT_TMR4/FREQ_TMR4; - double T = 10.0; -// PWM_out = 0.36*sin(2.0*PI*t/T); - PWM_out = 0.04*sin(2.0*PI*t/T); -// if(PWM_out > 0.0) PWM_out = 0.02; -// else if(PWM_out < 0.0) PWM_out = -0.02; + bool FLAG_current_control = false; + if(FLAG_current_control) { + cur_err = cur_ref - cur; + cur_err_int = cur_err_int + cur_err*DT_TMR4; + cur_err_diff = (cur_err - cur_err_old)*FREQ_TMR4; + cur_err_old = cur_err; + + double R_model = 150.0; // ohm + double L_model = 0.3; + double w0 = 2.0*3.14*90.0; + double KP_I = L_model*w0; + double KI_I = R_model*w0; + double KD_I = 0.0; + + double FF_gain = 0.0; + V_out = (int) (KP_I * cur_err + KI_I * cur_err_int + KD_I * cur_err_diff); +// V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV + V_out = V_out + FF_gain * (R_model*cur_ref + L_model*cur_ref_diff); // Unit : mV + + double Ka = 5.0/KP_I; + if(V_out > V_MAX) { + V_rem = V_out-V_MAX; + V_rem = Ka*V_rem; + V_out = V_MAX; + cur_err_int = cur_err_int - V_rem*DT_5k; + } else if(V_out < -V_MAX) { + V_rem = V_out-(-V_MAX); + V_rem = Ka*V_rem; + V_out = -V_MAX; + cur_err_int = cur_err_int - V_rem*DT_5k; + } + + } else { + // PWM_RAW : -5000.0mV~5000.0mV(full duty) + double t = (double)CNT_TMR4*DT_TMR4; + double T = 5.0; + + V_out = 1000.0*sin(2.0*PI*t/T); // Unit : mV +// if(V_out > 0.0) V_out = 1000.0; +// else if(V_out < 0.0) V_out = -1000.0; + } + + PWM_out= V_out/12000.0; // Full duty : 12000.0mV // Saturation of output voltage to 5.0V - if(PWM_out > (5.0/12.0)) PWM_out=(5.0/12.0); - else if (PWM_out < (-5.0/12.0)) PWM_out=(-5.0/12.0); + if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667 + else if (PWM_out < -0.41667) PWM_out=-0.41667; if (PWM_out>0.0) { dtc_v=0.0; @@ -188,10 +242,10 @@ if((CNT_TMR4%40)==0){ msg.id = 50; msg.len = 4; - int temp_CUR = (int)(a1); + int temp_CUR = (int)(cur*1000.0); msg.data[0]=0x00FF&temp_CUR; msg.data[1]=0x00FF&(temp_CUR>>8); - int temp_PWM = (int)(PWM_out*10000.0); + int temp_PWM = (int)(V_out); msg.data[2]=0x00FF&temp_PWM; msg.data[3]=0x00FF&(temp_PWM>>8); can.write(msg);