Diff: main.cpp

Revision:

7:b881681f7ff6

Parent:

6:e1e317fe8f7f

--- a/main.cpp	Thu Jul 14 10:49:40 2016 +0000
+++ b/main.cpp	Fri Jul 22 09:35:59 2016 +0000
@@ -40,44 +40,60 @@
 Ticker tikref;
 //Ticker cameraSI;
 
-float u_R_a = 0, u_R_b = 0, x=0, Ubat = 0, R_au = 0.9, L_au = 218e-6, K = 2.1856e-3, u_i_a2 = 0, w = 0, N = 7.46, n = 0, delay = 0.13;
-double u_i_a = 0, u_i_b = 0, e = 0;
+float u_Ri_a = 0, u_Ri_b = 0, n_ref=0, Ubat = 0, R_au = 1.067, L_au = 218e-6, K = 2.54e-3, pot2_skalirani = 0;
+float u_i_a2 = 0, u_i_b2 = 0, w_a = 0, w_b = 0, N = 7.46, n_a = 0, n_b = 0, delay = 0.13, u_Rw_a = 0, u_Rw_b =0;
+double u_i_a = 0, u_i_b = 0, e_a = 0, e_b = 0;
 double T_d = 1e-3;
 
 PI PI_a (0.04, 0.001, T_d);
 PI PI_b (0.04, 0.001, T_d);
 
-float polje[]={0.3,0.4};
-int index=0;
+PI RegulatorBrzine_a( 0.01, 0.01, T_d);
+PI RegulatorBrzine_b( 0.01, 0.01, T_d);
 
-void changeRef(){
-    index = ++index%2;
-    x = polje[index];
-    }
+Timer timer;
+
+
 void tick(){
-    if ((u_R_a - delay) <= 0) u_i_a = 0;
-    else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_R_a - delay);
-    if ((u_R_b - delay) <= 0) u_i_b = 0;
-    else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_R_b - delay);
-    x = 3 * pot2;
-    //x = 0.4;
-    PI_a.in(x - u_i_a);
-    PI_b.in(x - u_i_b);
-    u_R_a = PI_a.out();
-    u_R_b = PI_b.out();
+    timer.start();
+    Ubat = naponBaterije * 3.3 * (57.0/10.0);
+    timer.stop();
+    test_w.pulsewidth_us(timer.read_us());
+    timer.reset();    
+    if ((u_Ri_a - delay) <= 0) u_i_a = 0;
+    else u_i_a = 375.0/220.0*(3.3 * a_feedback)/(u_Ri_a - delay);
+    if ((u_Ri_b - delay) <= 0) u_i_b = 0;
+    else u_i_b = 375.0/220.0*(3.3 * b_feedback)/(u_Ri_b - delay);
+    //pot2_skalirani = 100 + 1900* pot2;
+    //x = pot2_skalirani;
+    //x = 3000 * pot2;
+    n_ref = 1000;
+    
+                
+    e_a = Ubat*(u_Ri_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d);
+    w_a = e_a / K;
+    n_a = w_a * (30/3.14159) / N;
+    u_i_a2 = u_i_a;
+    e_b = Ubat*(u_Ri_b - delay) - R_au*u_i_b - L_au * ((u_i_b - u_i_b2)/T_d);
+    w_b = e_b / K;
+    n_b = w_b * (30/3.14159) / N;
+    u_i_b2 = u_i_b;
+    
+    RegulatorBrzine_a.in(n_ref - n_a);
+    RegulatorBrzine_b.in(n_ref - n_b);
+    
+    u_Rw_a = RegulatorBrzine_a.out();
+    u_Rw_b = RegulatorBrzine_b.out();
+            
+    PI_a.in(u_Rw_a - u_i_a);
+    PI_b.in(u_Rw_b - u_i_b);
+    u_Ri_a = PI_a.out();
+    u_Ri_b = PI_b.out();
+      
+    motorA.pulsewidth(100e-6*u_Ri_a);
+    motorB.pulsewidth(100e-6*u_Ri_b);
 
-    motorA.pulsewidth(100e-6*u_R_a);
-    //motorB.pulsewidth(100e-6*u_R_b);
-    
-    Ubat = naponBaterije * 3.3 * (57.0/10.0); 
-    if(u_i_a < 0.1 + u_i_a2 && u_i_a > u_i_a2 - 0.1){              
-    e = Ubat*(u_R_a - delay) - R_au*u_i_a - L_au * ((u_i_a - u_i_a2)/T_d);
-    w = e / K;
-    n = w * (30/3.14159) / N;
-    u_i_a2 = u_i_a;
-    
-    test_w.pulsewidth_us(n);
-        }
+        
     }
 /**    
 void camSI(){
@@ -104,7 +120,10 @@
     
     PI_a.setOutputLimits (0, 1);
     PI_b.setOutputLimits (0, 1);
-
+    
+    RegulatorBrzine_a.setOutputLimits (0,3);
+    RegulatorBrzine_b.setOutputLimits(0,3);
+    
     motorA.period_us(100);
     motorB.period_us(100);
     test_w.period_us(10000);
@@ -118,7 +137,8 @@
 //    cameraSI.attach(&camSI, 0.00258);  
 float analogprint, analogprintb;
     while(1){
-        if(sw1 == 1)enable = 0;
+        if(sw1 == 1) enable = 0;
+        if(sw2 == 1) enable = 1;
         if(fault == 1) {
             d1=1;
             d3=0;