Diff: main.cpp

Revision:

7:8d8eb4676356

Parent:

6:bb7e29420efd

Child:

8:8072ee4f1740

--- a/main.cpp	Thu Mar 08 09:24:57 2018 +0000
+++ b/main.cpp	Fri Mar 23 17:09:28 2018 +0000
@@ -39,7 +39,7 @@
 #define MOTOR_LIMIT_MIN -1
 
 // Set 1 untuk aktifkan fitur pc.print
-#define DEBUG   1
+#define DEBUG   0
 
 ////////////////////////////////////////////////////////////////////////////////
 // Object Program                                                               //
@@ -49,7 +49,9 @@
 joysticknucleo stick(PIN_TX, PIN_RX);
 
 // Pneumatik
-DigitalOut pneumatik(PIN_PNEUMATIK);
+DigitalOut pneumatik[3]{PIN_PNEUMATIK_1, PIN_PNEUMATIK_2, PIN_PNEUMATIK_3};
+//DigitalOut pneumatik2(PIN_PNEUMATIK_2);
+//DigitalOut pneumatik3(PIN_PNEUMATIK_3);
 
 // Encoder
 encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
@@ -74,6 +76,7 @@
 // Variable-variable                                                          //
 ////////////////////////////////////////////////////////////////////////////////
 int joystick;
+int pn = 0;
 double pulse_A=0, pulse_B=0, pulse_C=0;
 double Vr = 0, Vr_max = 0;
 double Vw = 0;
@@ -94,7 +97,9 @@
     encoder_A.reset();
     encoder_B.reset();
     encoder_C.reset();
-    pneumatik = 0;
+    for (int i = 0; i<3; i++){
+        pneumatik[i] = 0;
+    }
     startMillis();
     stick.setup();
     stick.idle();
@@ -182,27 +187,18 @@
 
 void gerakMotor(){
     if ((Vw == 0) && (Vr_max == 0)){
-        // Deselerasi
-        if (Vr >= 0.05){
-            if (millis() - last_mt_desel > 70){
-                Vr -= 0.1;
-                last_mt_desel = millis();
-            }
-        } // Rem
-        else {
-            motor1.brake(BRAKE_HIGH);
-            motor2.brake(BRAKE_HIGH);
-            motor3.brake(BRAKE_HIGH);
-            
-            // Pengaturan variable saat berhenti
-            print_pulse = 0;
-            Vr = 0;
-        }
+        motor1.brake(BRAKE_HIGH);
+        motor2.brake(BRAKE_HIGH);
+        motor3.brake(BRAKE_HIGH);
+        
+        // Pengaturan variable saat berhenti
+        print_pulse = 0;
+        Vr = 0;
     } else {
         // Akselerasi
         if ((millis() - last_mt_aksel > 150) && Vr < Vr_max){
-            if (Vr < 0.275) 
-                Vr = 0.275;
+            if (Vr < 0.35) 
+                Vr = 0.35;
             else
                 Vr += 0.05;
             last_mt_aksel = millis();
@@ -212,9 +208,9 @@
             Vr = Vr_max;
         
         // Control Motor
-        motor1.speed((-1*Vr*cos(a) + Vw));
-        motor2.speed((Vr*(0.5*cos(a) + 0.866*sin(a)) + Vw));
-        motor3.speed((Vr*(0.5*cos(a) - 0.866*sin(a)) + Vw));
+        motor1.speed((Vr*cos(a) + Vw));
+        motor2.speed((Vr*(-0.5*cos(a) - 0.866*sin(a)) + Vw));
+        motor3.speed((Vr*(-0.5*cos(a) + 0.866*sin(a)) + Vw));
         print_pulse = 1;
     } 
 }
@@ -303,9 +299,25 @@
     }
     
     // Control Pneumatic
-    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
-        pneumatik = !pneumatik; 
-    if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran))
-        pneumatik_2 = !pneumatik_2; 
+    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran)){
+        pneumatik[pn]   = 1; 
+        pn++;
+        if (pn > 2) pn = 2;
+    }
+    if ((!stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(stick.lingkaran)){
+        pneumatik[pn]   = 0; 
+        pn--;
+        if (pn < 0) pn = 0;
+    }
+    if ((!stick.silang_click)&&(!stick.kotak)&&(stick.segitiga)&&(!stick.lingkaran))
+        for (int i = 0; i<3; i++){
+            pneumatik[i] = 0;
+            pn = 0;
+        }
+    if ((!stick.silang_click)&&(stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
+        for (int i = 0; i<3; i++){
+            pneumatik[i] = 1;
+            pn = 0;
+        }
 
 }
\ No newline at end of file