Diff: main.cpp

Revision:

39:e961db3b93f8

Parent:

38:118df027d851

--- a/main.cpp	Thu Nov 28 09:18:03 2019 +0000
+++ b/main.cpp	Thu Dec 05 14:29:22 2019 +0000
@@ -143,6 +143,9 @@
     MODE_DDV_POS_VS_FLOWRATE,                           //32
 };
 
+int messi = 0;
+float end_pos = 0.0f;
+
 int main()
 {
     /*********************************
@@ -152,11 +155,11 @@
     //pc.baud(9600);
 
     // i2c init
-    i2c.frequency(400 * 1000);          // 0.4 mHz
-    wait_ms(2);                         // Power Up wait
-    look_for_hardware_i2c();            // Hardware present
-    init_as5510(i2c_slave_addr1);
-    make_delay();
+    //i2c.frequency(400 * 1000);          // 0.4 mHz
+//    wait_ms(2);                         // Power Up wait
+//    look_for_hardware_i2c();            // Hardware present
+//    init_as5510(i2c_slave_addr1);
+//    make_delay();
 
 //    // spi init
     //eeprom.format(8,3);
@@ -169,9 +172,9 @@
     ROM_CALL_DATA();
     make_delay();
 
-    // ADC init
-    Init_ADC();
-    make_delay();
+//    // ADC init
+//    Init_ADC();
+//    make_delay();
 
     // Pwm init
     Init_PWM();
@@ -206,32 +209,27 @@
     make_delay();
 
     //DAC init
-    dac_1 = PRES_A_VREF / 3.3f;
-    dac_2 = PRES_B_VREF / 3.3f;
-    make_delay();
+//    dac_1 = PRES_A_VREF / 3.3f;
+//    dac_2 = PRES_B_VREF / 3.3f;
+//    make_delay();
 
-    for (int i=0; i<50; i++) {
-        if(i%2==0)
-            ID_index_array[i] = - i * 0.5f;
-        else
-            ID_index_array[i] =  (i+1) * 0.5f;
-    }
+//    for (int i=0; i<50; i++) {
+//        if(i%2==0)
+//            ID_index_array[i] = - i * 0.5f;
+//        else
+//            ID_index_array[i] =  (i+1) * 0.5f;
+//    }
+    
+    ENC_UPDATE();
+    pos.ref = pos.sen;
+    
 
     /************************************
     ***     Program is operating!
     *************************************/
     while(1) {
-        if(timer_while==1000) {
-            //pc.printf("cmd %d\n ");
-            //i2c
-            read_field(i2c_slave_addr1);
-            if(DIR_VALVE_ENC < 0) value = 1023 - value;
-
-//            if(LED==1) {
-//                LED=0;
-//            } else
-//                LED = 1;
-            timer_while = 0;
+        if(timer_while==100000) {
+            messi = 1;
 
         }
         timer_while ++;
@@ -452,12 +450,51 @@
 {
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
+        
+        if(messi == 1){
+            pos.ref = pos.ref + 0.007f;//0.009 -> 10ml/30min
+            pos.err = pos.ref - (float) pos.sen;
+            pos.err_diff = pos.err - pos.err_old;
+            pos.err_old = pos.err;
+            pos.err_sum += pos.err;
+            if (pos.err_sum > 1000) pos.err_sum = 1000;
+            if (pos.err_sum<-1000) pos.err_sum = -1000;
+            V_out = (float) 500.0f + (float) 3000.0f * 0.01f * pos.err + (float) 0.0f * 0.01f * pos.err_sum + (float) 0.0f * pos.err_diff;
+            
+            if (abs(pos.ref-pos.sen) > 1000){
+                V_out = 0;
+                pos.ref = pos.sen;
+                end_pos = pos.sen;
+                messi = 2;
+            } 
+        }
+        if(messi ==2){
+            pos.ref = pos.ref - 2.0f;
+            pos.err = pos.ref - (float) pos.sen;
+            pos.err_diff = pos.err - pos.err_old;
+            pos.err_old = pos.err;
+            pos.err_sum += pos.err;
+            if (pos.err_sum > 1000) pos.err_sum = 1000;
+            if (pos.err_sum<-1000) pos.err_sum = -1000;
+            V_out = (float) 3000.0f * 0.01f * pos.err + (float) 0.0f * 0.01f * pos.err_sum + (float) 0.0f * pos.err_diff;
+            
+           // if (abs(pos.ref-pos.sen) > 2000){
+//                V_out = 0;
+//                messi = 0;
+//            } 
+            
+            if (abs(end_pos-pos.sen) > 81920){
+                V_out = 0;
+                messi = 0;
+            } 
+        }
+        
 
         // CONTROL LOOP ------------------------------------------------------------
 
         switch (CONTROL_MODE) {
             case MODE_NO_ACT: {
-                V_out = 0.0f;
+                V_out = V_out;
                 break;
             }