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);