Revision 2:8db9af8bd3fd, committed 2018-08-07

Comitter:

MTSAung

Date:

Tue Aug 07 10:28:00 2018 +0000

Parent:

1:4088422c9f8a

Commit message:

myIRMobileRobo

Changed in this revision

--- a/main.cpp	Sun Jul 29 08:54:36 2018 +0000
+++ b/main.cpp	Tue Aug 07 10:28:00 2018 +0000
@@ -2,63 +2,78 @@
 #include "mbed.h"
 Serial pc(USBTX, USBRX);
 
-AnalogOut pwm    (p18);
-DigitalOut pin_a  (p19);
-DigitalOut pin_b  (p20);
-DigitalIn IRpin(p16);
-DigitalIn IRpin2(p17);
-int ir,ir2;
-long     counts_per_rev = (64);
-double   prv_ang        = 0.0;
-double   now_ang;
-double   now_omg        = 0.0;
-double   prv_time       = 0.0;
-double   now_time       = 0.0;
-double   samp_time      = 0.0;
-//double   now_x          = 0.0;
-//double   now_y          = 0.0;
+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;
+int     IRread1, IRread2, IRread3, IRread4, IRread5;
+int     OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;
+int     prv_err, act, err;
+
+int     des   = 0;
+double  alpha = 0.4;
+double  tau   = 0.1;
+long    counts_per_rev = 64;
+double  prv_ang, prv_time;
+int     prv_errF = 0;
+double  errL, errF, now_ang, now_omg, now_time, samp_time, GoLog;
 
 QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING);
 
-float pulsesToDegrees(float pulses)
-{
-    return ((pulses/counts_per_rev)*360);
-}
+float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); }
 
 int main()
 {
     Timer myTime;
     myTime.reset();
     myTime.start();
-    pc.baud(57600);
+    pc.baud(9600);
     while(1) {
-        ir        = IRpin.read();
-        ir2       = IRpin2.read();
-        now_time  = myTime.read_ms()/1000.0;
-        samp_time = now_time - prv_time;
-        now_ang   = pulsesToDegrees(wheel.getPulses());
-        now_omg   = (now_ang - prv_ang)/samp_time;
-        if(ir == 0 && ir2 == 1)
-        {
-        pwm   = 0.8 ; 
-        pin_a = 1;
-        pin_b = 0;
-        }
-        else if(ir == 1 && ir2 == 0)
-        {
-         pwm   = 0.8 ; 
-        pin_a = 0;
-        pin_b = 1;
-        }
-        else
-        {
-        pwm   = 0.8 ; 
-        pin_a = 0;
-        pin_b = 0;
-        }       
-        pc.printf("%d   %d  %f\r",ir,ir2,now_omg);
+        IRread1     =   IRpin1.read();
+        IRread2     =   IRpin2.read();
+        IRread3     =   IRpin3.read();
+        IRread4     =   IRpin4.read();
+        IRread5     =   IRpin5.read();
+        
+        now_time    =   myTime.read_ms()/1000.0;
+        samp_time   =   now_time - prv_time;
+        now_ang     =   pulsesToDegrees(wheel.getPulses());
+        now_omg     =   (now_ang - prv_ang)/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  = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
+        err  = des - act ;
+        errL = double ( err + alpha * ((err - prv_err)/samp_time ));
+        errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time );
+        
+        if     (errF < 0)    {   voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1;  }
+        else if(errF > 0)    {   voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; }
+        else if(errF == 0)   {   voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0;  }           
+        pwmR = (voltR/12.0); 
+        pwmL = (voltL/12.0); 
+        
+        //pc.printf("   %d   %f  %d  %d  %d  %d  %d  %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog);
+        pc.printf("   %d %f %f\r", err, errL, errF);
         printf("\n\r");
         prv_ang  = now_ang;
         prv_time = now_time;
+        prv_err  = err;
+        prv_errF = errF;
     }
-}
+}
\ No newline at end of file