velo_filter / Mbed 2 deprecated newprogram

updatw 08/09/18

Dependencies:   QEI mbed

main.cpp@0:8e7dc5d573f7, 2018-09-08 (annotated)

Committer:
NiNiHtayLwin
Date:
Sat Sep 08 09:50:53 2018 +0000
Revision:
0:8e7dc5d573f7
Child:
1:087dfa2bff27
update 08/09/18

Who changed what in which revision?

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