Diff: main.cpp

Revision:

40:5b937cac959a

Parent:

39:11358f3f61ff

Child:

41:336a19289c2d

--- a/main.cpp	Sat Feb 18 00:37:33 2017 +0000
+++ b/main.cpp	Sat Feb 18 03:24:05 2017 +0000
@@ -41,10 +41,9 @@
 #include "mbed.h"
 #include "JoystickPS3.h"
 #include "Motor.h"
-#include "Servo.h"
 #include "encoderKRAI.h"
 #include "millis.h"
-
+#include "Ping.h"
 /***********************************************/
 /*          Konstanta dan Variabel             */
 /***********************************************/
@@ -67,6 +66,8 @@
 
 const float pwmPowerUp        = 0.7;
 const float pwmPowerDown      = -0.9;
+
+float jarak_ping=0;
  
 // Variable Bawah
 float Vt;
@@ -87,6 +88,7 @@
 unsigned long int previousMillis2 = 0;  // PID launcher
 unsigned long int currentMillis2;
 unsigned long int previousMillis3 = 0;  // Pneumatik
+unsigned long int previousMillis4 = 0;  // Ping
 
 /* Variabel Stick */
 //Logic untuk masuk aktuator
@@ -132,10 +134,12 @@
 DigitalOut pneumatik(PB_3, PullUp);
 
 /*Dekalrasi Limit Switch */
-DigitalIn infraAtas(PC_9, PullUp);
+//DigitalIn infraAtas(PC_9, PullUp);
 DigitalIn limitTengah(PB_10, PullUp);
 DigitalIn limitBawah(PC_8, PullUp);
 
+/*deklarasi PING ultrasonic*/
+Ping pingAtas(PC_9);
 
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
@@ -227,21 +231,6 @@
         errorP = 0;
     return (float)Kp*errorP;    
 }
-/*
-void init_speed (){
-    if (speed>maxSpeed){
-        speed = maxSpeed;
-    }
-    if (speed<minSpeed){
-        speed = minSpeed;
-    }
-    if (speed2>maxSpeed){
-        speed2 = maxSpeed;
-    }
-    if (speed2<minSpeed){
-        speed2 = minSpeed;
-    }
-}*/
 
 void setCenter(){
 // Fungsi untuk menentukan center dari robot
@@ -348,8 +337,8 @@
         {
             // Power Screw Up
             //powerScrew.speed(pwmPowerUp);
-            //ReloadOn = !ReloadOn;
-            powerScrew.speed(pwmPowerUp);
+            ReloadOn = !ReloadOn;
+            //powerScrew.speed(pwmPowerUp);
             break;
         }
         case (12) : 
@@ -362,7 +351,7 @@
         {
             motorDpn.brake(1);
             motorBlk.brake(1);
-            if(isReload){
+           /* if(isReload){
                 powerScrew.speed(pwmPowerDown);
                 if(!limitBawah){
                     isReload = false;
@@ -374,12 +363,12 @@
             }
             else{
                 powerScrew.brake(1);
-            }
+            }*/
         }   
     } // End Switch
  }
  
-/*void reloader()
+void reloader()
 {
     if(ReloadOn){
         if(isReload){
@@ -392,17 +381,20 @@
         else if(!limitTengah){
             isReload = true;
         }
-        else if(!infraAtas){
-            powerScrew.brake(1);
+        else if(jarak_ping > 4){
+            powerScrew.speed(pwmPowerUp);
+        }
+        else if(jarak_ping < 3) {
+            powerScrew.speed(pwmPowerDown);
         }
         else{
-            powerScrew.speed(pwmPowerUp);
+            powerScrew.brake(1);
         }
     }
     else{
         powerScrew.brake(1);
     }
-}*/
+}
  
  
 void launcher()
@@ -478,12 +470,26 @@
     joystick.setup();
     pc.baud(115200);
     wait_ms(1000);
+    
+    // initializing encoder
     setCenter();
     wait_ms(500);
+    
+    //initializing PING
+    pingAtas.Send();
+    
     pc.printf("ready....");
     startMillis();
     while(1)
-    {
+    {   
+        // interupsi pembacaan PING setiap 30 ms
+        if(millis() - previousMillis4 >= 30){
+            jarak_ping = pingAtas.Read_cm()/2;
+            
+            pingAtas.Send();
+            previousMillis4 = millis();
+        }
+        
         // Interrupt Serial
         joystick.idle();        
         if(joystick.readable()) 
@@ -496,7 +502,7 @@
             case_joy = case_joystick();
             aktuator();
             launcher();
-            //reloader();
+            reloader();
             if ((millis()-previousMillis3 >= 425)&&(flag_Pneu)){
                 pneumatik = 1;
                 flag_Pneu = false;