![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
-
main.cpp
- Committer:
- MTSAung
- Date:
- 2018-09-25
- Revision:
- 0:8ffbb19fee71
- Child:
- 1:3b55e7add58b
File content as of revision 0:8ffbb19fee71:
#include "mbed.h" #include "QEI.h" Serial pc(USBTX, USBRX); PwmOut pwmR (p21); DigitalOut pin_R1 (p22); DigitalOut pin_R2 (p23); PwmOut pwmL (p24); DigitalOut pin_L1 (p25); DigitalOut pin_L2 (p26); DigitalIn IRpin1 (p16); DigitalIn IRpin2 (p17); DigitalIn IRpin3 (p18); DigitalIn IRpin4 (p19); DigitalIn IRpin5 (p20); double voltR, voltL, voltRF, voltLF, prv_voltRF, prv_voltLF; int IRread1, IRread2, IRread3, IRread4, IRread5; int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5; int act, des = 2000; double alpha = 0.5; double RFvelo, LFvelo, prv_RFvelo, prv_LFvelo; double err, errL, errF, now_time, samp_time; double prv_time, prv_err, prv_errF; double now_ang1, prv_ang1, now_ang2, prv_ang2, now_omg1, now_omg2; double W = 17.0; double PI = 3.1416; double fc = 0.5; double tau = 1/(2*PI*fc); double RactVelo, LactVelo, RdesVelo, LdesVelo, RveloErr, LveloErr, omega, VV; long counts_per_rev = (48*75); QEI wheel1 (p29, p27, NC, counts_per_rev, QEI::X2_ENCODING); QEI wheel2 (p30, p28, NC, counts_per_rev, QEI::X2_ENCODING); float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); } int main() { Timer myTime; myTime.reset(); myTime.start(); pc.baud(57600); while(1) { IRread1 = IRpin1.read(); IRread2 = IRpin2.read(); IRread3 = IRpin3.read(); IRread4 = IRpin4.read(); IRread5 = IRpin5.read(); now_time = myTime.read_us()/1000000.0; samp_time = now_time - prv_time; now_ang1 = pulsesToDegrees(wheel1.getPulses()); RactVelo = (now_ang1 - prv_ang1)/samp_time; now_ang2 = pulsesToDegrees(wheel2.getPulses()); LactVelo = (now_ang2 - prv_ang2)/samp_time; if(IRread1 == 0) {OIRread1 = 1;} else if(IRread1 == 1) {OIRread1 = 0;} if(IRread2 == 0) {OIRread2 = 1;} else if(IRread2 == 1) {OIRread2 = 0;} if(IRread3 == 0) {OIRread3 = 1;} else if(IRread3 == 1) {OIRread3 = 0;} if(IRread4 == 0) {OIRread4 = 1;} else if(IRread4 == 1) {OIRread4 = 0;} if(IRread5 == 0) {OIRread5 = 1;} else if(IRread5 == 1) {OIRread5 = 0;} act = ((IRread1*(0))+(IRread2*(1000))+(IRread3*(2000))+(IRread4*(3000))+(IRread5*(4000)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ; err = des - act ; errL = err + (alpha * ((err - prv_err)/samp_time )); omega = (err*0.05) + (((err - prv_err)/samp_time)*0.1); VV = (390.0); RdesVelo = VV - (omega*(W/2.0)); LdesVelo = VV + (omega*(W/2.0)); RFvelo = ( samp_time * RactVelo + (tau * prv_RFvelo) ) / ( samp_time + tau); LFvelo = ( samp_time * LactVelo + (tau * prv_LFvelo) ) / ( samp_time + tau); RveloErr = RdesVelo -RactVelo; LveloErr = LdesVelo -LactVelo; voltR = RveloErr*0.03; voltL = LveloErr*0.03; voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau); voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau); if (voltR>0) { pin_R1 = 0; pin_R2 = 1; } else { pin_R1 = 1; pin_R2 = 0; } if (voltL>0) { pin_L1 = 0; pin_L2 = 1; } else { pin_L1 = 1; pin_L2 = 0; } pwmR = (abs(voltR)/12.0); pwmL = (abs(voltL)/12.0); if (pwmR>1) {pwmR = 1;} else {pwmR = pwmR;} if (pwmL>1) {pwmL = 1;} else {pwmL = pwmL;} pc.printf(" %f %f", RdesVelo, RactVelo); printf("\n\r"); //on left motor, red and black cables are crossed-connected to have forward movement when pin_L1=1 & pin_L2=0. //on breadboard, + line represent +5v. - line represent GND. 12v is only used to supply L298. prv_time = now_time; prv_ang1 = now_ang1; prv_ang2 = now_ang2; prv_err = err; prv_errF = errF; prv_voltRF = voltRF; prv_voltLF = voltLF; prv_RFvelo = RFvelo; prv_LFvelo = LFvelo; } }