KATAPANG
/
PID
bla
Revision 1:679511199e94, committed 2018-10-31
- Comitter:
- arayhan878
- Date:
- Wed Oct 31 06:29:36 2018 +0000
- Parent:
- 0:1d81f3296468
- Commit message:
- baruu;
Changed in this revision
SRF05.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SRF05.lib Wed Oct 31 06:29:36 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/SRF05/#e758665e072c
--- a/main.cpp Wed Aug 29 15:59:31 2018 +0000 +++ b/main.cpp Wed Oct 31 06:29:36 2018 +0000 @@ -1,4 +1,7 @@ #include "mbed.h" +#include "SRF05.h" + +SRF05 srf(PB_5, PB_3); ///////////////////////PID/////////////////////// int out, e, integral, derivative, preError, output; @@ -129,45 +132,72 @@ { ch1 = bt.getc(); ch = ch1 - 90; - if(ch != -34) + printf("Distance = %.1f\n", srf.read()); + + if(ch != -34) // bola ada di kamera tp tdk di tengah { - pc.printf("yapppp "); - pc.printf(" sudut : %d",ch); + pc.printf("yapppp "); + pc.printf(" sudut : %d",ch); -//////////////////////////////////PID/////////////////////////////// -e = setpoint - ch; -integral = integral + (e * Dt); -derivative = (e - preError) / Dt; -output = (Kp * e) + (Ki * integral) + (Kd * derivative); -preError = e; -//////////////////////////////////PID/////////////////////////////// - out = output*-1; - atur(0, 0, out); - //pc.printf(" pw1 : %f",pw1); - //pc.printf(" pw2 : %f",pw2); - //pc.printf(" pw3 : %f",pw3); - - //pc.printf(" I : %d",integral); - //pc.printf(" D : %d",derivative); - //pc.printf(" error : %d",e); - //pc.printf(" pwm : %d",out); - pc.printf(" rod1 = %0.2f ", roda1 ); - pc.printf(" rod2 = %0.2f ", roda2 ); - pc.printf(" rod3 = %0.2f\n ", roda3 ); - pwm1.write(pw1); - pwm2.write(pw2); - pwm3.write(pw3); + //////////////////////////////////PID/////////////////////////////// + e = setpoint - ch; + integral = integral + (e * Dt); + derivative = (e - preError) / Dt; + output = (Kp * e) + (Ki * integral) + (Kd * derivative); + preError = e; + //////////////////////////////////PID/////////////////////////////// + out = output*-1; + atur(0, 0, out); + //pc.printf(" pw1 : %f",pw1); + //pc.printf(" pw2 : %f",pw2); + //pc.printf(" pw3 : %f",pw3); + //pc.printf(" I : %d",integral); + //pc.printf(" D : %d",derivative); + //pc.printf(" error : %d",e); + //pc.printf(" pwm : %d",out); + pc.printf(" rod1 = %0.2f ", roda1 ); + pc.printf(" rod2 = %0.2f ", roda2 ); + pc.printf(" rod3 = %0.2f\n ", roda3 ); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + + if(e==0) //bola sudah di tengah + { + atur(0, 0, 0); //stop + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + wait(0.5f); + atur(0, -50, 0); //maju samperin bola + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } } - else + if((ch == -34)&&(srf.read()>5)) //bola tdk ada di kamera dan ultra tdk mendeteksi apa2 { - pc.printf("nope \n"); + pc.printf("nope \n"); + atur(0, 0, -45); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } + if((ch == -34)&&(srf.read()<5)) + { + atur(0, 0, 0); + pwm1.write(pw1); + pwm2.write(pw2); + pwm3.write(pw3); + } + } + else + { atur(0, 0, 0); pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); - } } - //atur(0, 150, 0); //mundur //pc.printf("Mundur");