![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
-
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?
User | Revision | Line number | New 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 | } |