Diff: main.cpp

Revision:

0:8ffbb19fee71

Child:

1:3b55e7add58b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 25 07:31:28 2018 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "QEI.h"
+Serial pc(USBTX, USBRX);
+
+PwmOut      pwmR    (p21);
+DigitalOut  pin_R1  (p22);
+DigitalOut  pin_R2  (p23);
+
+PwmOut      pwmL    (p24);
+DigitalOut  pin_L1  (p25);
+DigitalOut  pin_L2  (p26);
+
+DigitalIn   IRpin1  (p16);
+DigitalIn   IRpin2  (p17);
+DigitalIn   IRpin3  (p18);
+DigitalIn   IRpin4  (p19);
+DigitalIn   IRpin5  (p20);
+
+double  voltR, voltL, voltRF, voltLF, prv_voltRF, prv_voltLF;
+int     IRread1, IRread2, IRread3, IRread4, IRread5;
+int     OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;
+
+int     act, des = 2000;
+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  now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
+double  W   = 17.0;
+double  PI  = 3.1416;
+double  fc  = 0.5;
+double  tau = 1/(2*PI*fc);
+double  RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV;
+long    counts_per_rev = (48*75);
+
+QEI wheel1 (p29, p27, NC, counts_per_rev, QEI::X2_ENCODING);
+QEI wheel2 (p30, p28, NC, counts_per_rev, QEI::X2_ENCODING);
+
+float pulsesToDegrees(float pulses)
+{
+    return ((pulses/counts_per_rev)*360);
+}
+
+int main()
+{
+    Timer myTime;
+    myTime.reset();
+    myTime.start();
+    pc.baud(57600);
+    while(1) {
+        IRread1   = IRpin1.read();
+        IRread2   = IRpin2.read();
+        IRread3   = IRpin3.read();
+        IRread4   = IRpin4.read();
+        IRread5   = IRpin5.read();
+        
+        now_time  = myTime.read_us()/1000000.0;
+        samp_time = now_time - prv_time;
+        
+        now_ang1  = pulsesToDegrees(wheel1.getPulses());
+        RactVelo  = (now_ang1 - prv_ang1)/samp_time;
+        
+        now_ang2  = pulsesToDegrees(wheel2.getPulses());
+        LactVelo  = (now_ang2 - prv_ang2)/samp_time;
+        
+        if(IRread1 == 0)  {OIRread1 = 1;} else if(IRread1 == 1)   {OIRread1 = 0;}
+        if(IRread2 == 0)  {OIRread2 = 1;} else if(IRread2 == 1)   {OIRread2 = 0;}
+        if(IRread3 == 0)  {OIRread3 = 1;} else if(IRread3 == 1)   {OIRread3 = 0;}
+        if(IRread4 == 0)  {OIRread4 = 1;} else if(IRread4 == 1)   {OIRread4 = 0;}
+        if(IRread5 == 0)  {OIRread5 = 1;} else if(IRread5 == 1)   {OIRread5 = 0;}
+        
+        act  = ((IRread1*(0))+(IRread2*(1000))+(IRread3*(2000))+(IRread4*(3000))+(IRread5*(4000)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
+        err  = des - act ;
+        errL = err + (alpha * ((err - prv_err)/samp_time ));
+        
+        omega    = (err*0.05) + (((err - prv_err)/samp_time)*0.1);
+        VV       = (390.0);
+        RdesVelo = VV - (omega*(W/2.0));
+        LdesVelo = VV + (omega*(W/2.0));
+        
+        RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau);
+        LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau);
+        
+        RveloErr = RdesVelo -RactVelo;
+        LveloErr = LdesVelo -LactVelo;
+        
+        voltR  = RveloErr*0.03; 
+        voltL  = LveloErr*0.03;
+        
+        voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
+        voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
+                
+        if (voltR>0)    {  pin_R1 = 0; pin_R2 = 1; }
+        else            {  pin_R1 = 1; pin_R2 = 0; }
+        if (voltL>0)    {  pin_L1 = 0; pin_L2 = 1; }
+        else            {  pin_L1 = 1; pin_L2 = 0; }
+        
+        pwmR  = (abs(voltR)/12.0); 
+        pwmL  = (abs(voltL)/12.0); 
+        if (pwmR>1)     {pwmR = 1;}
+        else            {pwmR = pwmR;}
+        if (pwmL>1)     {pwmL = 1;}
+        else            {pwmL = pwmL;}
+        
+        pc.printf("  %f     %f", RdesVelo, RactVelo);
+        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.
+        prv_time    = now_time;
+        prv_ang1    = now_ang1;
+        prv_ang2    = now_ang2;
+        prv_err     = err;
+        prv_errF    = errF;
+        prv_voltRF  = voltRF;
+        prv_voltLF  = voltLF;
+        prv_RFvelo  = RFvelo;
+        prv_LFvelo  = LFvelo;
+    }
+}
\ No newline at end of file