Diff: main.cpp

Revision:

1:3b55e7add58b

Parent:

0:8ffbb19fee71

diff -r 8ffbb19fee71 -r 3b55e7add58b main.cpp
--- a/main.cpp	Tue Sep 25 07:31:28 2018 +0000
+++ b/main.cpp	Tue Sep 25 09:58:15 2018 +0000
@@ -24,13 +24,21 @@
 double  alpha = 0.5;
 double  RFvelo, LFvelo, prv_RFvelo, prv_LFvelo;
 double  err, errL, errF, now_time, samp_time;
-double  prv_time, prv_err, prv_errF;
-
+double  prv_time, prv_err, prv_errF,prv_RveloErr,prv_LveloErr;
+double  a,b,c;
+double  a2,b2,c2;
 double  now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
+double  RF2velo,pr2v_RF2velo, prv_RF2velo,LF2velo, pr2v_LF2velo, prv_LF2velo;
+double  now_x,prv_x,Rlevantvelo,prv_Rlevantvelo,now_e,now_u1,prv_u1,now_x1,prv_x1,Llevantvelo,prv_Llevantvelo,now_e1,now_u2,prv_u2;
 double  W   = 17.0;
 double  PI  = 3.1416;
-double  fc  = 0.5;
+double  fc  = 2.5;
 double  tau = 1/(2*PI*fc);
+double   C              = (2.0*PI*fc)*(2.0*PI*fc);
+double   B              = (2.0*PI*fc)*sqrt(2.0);
+double   E              = 500.0 * (10 ^ (4));
+double   alpha2          = 1.1 * E;
+double   lambda         = sqrt(E);
 double  RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV;
 long    counts_per_rev = (48*75);
 
@@ -41,6 +49,11 @@
 {
     return ((pulses/counts_per_rev)*360);
 }
+double sign(double xe) 
+{
+  if (xe > 0) { return 1; } 
+  else { return -1; }
+}
 
 int main()
 {
@@ -81,12 +94,33 @@
         
         RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau);
         LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau);
+       
+        a         = ((samp_time*samp_time)*C)*RactVelo;
+        b         = (2.0+(samp_time*B))*prv_RF2velo;
+        c         = pr2v_RF2velo;
+        RF2velo   = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
         
-        RveloErr = RdesVelo -RactVelo;
-        LveloErr = LdesVelo -LactVelo;
+        a2         = ((samp_time*samp_time)*C)*LactVelo;
+        b2         = (2.0+(samp_time*B))*prv_LF2velo;
+        c2         = pr2v_LF2velo;
+        LF2velo    = (a2+b2-c2)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
         
-        voltR  = RveloErr*0.03; 
-        voltL  = LveloErr*0.03;
+         now_x       = (prv_Rlevantvelo* samp_time) + prv_x;
+         now_e       = now_x - now_ang1;
+         now_u1      = (-alpha2 * sign(now_e) * samp_time) + prv_u1;
+         Rlevantvelo = now_u1 - (lambda * sqrt(abs(now_e)) * sign(now_e));
+         
+         now_x1       = (prv_Llevantvelo* samp_time) + prv_x1;
+         now_e1       = now_x1 - now_ang2;
+         now_u2       = (-alpha2 * sign(now_e1) * samp_time) + prv_u2;
+         Llevantvelo  = now_u1 - (lambda * sqrt(abs(now_e1)) * sign(now_e1));
+        
+        
+        RveloErr = RdesVelo -RFvelo ;
+        LveloErr = LdesVelo -LFvelo;
+        
+        voltR  = RveloErr *0.03+(((RveloErr - prv_RveloErr)/samp_time)*0); 
+        voltL  = LveloErr*0.03+(((LveloErr - prv_LveloErr)/samp_time)*0);
         
         voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
         voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
@@ -103,7 +137,9 @@
         if (pwmL>1)     {pwmL = 1;}
         else            {pwmL = pwmL;}
         
-        pc.printf("  %f     %f", RdesVelo, RactVelo);
+        pc.printf("  %f     %f   ",RactVelo,Rlevantvelo);
+        //pc.printf("  %f", samp_time );
+        
         printf("\n\r");
         //on left motor, red and black cables are crossed-connected to have forward movement when pin_L1=1 & pin_L2=0.
         //on breadboard, + line represent +5v. - line represent GND. 12v is only used to supply L298.
@@ -116,5 +152,18 @@
         prv_voltLF  = voltLF;
         prv_RFvelo  = RFvelo;
         prv_LFvelo  = LFvelo;
+        prv_RveloErr = RveloErr;
+        prv_LveloErr = LveloErr;
+        prv_RF2velo = RF2velo;
+        pr2v_RF2velo = prv_RF2velo;
+        prv_LF2velo  = LF2velo;
+        pr2v_LF2velo = prv_LF2velo;
+        prv_x        =now_x;
+        prv_u1       =now_u1;
+        prv_x1       =now_x1;
+        prv_u2       =now_u2;
+        prv_Rlevantvelo =Rlevantvelo;
+        prv_Llevantvelo =Llevantvelo;
+            
     }
 }
\ No newline at end of file