Diff: main.cpp

Revision:

0:22acd37ed695

Child:

1:735173a3b218

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 24 07:57:51 2018 +0000
@@ -0,0 +1,172 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "JoystickPS3.h"
+#include "rtos.h"
+#include "pinList.h"
+
+#define PI 3.141592653593
+#define RAD_TO_DEG  57.2957795131
+#define PULSE_TO_MM 0.1177    //rev/pulse * K_lingkaran_roda
+#define L 298.0
+
+#define MOTOR_LIMIT_MAX 1
+#define MOTOR_LIMIT_MIN -1
+
+#define DEBUG   1
+
+// Serial
+Serial pc(USBTX,USBRX);
+joysticknucleo stick(PIN_TX, PIN_RX);
+
+// Pneumatik
+DigitalOut pneumatik(PIN_PNEUMATIK);
+
+// Encoder
+encoderKRAI encoder_A(PIN_A_CHANNEL_A, PIN_A_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder_B(PIN_B_CHANNEL_A, PIN_B_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI encoder_C(PIN_C_CHANNEL_A, PIN_C_CHANNEL_B, 540, encoderKRAI::X4_ENCODING);
+
+// Motor
+Motor motor1(PIN_PWM_A, PIN_FWD_A, PIN_REV_A);   
+Motor motor2(PIN_PWM_B, PIN_FWD_B, PIN_REV_B);   
+Motor motor3(PIN_PWM_C, PIN_FWD_C, PIN_REV_C);   
+
+// List Thread
+Thread thread1;
+Thread thread2;
+Thread thread3;
+//Thread thread4;
+
+// Fungsi dan Prosedur
+void getJoystick();
+void gerakMotor();
+void hitungPID();
+void printPulse();
+void case_gerak();
+
+// Variable-variable
+int joystick;
+int pulse_A=0;
+int pulse_B=0;
+int pulse_C=0;
+float Vr = 0;
+float Vw = 0;
+float a = 0;
+double pid_t=0;
+short sp_teta;
+int sum=0;
+
+int main(){
+    encoder_A.reset();
+    encoder_B.reset();
+    encoder_C.reset();
+    pc.baud(9600);
+    stick.setup();
+    stick.idle();
+    pneumatik = 0;
+
+    // Thread
+    // thread1.start(getJoystick);
+    thread2.start(gerakMotor);
+    //thread3.start(printPulse);
+    
+    while(1){
+        // do nothing
+        if(stick.readable() ) {
+            // Panggil fungsi pembacaan joystik
+            stick.baca_data();
+            // Panggil fungsi pengolahan data joystik
+            stick.olah_data();
+
+            case_gerak();            
+            
+            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);
+    }
+}
+
+void hitungPID(){
+    
+}
+
+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 {
+           motor1.brake(BRAKE_HIGH);
+           motor2.brake(BRAKE_HIGH);
+           motor3.brake(BRAKE_HIGH);
+        } 
+        Thread::wait(1);
+    }
+}
+
+void printPulse(){
+    while(1){
+        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);    
+        encoder_A.reset();
+        encoder_B.reset();
+        encoder_C.reset();//*/
+        Thread::wait(20);
+    }
+}
+
+void 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;
+    
+    // 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;
+    }
+    
+    if ((stick.silang_click)&&(!stick.kotak)&&(!stick.segitiga)&&(!stick.lingkaran))
+        pneumatik = !pneumatik; // Silang = Toggle pneumatik
+            
+}
\ No newline at end of file