velo_filter / Mbed 2 deprecated myIRWKine24Sept

-

Dependencies:   QEI mbed

main.cpp@1:3b55e7add58b, 2018-09-25 (annotated)

Committer:
NiNiHtayLwin
Date:
Tue Sep 25 09:58:15 2018 +0000
Revision:
1:3b55e7add58b
Parent:
0:8ffbb19fee71 
---

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;
NiNiHtayLwin 1:3b55e7add58b 27 double prv_time, prv_err, prv_errF,prv_RveloErr,prv_LveloErr;
NiNiHtayLwin 1:3b55e7add58b 28 double a,b,c;
NiNiHtayLwin 1:3b55e7add58b 29 double a2,b2,c2;
MTSAung 0:8ffbb19fee71 30 double now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2;
NiNiHtayLwin 1:3b55e7add58b 31 double RF2velo,pr2v_RF2velo, prv_RF2velo,LF2velo, pr2v_LF2velo, prv_LF2velo;
NiNiHtayLwin 1:3b55e7add58b 32 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;
MTSAung 0:8ffbb19fee71 33 double W = 17.0;
MTSAung 0:8ffbb19fee71 34 double PI = 3.1416;
NiNiHtayLwin 1:3b55e7add58b 35 double fc = 2.5;
MTSAung 0:8ffbb19fee71 36 double tau = 1/(2*PI*fc);
NiNiHtayLwin 1:3b55e7add58b 37 double C = (2.0*PI*fc)*(2.0*PI*fc);
NiNiHtayLwin 1:3b55e7add58b 38 double B = (2.0*PI*fc)*sqrt(2.0);
NiNiHtayLwin 1:3b55e7add58b 39 double E = 500.0 * (10 ^ (4));
NiNiHtayLwin 1:3b55e7add58b 40 double alpha2 = 1.1 * E;
NiNiHtayLwin 1:3b55e7add58b 41 double lambda = sqrt(E);
MTSAung 0:8ffbb19fee71 42 double RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV;
MTSAung 0:8ffbb19fee71 43 long counts_per_rev = (48*75);
MTSAung 0:8ffbb19fee71 44
MTSAung 0:8ffbb19fee71 45 QEI wheel1 (p29, p27, NC, counts_per_rev, QEI::X2_ENCODING);
MTSAung 0:8ffbb19fee71 46 QEI wheel2 (p30, p28, NC, counts_per_rev, QEI::X2_ENCODING);
MTSAung 0:8ffbb19fee71 47
MTSAung 0:8ffbb19fee71 48 float pulsesToDegrees(float pulses)
MTSAung 0:8ffbb19fee71 49 {
MTSAung 0:8ffbb19fee71 50 return ((pulses/counts_per_rev)*360);
MTSAung 0:8ffbb19fee71 51 }
NiNiHtayLwin 1:3b55e7add58b 52 double sign(double xe)
NiNiHtayLwin 1:3b55e7add58b 53 {
NiNiHtayLwin 1:3b55e7add58b 54 if (xe > 0) { return 1; }
NiNiHtayLwin 1:3b55e7add58b 55 else { return -1; }
NiNiHtayLwin 1:3b55e7add58b 56 }
MTSAung 0:8ffbb19fee71 57
MTSAung 0:8ffbb19fee71 58 int main()
MTSAung 0:8ffbb19fee71 59 {
MTSAung 0:8ffbb19fee71 60 Timer myTime;
MTSAung 0:8ffbb19fee71 61 myTime.reset();
MTSAung 0:8ffbb19fee71 62 myTime.start();
MTSAung 0:8ffbb19fee71 63 pc.baud(57600);
MTSAung 0:8ffbb19fee71 64 while(1) {
MTSAung 0:8ffbb19fee71 65 IRread1 = IRpin1.read();
MTSAung 0:8ffbb19fee71 66 IRread2 = IRpin2.read();
MTSAung 0:8ffbb19fee71 67 IRread3 = IRpin3.read();
MTSAung 0:8ffbb19fee71 68 IRread4 = IRpin4.read();
MTSAung 0:8ffbb19fee71 69 IRread5 = IRpin5.read();
MTSAung 0:8ffbb19fee71 70
MTSAung 0:8ffbb19fee71 71 now_time = myTime.read_us()/1000000.0;
MTSAung 0:8ffbb19fee71 72 samp_time = now_time - prv_time;
MTSAung 0:8ffbb19fee71 73
MTSAung 0:8ffbb19fee71 74 now_ang1 = pulsesToDegrees(wheel1.getPulses());
MTSAung 0:8ffbb19fee71 75 RactVelo = (now_ang1 - prv_ang1)/samp_time;
MTSAung 0:8ffbb19fee71 76
MTSAung 0:8ffbb19fee71 77 now_ang2 = pulsesToDegrees(wheel2.getPulses());
MTSAung 0:8ffbb19fee71 78 LactVelo = (now_ang2 - prv_ang2)/samp_time;
MTSAung 0:8ffbb19fee71 79
MTSAung 0:8ffbb19fee71 80 if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;}
MTSAung 0:8ffbb19fee71 81 if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;}
MTSAung 0:8ffbb19fee71 82 if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;}
MTSAung 0:8ffbb19fee71 83 if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;}
MTSAung 0:8ffbb19fee71 84 if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;}
MTSAung 0:8ffbb19fee71 85
MTSAung 0:8ffbb19fee71 86 act = ((IRread1*(0))+(IRread2*(1000))+(IRread3*(2000))+(IRread4*(3000))+(IRread5*(4000)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ;
MTSAung 0:8ffbb19fee71 87 err = des - act ;
MTSAung 0:8ffbb19fee71 88 errL = err + (alpha * ((err - prv_err)/samp_time ));
MTSAung 0:8ffbb19fee71 89
MTSAung 0:8ffbb19fee71 90 omega = (err*0.05) + (((err - prv_err)/samp_time)*0.1);
MTSAung 0:8ffbb19fee71 91 VV = (390.0);
MTSAung 0:8ffbb19fee71 92 RdesVelo = VV - (omega*(W/2.0));
MTSAung 0:8ffbb19fee71 93 LdesVelo = VV + (omega*(W/2.0));
MTSAung 0:8ffbb19fee71 94
MTSAung 0:8ffbb19fee71 95 RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 96 LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau);
NiNiHtayLwin 1:3b55e7add58b 97
NiNiHtayLwin 1:3b55e7add58b 98 a = ((samp_time*samp_time)*C)*RactVelo;
NiNiHtayLwin 1:3b55e7add58b 99 b = (2.0+(samp_time*B))*prv_RF2velo;
NiNiHtayLwin 1:3b55e7add58b 100 c = pr2v_RF2velo;
NiNiHtayLwin 1:3b55e7add58b 101 RF2velo = (a+b-c)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
MTSAung 0:8ffbb19fee71 102
NiNiHtayLwin 1:3b55e7add58b 103 a2 = ((samp_time*samp_time)*C)*LactVelo;
NiNiHtayLwin 1:3b55e7add58b 104 b2 = (2.0+(samp_time*B))*prv_LF2velo;
NiNiHtayLwin 1:3b55e7add58b 105 c2 = pr2v_LF2velo;
NiNiHtayLwin 1:3b55e7add58b 106 LF2velo = (a2+b2-c2)/(((samp_time*samp_time)*C)+(samp_time*B)+1.0);
MTSAung 0:8ffbb19fee71 107
NiNiHtayLwin 1:3b55e7add58b 108 now_x = (prv_Rlevantvelo* samp_time) + prv_x;
NiNiHtayLwin 1:3b55e7add58b 109 now_e = now_x - now_ang1;
NiNiHtayLwin 1:3b55e7add58b 110 now_u1 = (-alpha2 * sign(now_e) * samp_time) + prv_u1;
NiNiHtayLwin 1:3b55e7add58b 111 Rlevantvelo = now_u1 - (lambda * sqrt(abs(now_e)) * sign(now_e));
NiNiHtayLwin 1:3b55e7add58b 112
NiNiHtayLwin 1:3b55e7add58b 113 now_x1 = (prv_Llevantvelo* samp_time) + prv_x1;
NiNiHtayLwin 1:3b55e7add58b 114 now_e1 = now_x1 - now_ang2;
NiNiHtayLwin 1:3b55e7add58b 115 now_u2 = (-alpha2 * sign(now_e1) * samp_time) + prv_u2;
NiNiHtayLwin 1:3b55e7add58b 116 Llevantvelo = now_u1 - (lambda * sqrt(abs(now_e1)) * sign(now_e1));
NiNiHtayLwin 1:3b55e7add58b 117
NiNiHtayLwin 1:3b55e7add58b 118
NiNiHtayLwin 1:3b55e7add58b 119 RveloErr = RdesVelo -RFvelo ;
NiNiHtayLwin 1:3b55e7add58b 120 LveloErr = LdesVelo -LFvelo;
NiNiHtayLwin 1:3b55e7add58b 121
NiNiHtayLwin 1:3b55e7add58b 122 voltR = RveloErr *0.03+(((RveloErr - prv_RveloErr)/samp_time)*0);
NiNiHtayLwin 1:3b55e7add58b 123 voltL = LveloErr*0.03+(((LveloErr - prv_LveloErr)/samp_time)*0);
MTSAung 0:8ffbb19fee71 124
MTSAung 0:8ffbb19fee71 125 voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 126 voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau);
MTSAung 0:8ffbb19fee71 127
MTSAung 0:8ffbb19fee71 128 if (voltR>0) { pin_R1 = 0; pin_R2 = 1; }
MTSAung 0:8ffbb19fee71 129 else { pin_R1 = 1; pin_R2 = 0; }
MTSAung 0:8ffbb19fee71 130 if (voltL>0) { pin_L1 = 0; pin_L2 = 1; }
MTSAung 0:8ffbb19fee71 131 else { pin_L1 = 1; pin_L2 = 0; }
MTSAung 0:8ffbb19fee71 132
MTSAung 0:8ffbb19fee71 133 pwmR = (abs(voltR)/12.0);
MTSAung 0:8ffbb19fee71 134 pwmL = (abs(voltL)/12.0);
MTSAung 0:8ffbb19fee71 135 if (pwmR>1) {pwmR = 1;}
MTSAung 0:8ffbb19fee71 136 else {pwmR = pwmR;}
MTSAung 0:8ffbb19fee71 137 if (pwmL>1) {pwmL = 1;}
MTSAung 0:8ffbb19fee71 138 else {pwmL = pwmL;}
MTSAung 0:8ffbb19fee71 139
NiNiHtayLwin 1:3b55e7add58b 140 pc.printf(" %f %f ",RactVelo,Rlevantvelo);
NiNiHtayLwin 1:3b55e7add58b 141 //pc.printf(" %f", samp_time );
NiNiHtayLwin 1:3b55e7add58b 142
MTSAung 0:8ffbb19fee71 143 printf("\n\r");
MTSAung 0:8ffbb19fee71 144 //on left motor, red and black cables are crossed-connected to have forward movement when pin_L1=1 & pin_L2=0.
MTSAung 0:8ffbb19fee71 145 //on breadboard, + line represent +5v. - line represent GND. 12v is only used to supply L298.
MTSAung 0:8ffbb19fee71 146 prv_time = now_time;
MTSAung 0:8ffbb19fee71 147 prv_ang1 = now_ang1;
MTSAung 0:8ffbb19fee71 148 prv_ang2 = now_ang2;
MTSAung 0:8ffbb19fee71 149 prv_err = err;
MTSAung 0:8ffbb19fee71 150 prv_errF = errF;
MTSAung 0:8ffbb19fee71 151 prv_voltRF = voltRF;
MTSAung 0:8ffbb19fee71 152 prv_voltLF = voltLF;
MTSAung 0:8ffbb19fee71 153 prv_RFvelo = RFvelo;
MTSAung 0:8ffbb19fee71 154 prv_LFvelo = LFvelo;
NiNiHtayLwin 1:3b55e7add58b 155 prv_RveloErr = RveloErr;
NiNiHtayLwin 1:3b55e7add58b 156 prv_LveloErr = LveloErr;
NiNiHtayLwin 1:3b55e7add58b 157 prv_RF2velo = RF2velo;
NiNiHtayLwin 1:3b55e7add58b 158 pr2v_RF2velo = prv_RF2velo;
NiNiHtayLwin 1:3b55e7add58b 159 prv_LF2velo = LF2velo;
NiNiHtayLwin 1:3b55e7add58b 160 pr2v_LF2velo = prv_LF2velo;
NiNiHtayLwin 1:3b55e7add58b 161 prv_x =now_x;
NiNiHtayLwin 1:3b55e7add58b 162 prv_u1 =now_u1;
NiNiHtayLwin 1:3b55e7add58b 163 prv_x1 =now_x1;
NiNiHtayLwin 1:3b55e7add58b 164 prv_u2 =now_u2;
NiNiHtayLwin 1:3b55e7add58b 165 prv_Rlevantvelo =Rlevantvelo;
NiNiHtayLwin 1:3b55e7add58b 166 prv_Llevantvelo =Llevantvelo;
NiNiHtayLwin 1:3b55e7add58b 167
MTSAung 0:8ffbb19fee71 168 }
MTSAung 0:8ffbb19fee71 169 }