Diff: main.cpp

Revision:

4:7a7a8aa33fd5

Parent:

3:43d4cb3ece1b

Child:

5:256f6eac0358

--- a/main.cpp	Tue Apr 26 14:57:35 2016 +0000
+++ b/main.cpp	Wed Apr 27 14:28:05 2016 +0000
@@ -6,6 +6,7 @@
 #include "PS_PAD.h"
 #include "esc.h"
 #include "Servo.h"
+#include "Ping.h"
 
 /*********************************************************************************************/
 /**     DEKLARASI INPUT OUTPUT                                                              **/
@@ -22,9 +23,9 @@
 Motor motor2(PA_11, PA_6, PA_5);
 Motor motor3(PA_9, PC_2, PC_3);
 Motor motor4(PA_10, PB_5, PB_4);
-Motor motorS(PB_7, PA_14, PA_15);
+Motor motorC2(PB_7, PA_14, PA_15);
 Motor motorC1(PB_9, PA_12, PC_5);
-Motor motorC2(PB_8, PB_1, PA_13);
+Motor motorS(PB_8, PB_1, PA_13);
 //Motor motor4(PB_6, PA_7, PB_12);
 
 //pnuematik
@@ -53,7 +54,8 @@
 //Timer
 Ticker timer;
 
-
+//Sensor Ping
+Ping ping(PB_2);
 
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI                                                       **/
@@ -70,9 +72,9 @@
 const float tuning4 = 0.04;
 
 const float driver0 = 1;
-const float driver1 = 0.85;
-const float driver2 = 0.65;
-const float driver3 = 0.2;
+const float driver1 = 0.82;
+const float driver2 = 0.6;
+const float driver3 = 0.15;
 
 
 // inisialisasi pwm awal servo
@@ -93,13 +95,16 @@
 int over;
 int g_flag;
 
-///////
+
 void aktuator(int f);
 void edf_servo();
+
 void init_sensor();
 void linetracer(float speed);
 void flag_sensor();
 
+float read_jarak();
+
 /*********************************************************************************************/
 /*********************************************************************************************/
 /**     PROGRAM UTAMA                                                                       **/
@@ -129,8 +134,8 @@
     //inisiasi pnuematik
     pnuematik1 = 1;
     pnuematik2 = 1;
-    pnuematik3 = 0;
-    pnuematik4 = 0;
+    pnuematik3 = 1;
+    pnuematik4 = 1;
     
     //inisisasi TIMER
     timer.attach(&flag_sensor,0.0005);
@@ -139,7 +144,8 @@
     //kondisi robot
     bool manual=true; 
     bool pool= false;
- 
+    
+    //running manual
     while(manual){
         
         ps2.poll();
@@ -147,24 +153,28 @@
         aktuator(field);
         edf_servo();
         
-        if(ps2.read(PS_PAD::ANALOG_LEFT)==1)    manual = false;
+        if(limit4==0)    manual = false;
         
     }
     
+    //running otomatis
     timer.attach(&flag_sensor,0.0005);
     
     while(!pool){
         init_sensor();
         
-        if(vcurr > 0.3)  vcurr = (float) 0.3;
+        if(vcurr > 0.4)  vcurr = (float) 0.4;
+        //else if (vcurr < 0.2)   vcurr = (float) 0.2;
       
         linetracer(vcurr);
         //laser = 1;
             
-        vcurr+=ax;
+        if((limit3==0) || (read_jarak() <= 9.0))   pool=true;
         
-        if(limit3==0)   pool=true;
-        
+        //if(read_jarak() <= 45.0)        vcurr -= ax;
+        //else                        vcurr += ax;
+        vcurr += ax;
+    }
     }
     motor1.brake(1);
     motor2.brake(1);
@@ -175,9 +185,9 @@
         
     pnuematik1=0;
     wait_ms(1500);
-
+    
     while(limit4!=0){
-        motorC1.speed(1);
+        motorC1.speed(0.95);
         motorC2.speed(-1);
     }
     
@@ -504,7 +514,7 @@
         if(ps2.read(PS_PAD::PAD_X)==1){
             //PWM ++
             pwm += 0.0007;
-            if( pwm > 0.7)  pwm = 0.8;
+            if( pwm > 0.8)  pwm = 0.8;
             pc.printf("gaspol \n");
         }
         else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
@@ -534,9 +544,6 @@
             
             sudut = 0;
             pwm = 0.22;
-            
-            pnuematik3 = 1;
-            pnuematik4 = 1;
         }
         
         
@@ -545,8 +552,8 @@
         edf.pulse();
 }
 
+/////////////////////////////////////////LINE FOLLOWER/////////////////////////
 
-/////////////////LINE FOLLOWER/////////////////////////
 void init_sensor(){
     char data;
     if(com.readable()){  
@@ -651,10 +658,10 @@
             }
         } 
         
-        motor1.speed((float)-(speed1-0.0));
-        motor2.speed((float)-(speed2-0.04));
-        motor3.speed((float)-(speed3-0.0));
-        motor4.speed((float)-(speed4-0.0));
+        motor1.speed(-(float)(speed1-0.04));
+        motor2.speed(-(float)(speed2-0.04));
+        motor3.speed(-(float)(speed3-0.0));
+        motor4.speed(-(float)(speed4-0.0));
 
 }
 
@@ -666,4 +673,15 @@
     else if(datasensor[4] == 1)                         g_flag = 5;
     else if(datasensor[0] == 1)                         g_flag = 1;
     else if(datasensor[5] == 1)                         g_flag = 6;
+}
+
+
+////////////////////////SENSOR PING///////////////////////////////////////
+float read_jarak() {
+    float jarak;
+    
+    ping.Send();
+    wait_ms(45);
+    jarak = ping.Read_cm()/2;
+    return jarak;
 }
\ No newline at end of file