velo_filter / Mbed 2 deprecated myIRWKine24Sept

-

Dependencies:   QEI mbed

main.cpp@0:8ffbb19fee71, 2018-09-25 (annotated)

Committer:
MTSAung
Date:
Tue Sep 25 07:31:28 2018 +0000
Revision:
0:8ffbb19fee71
Child:
1:3b55e7add58b
myLFMo25Sept18

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MTSAung 0:8ffbb19fee71 1 #include "mbed.h"
MTSAung 0:8ffbb19fee71 2 #include "QEI.h"
MTSAung 0:8ffbb19fee71 3 Serial pc(USBTX, USBRX);
MTSAung 0:8ffbb19fee71 4
MTSAung 0:8ffbb19fee71 5 PwmOut pwmR (p21);
MTSAung 0:8ffbb19fee71 6 DigitalOut pin_R1 (p22);
MTSAung 0:8ffbb19fee71 7 DigitalOut pin_R2 (p23);
MTSAung 0:8ffbb19fee71 8
MTSAung 0:8ffbb19fee71 9 PwmOut pwmL (p24);
MTSAung 0:8ffbb19fee71 10 DigitalOut pin_L1 (p25);
MTSAung 0:8ffbb19fee71 11 DigitalOut pin_L2 (p26);
MTSAung 0:8ffbb19fee71 12
MTSAung 0:8ffbb19fee71 13 DigitalIn IRpin1 (p16);
MTSAung 0:8ffbb19fee71 14 DigitalIn IRpin2 (p17);
MTSAung 0:8ffbb19fee71 15 DigitalIn IRpin3 (p18);
MTSAung 0:8ffbb19fee71 16 DigitalIn IRpin4 (p19);
MTSAung 0:8ffbb19fee71 17 DigitalIn IRpin5 (p20);
MTSAung 0:8ffbb19fee71 18
MTSAung 0:8ffbb19fee71 19 double voltR, voltL, voltRF, voltLF, prv_voltRF, prv_voltLF;
MTSAung 0:8ffbb19fee71 20 int IRread1, IRread2, IRread3, IRread4, IRread5;
MTSAung 0:8ffbb19fee71 21 int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5;
MTSAung 0:8ffbb19fee71 22
MTSAung 0:8ffbb19fee71 23 int act, des = 2000;
MTSAung 0:8ffbb19fee71 24 double alpha = 0.5;
MTSAung 0:8ffbb19fee71 25 double RFvelo, LFvelo, prv_RFvelo, prv_LFvelo;
MTSAung 0:8ffbb19fee71 26 double err, errL, errF, now_time, samp_time;
MTSAung 0:8ffbb19fee71 27 double prv_time, prv_err, prv_errF;
MTSAung 0:8ffbb19fee71 28
MTSAung 0:8ffbb19fee71 29 double now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
MTSAung 0:8ffbb19fee71 30 double W = 17.0;
MTSAung 0:8ffbb19fee71 31 double PI = 3.1416;
MTSAung 0:8ffbb19fee71 32 double fc = 0.5;
MTSAung 0:8ffbb19fee71 33 double tau = 1/(2*PI*fc);
MTSAung 0:8ffbb19fee71 34 double RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV;
MTSAung 0:8ffbb19fee71 35 long counts_per_rev = (48*75);
MTSAung 0:8ffbb19fee71 36
MTSAung 0:8ffbb19fee71 37 QEI wheel1 (p29, p27, NC, counts_per_rev, QEI::X2_ENCODING);
MTSAung 0:8ffbb19fee71 38 QEI wheel2 (p30, p28, NC, counts_per_rev, QEI::X2_ENCODING);
MTSAung 0:8ffbb19fee71 39
MTSAung 0:8ffbb19fee71 40 float pulsesToDegrees(float pulses)
MTSAung 0:8ffbb19fee71 41 {
MTSAung 0:8ffbb19fee71 42 return ((pulses/counts_per_rev)*360);
MTSAung 0:8ffbb19fee71 43 }
MTSAung 0:8ffbb19fee71 44
MTSAung 0:8ffbb19fee71 45 int main()
MTSAung 0:8ffbb19fee71 46 {
MTSAung 0:8ffbb19fee71 47 Timer myTime;
MTSAung 0:8ffbb19fee71 48 myTime.reset();
MTSAung 0:8ffbb19fee71 49 myTime.start();
MTSAung 0:8ffbb19fee71 50 pc.baud(57600);
MTSAung 0:8ffbb19fee71 51 while(1) {
MTSAung 0:8ffbb19fee71 52 IRread1 = IRpin1.read();
MTSAung 0:8ffbb19fee71 53 IRread2 = IRpin2.read();
MTSAung 0:8ffbb19fee71 54 IRread3 = IRpin3.read();
MTSAung 0:8ffbb19fee71 55 IRread4 = IRpin4.read();
MTSAung 0:8ffbb19fee71 56 IRread5 = IRpin5.read();
MTSAung 0:8ffbb19fee71 57
MTSAung 0:8ffbb19fee71 58 now_time = myTime.read_us()/1000000.0;
MTSAung 0:8ffbb19fee71 59 samp_time = now_time - prv_time;
MTSAung 0:8ffbb19fee71 60
MTSAung 0:8ffbb19fee71 61 now_ang1 = pulsesToDegrees(wheel1.getPulses());
MTSAung 0:8ffbb19fee71 62 RactVelo = (now_ang1 - prv_ang1)/samp_time;
MTSAung 0:8ffbb19fee71 63
MTSAung 0:8ffbb19fee71 64 now_ang2 = pulsesToDegrees(wheel2.getPulses());
MTSAung 0:8ffbb19fee71 65 LactVelo = (now_ang2 - prv_ang2)/samp_time;
MTSAung 0:8ffbb19fee71 66
MTSAung 0:8ffbb19fee71 67 if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;}
MTSAung 0:8ffbb19fee71 68 if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;}
MTSAung 0:8ffbb19fee71 69 if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;}
MTSAung 0:8ffbb19fee71 70 if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;}
MTSAung 0:8ffbb19fee71 71 if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;}
MTSAung 0:8ffbb19fee71 72
MTSAung 0:8ffbb19fee71 73 act = ((IRread1*(0))+(IRread2*(1000))+(IRread3*(2000))+(IRread4*(3000))+(IRread5*(4000)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
MTSAung 0:8ffbb19fee71 74 err = des - act ;
MTSAung 0:8ffbb19fee71 75 errL = err + (alpha * ((err - prv_err)/samp_time ));
MTSAung 0:8ffbb19fee71 76
MTSAung 0:8ffbb19fee71 77 omega = (err*0.05) + (((err - prv_err)/samp_time)*0.1);
MTSAung 0:8ffbb19fee71 78 VV = (390.0);
MTSAung 0:8ffbb19fee71 79 RdesVelo = VV - (omega*(W/2.0));
MTSAung 0:8ffbb19fee71 80 LdesVelo = VV + (omega*(W/2.0));
MTSAung 0:8ffbb19fee71 81
MTSAung 0:8ffbb19fee71 82 RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 83 LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 84
MTSAung 0:8ffbb19fee71 85 RveloErr = RdesVelo -RactVelo;
MTSAung 0:8ffbb19fee71 86 LveloErr = LdesVelo -LactVelo;
MTSAung 0:8ffbb19fee71 87
MTSAung 0:8ffbb19fee71 88 voltR = RveloErr*0.03;
MTSAung 0:8ffbb19fee71 89 voltL = LveloErr*0.03;
MTSAung 0:8ffbb19fee71 90
MTSAung 0:8ffbb19fee71 91 voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 92 voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 93
MTSAung 0:8ffbb19fee71 94 if (voltR>0) { pin_R1 = 0; pin_R2 = 1; }
MTSAung 0:8ffbb19fee71 95 else { pin_R1 = 1; pin_R2 = 0; }
MTSAung 0:8ffbb19fee71 96 if (voltL>0) { pin_L1 = 0; pin_L2 = 1; }
MTSAung 0:8ffbb19fee71 97 else { pin_L1 = 1; pin_L2 = 0; }
MTSAung 0:8ffbb19fee71 98
MTSAung 0:8ffbb19fee71 99 pwmR = (abs(voltR)/12.0);
MTSAung 0:8ffbb19fee71 100 pwmL = (abs(voltL)/12.0);
MTSAung 0:8ffbb19fee71 101 if (pwmR>1) {pwmR = 1;}
MTSAung 0:8ffbb19fee71 102 else {pwmR = pwmR;}
MTSAung 0:8ffbb19fee71 103 if (pwmL>1) {pwmL = 1;}
MTSAung 0:8ffbb19fee71 104 else {pwmL = pwmL;}
MTSAung 0:8ffbb19fee71 105
MTSAung 0:8ffbb19fee71 106 pc.printf(" %f %f", RdesVelo, RactVelo);
MTSAung 0:8ffbb19fee71 107 printf("\n\r");
MTSAung 0:8ffbb19fee71 108 //on left motor, red and black cables are crossed-connected to have forward movement when pin_L1=1 & pin_L2=0.
MTSAung 0:8ffbb19fee71 109 //on breadboard, + line represent +5v. - line represent GND. 12v is only used to supply L298.
MTSAung 0:8ffbb19fee71 110 prv_time = now_time;
MTSAung 0:8ffbb19fee71 111 prv_ang1 = now_ang1;
MTSAung 0:8ffbb19fee71 112 prv_ang2 = now_ang2;
MTSAung 0:8ffbb19fee71 113 prv_err = err;
MTSAung 0:8ffbb19fee71 114 prv_errF = errF;
MTSAung 0:8ffbb19fee71 115 prv_voltRF = voltRF;
MTSAung 0:8ffbb19fee71 116 prv_voltLF = voltLF;
MTSAung 0:8ffbb19fee71 117 prv_RFvelo = RFvelo;
MTSAung 0:8ffbb19fee71 118 prv_LFvelo = LFvelo;
MTSAung 0:8ffbb19fee71 119 }
MTSAung 0:8ffbb19fee71 120 }