Diff: main.cpp

Revision:

1:735173a3b218

Parent:

0:22acd37ed695

Child:

2:863436c840bf

--- a/main.cpp	Sat Feb 24 07:57:51 2018 +0000
+++ b/main.cpp	Mon Feb 26 07:29:05 2018 +0000
@@ -56,6 +56,7 @@
 double pid_t=0;
 short sp_teta;
 int sum=0;
+int print_pulse = 0;
 
 int main(){
     encoder_A.reset();
@@ -67,34 +68,61 @@
     pneumatik = 0;
 
     // Thread
-    // thread1.start(getJoystick);
+    thread1.start(getJoystick);
     thread2.start(gerakMotor);
-    //thread3.start(printPulse);
+    thread3.start(printPulse);
     
     while(1){
         // do nothing
-        if(stick.readable() ) {
+         if(stick.readable() ) {
             // Panggil fungsi pembacaan joystik
             stick.baca_data();
             // Panggil fungsi pengolahan data joystik
             stick.olah_data();
 
-            case_gerak();            
+            // Rotasi
+            if (!stick.L1 && stick.R1)        // Pivot Kanan
+                Vw = 0.3;
+            else if (!stick.R1 && stick.L1)   // Pivot Kiri
+                Vw = -0.3;
+            else 
+                Vw = 0;
             
-            pc.printf("Rotasi: %.2f\t", Vw);
-            pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a);
+            // Linier
+            Vr = 0.5;
+            if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
+                a = -90/RAD_TO_DEG; // Maju
+            else if ((!stick.atas)&&(stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
+                a = 90/RAD_TO_DEG; // Mundur
+            else if ((stick.atas)&&(!stick.bawah)&&(!stick.kiri)&&(stick.kanan))
+                a = -135/RAD_TO_DEG; // Serong Atas Kanan
+            else if ((!stick.atas)&&(stick.bawah)&&(!stick.kiri)&&(stick.kanan))
+                a = 135/RAD_TO_DEG; // Serong Bawah Kanan
+            else if ((stick.atas)&&(!stick.bawah)&&(stick.kiri)&&(!stick.kanan))
+                a = -45/RAD_TO_DEG; // Serong Atas Kiri
+            else if ((!stick.atas)&&(stick.bawah)&&(stick.kiri)&&(!stick.kanan))
+                a = 45/RAD_TO_DEG; // Serong Bawah Kiri
+            else if ((!stick.atas)&&(!stick.bawah)&&(stick.kanan)&&(!stick.kiri))
+                a = 180/RAD_TO_DEG; // Kanan
+            else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri))
+                a = 0/RAD_TO_DEG; // Kiri
+            else {
+                Vr = 0;
+                a = 0;
+            }
+            
+            if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
+                pneumatik = !pneumatik; // Silang = Toggle pneumatik
+            
+            //pc.printf("Rotasi: %.2f\t", Vw);
+            //pc.printf("Tarnslasi: %.2f, %.1f\n", Vr, a);
 
-        } else {
-            Vr=0;
-            Vw=0;
-            pc.printf("Masalah Joystick\n");
-        }
-    }
+        } 
+   }
 }
 
 void getJoystick(){
     while(1){
-        
         Thread::wait(1);
     }
 }
@@ -105,18 +133,16 @@
 
 void gerakMotor(){
     while(1){
-        if(Vw != 0){
-            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));
-        } else if (Vw == 0) { // mungkin bisa (a+pid_t)
-            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));
-        } else {
+        if ((Vw == 0) && (Vr == 0)){
            motor1.brake(BRAKE_HIGH);
            motor2.brake(BRAKE_HIGH);
            motor3.brake(BRAKE_HIGH);
+           print_pulse = 0;
+        } else {
+            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));
+            print_pulse = 1;
         } 
         Thread::wait(1);
     }
@@ -127,7 +153,8 @@
         pulse_A = encoder_A.getPulses();
         pulse_B = encoder_B.getPulses();
         pulse_C = encoder_C.getPulses();
-        pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C);    
+        if (print_pulse)
+            pc.printf("%d\t%d\t%d\n", pulse_A, pulse_B, pulse_C);    
         encoder_A.reset();
         encoder_B.reset();
         encoder_C.reset();//*/