Myo Thant Sin Aung / Mbed 2 deprecated myIRRobo

01

Dependencies:   QEI mbed

Fork of my1stOLPFilter by velo_filter

main.cpp@2:8db9af8bd3fd, 2018-08-07 (annotated)

Committer:
MTSAung
Date:
Tue Aug 07 10:28:00 2018 +0000
Revision:
2:8db9af8bd3fd
Parent:
1:4088422c9f8a 
myIRMobileRobo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MTSAung 0:5da774f9c59c 1 #include "QEI.h"
MTSAung 0:5da774f9c59c 2 #include "mbed.h"
MTSAung 0:5da774f9c59c 3 Serial pc(USBTX, USBRX);
MTSAung 0:5da774f9c59c 4
MTSAung 2:8db9af8bd3fd 5 PwmOut pwmR (p21);
MTSAung 2:8db9af8bd3fd 6 DigitalOut pin_R1 (p22);
MTSAung 2:8db9af8bd3fd 7 DigitalOut pin_R2 (p23);
MTSAung 2:8db9af8bd3fd 8
MTSAung 2:8db9af8bd3fd 9 PwmOut pwmL (p24);
MTSAung 2:8db9af8bd3fd 10 DigitalOut pin_L1 (p25);
MTSAung 2:8db9af8bd3fd 11 DigitalOut pin_L2 (p26);
MTSAung 2:8db9af8bd3fd 12
MTSAung 2:8db9af8bd3fd 13 DigitalIn IRpin1 (p16);
MTSAung 2:8db9af8bd3fd 14 DigitalIn IRpin2 (p17);
MTSAung 2:8db9af8bd3fd 15 DigitalIn IRpin3 (p18);
MTSAung 2:8db9af8bd3fd 16 DigitalIn IRpin4 (p19);
MTSAung 2:8db9af8bd3fd 17 DigitalIn IRpin5 (p20);
MTSAung 2:8db9af8bd3fd 18
MTSAung 2:8db9af8bd3fd 19 double voltR, voltL;
MTSAung 2:8db9af8bd3fd 20 int IRread1, IRread2, IRread3, IRread4, IRread5;
MTSAung 2:8db9af8bd3fd 21 int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;
MTSAung 2:8db9af8bd3fd 22 int prv_err, act, err;
MTSAung 2:8db9af8bd3fd 23
MTSAung 2:8db9af8bd3fd 24 int des = 0;
MTSAung 2:8db9af8bd3fd 25 double alpha = 0.4;
MTSAung 2:8db9af8bd3fd 26 double tau = 0.1;
MTSAung 2:8db9af8bd3fd 27 long counts_per_rev = 64;
MTSAung 2:8db9af8bd3fd 28 double prv_ang, prv_time;
MTSAung 2:8db9af8bd3fd 29 int prv_errF = 0;
MTSAung 2:8db9af8bd3fd 30 double errL, errF, now_ang, now_omg, now_time, samp_time, GoLog;
MTSAung 0:5da774f9c59c 31
MTSAung 0:5da774f9c59c 32 QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING);
MTSAung 0:5da774f9c59c 33
MTSAung 2:8db9af8bd3fd 34 float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); }
MTSAung 0:5da774f9c59c 35
MTSAung 0:5da774f9c59c 36 int main()
MTSAung 0:5da774f9c59c 37 {
MTSAung 0:5da774f9c59c 38 Timer myTime;
MTSAung 0:5da774f9c59c 39 myTime.reset();
MTSAung 0:5da774f9c59c 40 myTime.start();
MTSAung 2:8db9af8bd3fd 41 pc.baud(9600);
MTSAung 0:5da774f9c59c 42 while(1) {
MTSAung 2:8db9af8bd3fd 43 IRread1 = IRpin1.read();
MTSAung 2:8db9af8bd3fd 44 IRread2 = IRpin2.read();
MTSAung 2:8db9af8bd3fd 45 IRread3 = IRpin3.read();
MTSAung 2:8db9af8bd3fd 46 IRread4 = IRpin4.read();
MTSAung 2:8db9af8bd3fd 47 IRread5 = IRpin5.read();
MTSAung 2:8db9af8bd3fd 48
MTSAung 2:8db9af8bd3fd 49 now_time = myTime.read_ms()/1000.0;
MTSAung 2:8db9af8bd3fd 50 samp_time = now_time - prv_time;
MTSAung 2:8db9af8bd3fd 51 now_ang = pulsesToDegrees(wheel.getPulses());
MTSAung 2:8db9af8bd3fd 52 now_omg = (now_ang - prv_ang)/samp_time;
MTSAung 2:8db9af8bd3fd 53
MTSAung 2:8db9af8bd3fd 54 if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;}
MTSAung 2:8db9af8bd3fd 55 if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;}
MTSAung 2:8db9af8bd3fd 56 if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;}
MTSAung 2:8db9af8bd3fd 57 if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;}
MTSAung 2:8db9af8bd3fd 58 if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;}
MTSAung 2:8db9af8bd3fd 59
MTSAung 2:8db9af8bd3fd 60 act = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
MTSAung 2:8db9af8bd3fd 61 err = des - act ;
MTSAung 2:8db9af8bd3fd 62 errL = double ( err + alpha * ((err - prv_err)/samp_time ));
MTSAung 2:8db9af8bd3fd 63 errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time );
MTSAung 2:8db9af8bd3fd 64
MTSAung 2:8db9af8bd3fd 65 if (errF < 0) { voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1; }
MTSAung 2:8db9af8bd3fd 66 else if(errF > 0) { voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; }
MTSAung 2:8db9af8bd3fd 67 else if(errF == 0) { voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0; }
MTSAung 2:8db9af8bd3fd 68 pwmR = (voltR/12.0);
MTSAung 2:8db9af8bd3fd 69 pwmL = (voltL/12.0);
MTSAung 2:8db9af8bd3fd 70
MTSAung 2:8db9af8bd3fd 71 //pc.printf(" %d %f %d %d %d %d %d %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog);
MTSAung 2:8db9af8bd3fd 72 pc.printf(" %d %f %f\r", err, errL, errF);
MTSAung 0:5da774f9c59c 73 printf("\n\r");
MTSAung 0:5da774f9c59c 74 prv_ang = now_ang;
MTSAung 0:5da774f9c59c 75 prv_time = now_time;
MTSAung 2:8db9af8bd3fd 76 prv_err = err;
MTSAung 2:8db9af8bd3fd 77 prv_errF = errF;
MTSAung 0:5da774f9c59c 78 }
MTSAung 2:8db9af8bd3fd 79 }