Diff: main.cpp

Revision:

5:34be90fa6d27

Parent:

4:65d65a108b68

Child:

6:0e159860e5c6

--- a/main.cpp	Thu Feb 18 09:18:39 2016 +0000
+++ b/main.cpp	Fri Feb 19 12:49:34 2016 +0000
@@ -1,17 +1,3 @@
-/*********************************************************************************************/
-/** GARUDAGO-ITB (KRAI2016)                                                                 **/
-/** #ROADTOBANGKOK!                                                                         **/
-/**                                                                                         **/
-/** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS                                                 **/
-/**                                                                                         **/
-/**                                                                                         **/
-/** Created by :                                                                            **/
-/** Rizqi Cahyo Yuwono                                                                      **/
-/** EL'14 - 13214090                                                                        **/
-/**                                                                                         **/
-/** Last Update : 19 Desember 2015, 06.10 WIB                                               **/
-/*********************************************************************************************/
-
 /*********************************************************************************************/
 /**     FILE HEADER                                                                         **/
 /*********************************************************************************************/
@@ -66,8 +52,11 @@
 
 DigitalOut pnuematik_lengan(PC_10);
 DigitalOut pnuematik_gripper(PC_11);
+DigitalOut pnuematik_atas(PD_2);
+DigitalOut pnuematik_bawah(PC_12);
 
 Servo servo_gripper(PB_9);
+//Deklarasi Input Limit Switch
 
 /*
 // Sensor
@@ -87,7 +76,7 @@
 DigitalOut calibrate(PA_15);
 */
 
-DigitalIn button(USER_BUTTON);
+//DigitalIn button(USER_BUTTON);
 
 
 //bool sensor[13];
@@ -107,6 +96,9 @@
 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
 float gTuning = 0.16;
 
+float gripper_up = 1;
+float gripper_down = 0.2;
+
 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
 unsigned char gCase=0;  //variabel keadaan proses
 
@@ -114,7 +106,8 @@
 int over=0;
 int lapangan = 1;
 
-
+bool state_atas = 0;
+bool state_bawah = 0;
   
 /*********************************************************************************************/
 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
@@ -127,6 +120,7 @@
 /*********************************************************************************************/
 /*********************************************************************************************/
 
+    
 void ambil(int c){
     switch (c)
     {
@@ -134,7 +128,7 @@
             servo_gripper.position(170);
             pc.printf("grip lost");
             wait_ms(25);
-            pnuematik_lengan=1;
+            pnuematik_lengan=0;
             wait_ms(500);
             
         break;
@@ -147,7 +141,7 @@
             wait_ms(1000); 
             pnuematik_gripper=1;
             wait_ms(800);
-            pnuematik_lengan=0;
+            pnuematik_lengan=1;
             wait_ms(200);
         break;
     }
@@ -201,8 +195,10 @@
     float speed;
     
     servo_gripper.position(140);
-    pnuematik_lengan=0;
+    pnuematik_lengan=1;
     pnuematik_gripper=1;
+    pnuematik_atas = state_atas;
+    pnuematik_bawah = state_bawah;
     
     while(1)
     {  
@@ -220,7 +216,7 @@
             //MOTOR BELAKANG
             speed = gMax_speed;
                         
-            motor1.speed(-(speed-gTuning));
+            motor1.speed(-speed);
             motor2.speed(-speed);
             pc.printf("mundur \n");
         }
@@ -279,12 +275,22 @@
         
         if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
             //POWER WINDOW ATAS
-            gripper.speed(1);
+            
+            gripper.speed(gripper_up);
             pc.printf("up \n");
         }
         else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
-            //POWER WINDOW BAWAH
-            gripper.speed(-0.2);
+            //POWER WINDOW BAWAH 
+            if ((state_atas==1) || (state_bawah==1))
+            {
+                gripper_down = 1;
+            }
+            else
+            {
+                gripper_down = 0.2;
+            }
+                       
+            gripper.speed(-gripper_down);
             pc.printf("down \n");
         }
         else
@@ -316,13 +322,30 @@
             sudut += 3;
             pc.printf("servo max \n");
         }
+        
+        //gripper pole
+        
+        if ((ps2.read(PS_PAD::PAD_L2)==1))
+        {
                 
+                state_atas = !state_atas;
+                pnuematik_atas= state_atas;
+                wait_ms(300);
+        }
+        else if ((ps2.read(PS_PAD::PAD_R2)==1))
+        {
+                state_bawah = !state_bawah;
+                pnuematik_bawah = state_bawah;
+                wait_ms(300);
+        }
+        
+                        
         init_servo(lapangan);
         init_pwm();
         myservo.position(sudut);
         wait_ms(25);
         //edf.setThrottle(pwm);
         //edf.pulse();
-        //wait_ms(25);
+        //wait_ms(25); 
     }
 }
\ No newline at end of file