Diff: main.cpp

Revision:

0:96a0fbdff740

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Feb 18 12:54:07 2019 +0000
@@ -0,0 +1,159 @@
+#include "JoystickPS3.h"
+#include "Motor.h"
+#include "mbed.h"
+/* Inisialisasi  Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
+/* Deklarasi Motor Base */
+Motor motorDepan(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 
+Motor motorKanan(PB_3, PA_10, PC_4);
+Motor motorKiri  (PB_9, PA_6, PA_12);
+int case_joy;
+/****************************************************/
+/*         Deklarasi Fungsi dan Procedure           */
+/****************************************************/
+int case_joystick()
+{
+//---------------------------------------------------//
+//  Gerak Motor Base                                 //
+//   Case 1  : Pivot kanan                           //
+//   Case 2  : Pivot Kiri                            //
+//   Case 3  : Maju                                  //
+//   Case 4  : Kiri                                  //
+//   Case 5  : Bawah                                 //
+//   Case 6  : Kanan                                 //
+//---------------------------------------------------//
+    
+    int caseJoystick;
+    if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Pivot Kanan
+        caseJoystick = 1;
+    } 
+    else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Pivot Kiri
+        caseJoystick = 2;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Pivot Kiri
+        caseJoystick = 3;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
+        // Pivot Kiri
+        caseJoystick = 4;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
+        // Pivot Kiri
+        caseJoystick = 5;
+    }
+    else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
+        // Pivot Kiri
+        caseJoystick = 6;
+    }
+	    
+    return(caseJoystick);
+}
+
+void aktuator()
+{
+    switch (case_joy) {
+        case (1): 
+        {       
+            // Pivot Kanan
+			motorDepan.speed(0.23);
+			motorKanan.speed(0.17);
+			motorKiri.speed(0.23);
+            break;
+        }
+        case (2): 
+        {
+            // Pivot Kiri
+			motorDepan.speed(-0.23);
+			motorKanan.speed(-0.17);
+			motorKiri.speed(-0.23);
+            break;
+        }
+        case (3): 
+        {
+            // Maju
+			motorDepan.speed(0);
+			motorKanan.speed(-0.17);
+			motorKiri.speed(0.3);
+            break;
+        }
+        case (4): 
+        {
+            // Kiri
+			motorDepan.speed(-0.23);
+			motorKanan.speed(0.17);
+			motorKiri.speed(0.3);
+            break;
+        }
+        case (5): 
+        {
+            //Belakang
+			motorDepan.speed(0);
+			motorKanan.speed(0.17);
+			motorKiri.speed(-0.3);
+            break;
+        }
+        case (6): 
+        {
+            // Kanan
+			motorDepan.speed(0.23);
+			motorKanan.speed(-0.17);
+			motorKiri.speed(-0.3);
+            break;
+        }
+		default : 
+        {
+            motorDepan.brake(1);
+            motorKanan.brake(1);
+            motorKiri.brake(1);
+        }
+	}
+}
+/*********************************************************/
+/*                  Main Function                        */
+/*********************************************************/
+
+int main (void)
+{
+    // Set baud rate - 115200
+    joystick.setup();
+    pc.baud(115200);
+    wait_ms(1000);
+//    startMillis();
+    while(1)
+    {   
+        // Interrupt Serial
+        joystick.idle();        
+        if(joystick.readable()) 
+        {
+            // Panggil fungsi pembacaan joystik
+            joystick.baca_data();
+            // Panggil fungsi pengolahan data joystik
+            joystick.olah_data();
+            // Masuk ke case joystick
+            case_joy = case_joystick();
+            //pc.printf("%d\n",case_joy);
+            aktuator();
+        }
+        else
+        {
+			joystick.idle();
+        }
+        
+//        if(millis() - previousMillis5 >= 400)
+//        {    
+//            display.write(0,((int) rpm2) / 10);
+//            display.write(1,((int)rpm2) % 10);
+//            display.write(2, (int)target_rpm2 / 10);
+//            display.write(3, (int)target_rpm2 % 10);
+//            display.setColon(true);
+//            
+//            previousMillis5 = millis();
+//        }
+		
+    }
+	
+}
\ No newline at end of file