velo_filter
/
newprogram
updatw 08/09/18
Revision 1:087dfa2bff27, committed 2018-09-16
- Comitter:
- k_waihlyan
- Date:
- Sun Sep 16 07:03:07 2018 +0000
- Parent:
- 0:8e7dc5d573f7
- Commit message:
- 16 sept update
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 8e7dc5d573f7 -r 087dfa2bff27 main.cpp --- a/main.cpp Sat Sep 08 09:50:53 2018 +0000 +++ b/main.cpp Sun Sep 16 07:03:07 2018 +0000 @@ -46,7 +46,7 @@ Timer myTime; myTime.reset(); myTime.start(); - pc.baud(19200); + pc.baud(57600); while(1) { IRread1 = IRpin1.read(); IRread2 = IRpin2.read(); @@ -76,23 +76,28 @@ speedDiff = (errL*0.004);// + (0.0001*((err - prv_err)/samp_time)); - voltR = 12.0 + speedDiff; - voltL = 12.0 - speedDiff; + voltR = 12.0 - speedDiff; + voltL = 12.0 + speedDiff; //voltR = voltR*1.0; //voltL = voltL*1.0; - voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau); - voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau); - - if (voltRF>0) { pin_R1 = 1; pin_R2 = 0; } - else { pin_R1 = 0; pin_R2 = 1; } - if (voltLF>0) { pin_L1 = 1; pin_L2 = 0; } - else { pin_L1 = 0; pin_L2 = 1; } + voltRF = ( samp_time * voltR + (tau * prv_voltRF) ) / ( samp_time + tau); + voltLF = ( samp_time * voltL + (tau * prv_voltLF) ) / ( samp_time + tau); + // voltRF = 10.0; + //voltLF = 10.0; + if (voltRF>8) {voltRF = 8;} + else {voltRF = voltRF;} + if (voltLF>8) {voltLF = 8;} + else {voltLF = voltLF;} + + if (voltRF>0) { pin_R1 = 0; pin_R2 = 1; } + else { pin_R1 = 1; pin_R2 = 0; } + if (voltLF>0) { pin_L1 = 0; pin_L2 = 1; } + else { pin_L1 = 1; pin_L2 = 0; } pwmR = (abs(voltRF)/12.0); pwmL = (abs(voltLF)/12.0); - if (pwmR>1) {pwmR = 1;} else {pwmR = pwmR;} if (pwmL>1) {pwmL = 1;} @@ -100,9 +105,11 @@ //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 ); //pc.printf(" %f %f %f\r", err, errL, errF); - pc.printf(" %d %d %f %f %f", des,act,err,now_omg1,now_omg2); + //pc.printf(" %d %d %f %f %f", des,act,err,now_omg1,now_omg2); + pc.printf("%f %f", now_omg1,now_omg2); 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;