Diff: main.cpp

Revision:

2:863436c840bf

Parent:

1:735173a3b218

Child:

3:b1403fcdaeb1

--- a/main.cpp	Mon Feb 26 07:29:05 2018 +0000
+++ b/main.cpp	Thu Mar 01 06:39:48 2018 +0000
@@ -2,13 +2,23 @@
 #include "Motor.h"
 #include "encoderKRAI.h"
 #include "JoystickPS3.h"
-#include "rtos.h"
 #include "pinList.h"
+#include "millis.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 MAX_W_SPEED 15000           //max angular speed of robot
+
+#define TOLERANCET 1.2              //theta tolerance
+#define PULSE_TO_JARAK 0.581776     //kll roda / pulses
+#define L 298.0                     //roda to center of robot
+#define TS 2.0                      //time sampling
+#define LIMITPWM 0.4                //limit pwm motor
+
+//Konstanta PID Sudut
+#define KP_W 5.0
+#define KI_W 0.02
+#define KD_W 10.0
 
 #define MOTOR_LIMIT_MAX 1
 #define MOTOR_LIMIT_MIN -1
@@ -32,144 +42,140 @@
 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 hitungPID(float theta_s);
 void printPulse();
 void case_gerak();
 
 // Variable-variable
 int joystick;
-int pulse_A=0;
-int pulse_B=0;
-int pulse_C=0;
+float pulse_A=0;
+float pulse_B=0;
+float pulse_C=0;
 float Vr = 0;
 float Vw = 0;
 float a = 0;
-double pid_t=0;
-short sp_teta;
-int sum=0;
-int print_pulse = 0;
+float w = 0;
+float theta_s = 0;
+float theta = 0;
+float theta_prev = 0;
+float theta_error_prev = 0;
+float sum_theta_error = 0;
+float theta_error;
+unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
+bool print_pulse = 0;
 
 int main(){
     encoder_A.reset();
     encoder_B.reset();
     encoder_C.reset();
-    pc.baud(9600);
+    pc.baud(115200);
     stick.setup();
     stick.idle();
     pneumatik = 0;
-
-    // Thread
-    thread1.start(getJoystick);
-    thread2.start(gerakMotor);
-    thread3.start(printPulse);
+    startMillis();
     
     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();
-
-            // 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;
+            // Ambil data joystick
+            case_gerak();
+    
+            gerakMotor();
             
-            // 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 ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) ){
+                hitungPID(theta_s);
+                last_mt_pid = millis();
             }
-            
-            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);
-
-        } 
+            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET){
+                pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
+                pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
+                pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
+                
+                //Compute value
+                theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
+                
+                //Update value
+                theta_prev = theta;
+                
+                encoder_A.reset();
+                encoder_B.reset();
+                encoder_C.reset();
+                Vw = 0;
+            }
+            if (millis() - last_mt_print > TS+5){
+                if (print_pulse && DEBUG)
+                    printPulse();
+                last_mt_print = millis();
+            }
+        }
    }
 }
 
-void getJoystick(){
-    while(1){
-        Thread::wait(1);
+void hitungPID(float theta_s){
+    pulse_A = encoder_A.getPulses()*PULSE_TO_JARAK;
+    pulse_B = encoder_B.getPulses()*PULSE_TO_JARAK;
+    pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
+
+    //Compute value
+    theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
+    
+    //Update value
+    theta_prev = theta;
+
+    encoder_A.reset();
+    encoder_B.reset();
+    encoder_C.reset();
+    
+    //theta_s = theta_s/RAD_TO_DEG;
+    //menghitung error jarak x,y terhaadap xs,ys
+    theta_error = theta_s - (theta*RAD_TO_DEG);
+    sum_theta_error += theta_error;
+
+    //kalkulasi PID Theta
+    w = KP_W*theta_error + KI_W*TS*sum_theta_error + KD_W*(theta_error - theta_error_prev)/TS;
+    Vw = (w*L/MAX_W_SPEED)*LIMITPWM;
+
+    //update
+    theta_error_prev = theta_error;
+    //saturasi vw
+    if (Vw > 0.3){
+        Vw = 0.3;
     }
-}
-
-void hitungPID(){
-    
+    else if ( Vw < -0.3){
+        Vw = -0.3;
+    }
 }
 
 void gerakMotor(){
-    while(1){
-        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);
-    }
+    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;
+    } 
 }
 
 void printPulse(){
-    while(1){
-        pulse_A = encoder_A.getPulses();
-        pulse_B = encoder_B.getPulses();
-        pulse_C = encoder_C.getPulses();
-        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();//*/
-        Thread::wait(20);
-    }
+        pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, theta*RAD_TO_DEG, theta_s);    
 }
 
 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;
+    if (!stick.L1_click && stick.R1_click)        // Pivot Kanan
+        theta_s += 10;
+    else if (!stick.R1_click && stick.L1_click)   // Pivot Kiri
+        theta_s -= 10;
     
     // Linier
     Vr = 0.5;
@@ -191,9 +197,9 @@
         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
-            
 }
\ No newline at end of file