Diff: main.cpp

Revision:

1:679511199e94

Parent:

0:1d81f3296468

diff -r 1d81f3296468 -r 679511199e94 main.cpp
--- 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");