Diff: main.cpp

Revision:

30:d69cc27ac644

Parent:

29:7b372b0aaa61

Child:

31:d5cbda07fd95

--- a/main.cpp	Sun Feb 12 08:07:04 2017 +0000
+++ b/main.cpp	Sun Feb 12 11:19:48 2017 +0000
@@ -19,18 +19,18 @@
 /****************************************************************************/
 /*                                                                          */
 /*  Joystick                                                                */
-/*  Kanan => Posisi target x ditambah 'perpindahan'                         */
-/*  Kiri  => Posisi target x dikurang 'perpindahan'                         */
+/*  Kanan =>                                                                */
+/*  Kiri  =>                                                                */
 /*                                                                          */
-/*  Tombol silang   => Kembali keposisi Awal                                */
-/*  Tombol segitiga => Aktif motor Launcher                                 */
-/*  Tombol lingkaran=> Aktif servo Launcher                                 */
-/*  Tombol L1       => Pivot Kiri                                           */
-/*  Tombol R1       => Pivot Kanan                                          */
-/*  Tombol L3       => PWM Motor Belakang Dikurangi                         */
-/*  Tombol R3       => PWM Motor Belakang Ditambah                          */
-/*  Tombol L2       => PWM Motor Depan Dikurangi                            */
-/*  Tombol R2       => PWM Motor Depan Ditambah                             */
+/*  Tombol silang    => Power Screw Aktif                                   */
+/*  Tombol segitiga  => Aktif motor Launcher                                */
+/*  Tombol lingkaran => Aktif Pneumatik Launcher                            */
+/*  Tombol L1        => Pivot Kiri                                          */
+/*  Tombol R1        => Pivot Kanan                                         */
+/*  Tombol L3        => PWM Motor Belakang Dikurangi                        */
+/*  Tombol R3        => PWM Motor Belakang Ditambah                         */
+/*  Tombol L2        => PWM Motor Depan Dikurangi                           */
+/*  Tombol R2        => PWM Motor Depan Ditambah                            */
 /*                                                                          */
 /*  Bismillahirahmanirrahim                                                 */
 /*  Jagalah Kebersihan Kodingan                                             */
@@ -62,6 +62,7 @@
 double last_error2 = 0, current_error2, sum_error2 = 0;
 float rpm, target_rpm = 2.0;
 float rpm2, target_rpm2 = 4.0;
+bool henti=false, hentis=false; 
  
 // Variable Bawah
 float Vt;
@@ -77,8 +78,8 @@
 encoderKRAI encoderKiri(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
 
 /* Deklarasi Encoder Launcher */
-encoderKRAI encoderAtas( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
-encoderKRAI encoderAtas2( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encoderAtas2( PC_10, PC_11, 28, encoderKRAI::X2_ENCODING);
+encoderKRAI encoderAtas( PD_2, PC_12, 28, encoderKRAI::X2_ENCODING);
 
 /* Deklarasi Motor Base */
 Motor motor1(PB_9, PA_12, PC_5);    // pwm, fwd, rev, Motor Depan
@@ -93,9 +94,9 @@
 DigitalOut pneumatik(PB_3, PullUp);
 
 /*Dekalrasi Limit Switch */
-DigitalIn limitA (PC_9, PullUp);
+DigitalIn limitA(PC_9, PullUp);
 DigitalIn limitT(PB_10, PullUp);
-DigitalIn limitB (PC_8, PullUp);
+DigitalIn limitB(PC_8, PullUp);
 
 /**
  *  posX dan posY berdasarkan arah robot
@@ -179,11 +180,14 @@
     if (reload){
         if (!limitA){
             motorP.brake (1);
-            wait_ms(1000);
+            if (hentis != henti){
+                wait_ms(1500);           
+            }
+            hentis = henti;
         }
         else
         {
-            motorP.speed(pwmP);
+            henti = false;
             if (!limitT)
             {
                 while (limitB) 
@@ -192,8 +196,16 @@
                 }
                 motorP.brake(1);
                 reload = !reload;
+            } 
+            else
+            {
+                 motorP.speed(pwmP); 
             }
         }
+    } 
+    else
+    {
+            motorP.brake (1);
     }
     
 /*Motor Atas*/
@@ -369,7 +381,8 @@
             }
             if (joystick.lingkaran_click){
                 pneumatik = 0;
-                wait_ms(1000);
+                henti = true;
+                wait_ms(700);
                 pneumatik = 1;
             }
             speedKalibrasiMotor();