![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
01
Fork of my1stOLPFilter by
main.cpp
- Committer:
- MTSAung
- Date:
- 2018-08-07
- Revision:
- 2:8db9af8bd3fd
- Parent:
- 1:4088422c9f8a
File content as of revision 2:8db9af8bd3fd:
#include "QEI.h" #include "mbed.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; int IRread1, IRread2, IRread3, IRread4, IRread5; int OIRread1, OIRread2, OIRread3, OIRread4, OIRread5; int prv_err, act, err; int des = 0; double alpha = 0.4; double tau = 0.1; long counts_per_rev = 64; double prv_ang, prv_time; int prv_errF = 0; double errL, errF, now_ang, now_omg, now_time, samp_time, GoLog; QEI wheel (p29, p30, NC, counts_per_rev, QEI::X4_ENCODING); float pulsesToDegrees(float pulses) { return ((pulses/counts_per_rev)*360); } int main() { Timer myTime; myTime.reset(); myTime.start(); pc.baud(9600); while(1) { IRread1 = IRpin1.read(); IRread2 = IRpin2.read(); IRread3 = IRpin3.read(); IRread4 = IRpin4.read(); IRread5 = IRpin5.read(); now_time = myTime.read_ms()/1000.0; samp_time = now_time - prv_time; now_ang = pulsesToDegrees(wheel.getPulses()); now_omg = (now_ang - prv_ang)/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 = ((OIRread1*(-4))+(OIRread2*(-2))+(OIRread3*(0))+(OIRread4*(2))+(OIRread5*(4)))/ (IRread1 + IRread2 + IRread3 + IRread4 + IRread5) ; err = des - act ; errL = double ( err + alpha * ((err - prv_err)/samp_time )); errF = ((errL*samp_time) + (tau*prv_errF))/( tau + samp_time ); if (errF < 0) { voltR = 0.00; voltL = 12.00; pin_R1 = 0; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 1; } else if(errF > 0) { voltR = 12.00; voltL = 0.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 0; pin_L2 = 0; GoLog = -1; } else if(errF == 0) { voltR = 12.00; voltL = 12.00; pin_R1 = 1; pin_R2 = 0; pin_L1 = 1; pin_L2 = 0; GoLog = 0; } pwmR = (voltR/12.0); pwmL = (voltL/12.0); //pc.printf(" %d %f %d %d %d %d %d %f\r", err, errL, IRread1, IRread2, IRread3, IRread4, IRread5, GoLog); pc.printf(" %d %f %f\r", err, errL, errF); printf("\n\r"); prv_ang = now_ang; prv_time = now_time; prv_err = err; prv_errF = errF; } }