Diff: main.cpp

Revision:

3:b1403fcdaeb1

Parent:

2:863436c840bf

Child:

4:3c890389e256

Child:

5:4a70c53d7f86

--- a/main.cpp	Thu Mar 01 06:39:48 2018 +0000
+++ b/main.cpp	Thu Mar 01 11:02:03 2018 +0000
@@ -9,16 +9,16 @@
 #define RAD_TO_DEG  57.2957795131
 #define MAX_W_SPEED 15000           //max angular speed of robot
 
-#define TOLERANCET 1.2              //theta tolerance
+#define TOLERANCET 0.8              //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 KP_W 1.0
+#define KI_W 0.0065
+#define KD_W 125
 
 #define MOTOR_LIMIT_MAX 1
 #define MOTOR_LIMIT_MIN -1
@@ -47,6 +47,7 @@
 void hitungPID(float theta_s);
 void printPulse();
 void case_gerak();
+float compute_Alpha(float x_s, float y_s, float x, float y,float theta);
 
 // Variable-variable
 int joystick;
@@ -57,6 +58,12 @@
 float Vw = 0;
 float a = 0;
 float w = 0;
+float x =0;
+float x_s = 0;
+float y =0;
+float y_s = 0;
+float x_prev=0;
+float y_prev=0;
 float theta_s = 0;
 float theta = 0;
 float theta_prev = 0;
@@ -65,6 +72,7 @@
 float theta_error;
 unsigned long last_mt_print, last_mt_pid, last_mt_rotasi;
 bool print_pulse = 0;
+bool modeauto = 0;
 
 int main(){
     encoder_A.reset();
@@ -88,25 +96,30 @@
     
             gerakMotor();
             
-            if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) ){
+            if ( (millis() - last_mt_pid > TS) && !(fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET) && modeauto ){
                 hitungPID(theta_s);
                 last_mt_pid = millis();
             }
-            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET){
+            if (fabs(theta_s - (theta*RAD_TO_DEG))<TOLERANCET || !modeauto){
                 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);
+                x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
+                y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
                 
                 //Update value
                 theta_prev = theta;
+                x_prev = x;
+                y_prev = y;
                 
                 encoder_A.reset();
                 encoder_B.reset();
                 encoder_C.reset();
-                Vw = 0;
+                
+                if(modeauto) Vw = 0;
             }
             if (millis() - last_mt_print > TS+5){
                 if (print_pulse && DEBUG)
@@ -123,9 +136,13 @@
     pulse_C = encoder_C.getPulses()*PULSE_TO_JARAK;
 
     //Compute value
+    x = x_prev + (2*pulse_A - pulse_C - pulse_B)/3*cos(theta_prev) - (-pulse_C+pulse_B)*0.5773*sin(theta_prev);
+    y = y_prev + (2*pulse_A - pulse_C - pulse_B)/3*sin(theta_prev) + (-pulse_C+pulse_B)*0.5773*cos(theta_prev);
     theta = theta_prev + (pulse_A + pulse_C + pulse_B)/(3.0*L);
     
     //Update value
+    x_prev = x;
+    y_prev = y;
     theta_prev = theta;
 
     encoder_A.reset();
@@ -139,16 +156,16 @@
 
     //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;
+    Vw += (w*L/MAX_W_SPEED)*LIMITPWM;
 
     //update
     theta_error_prev = theta_error;
     //saturasi vw
-    if (Vw > 0.3){
-        Vw = 0.3;
+    if (Vw > 0.2){
+        Vw = 0.2;
     }
-    else if ( Vw < -0.3){
-        Vw = -0.3;
+    else if ( Vw < -0.2){
+        Vw = -0.2;
     }
 }
 
@@ -159,6 +176,7 @@
        motor3.brake(BRAKE_HIGH);
        print_pulse = 0;
     } else {
+        //a = compute_Alpha(x_s, y_s, x, y, theta);
         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));
@@ -167,34 +185,87 @@
 }
 
 void printPulse(){
-        pc.printf("%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n", pulse_A, pulse_B, pulse_C, theta*RAD_TO_DEG, theta_s);    
+        pc.printf("%.2f\t%.2f\n", theta*RAD_TO_DEG, theta_s);    
+}
+
+float compute_Alpha(float x_s, float y_s, float x, float y,float theta){
+//fungsi untuk menghitung alpha sebagai arah gerak robot
+    float temp = atan((y_s - y)/(x_s - x)) - theta;
+    
+    if (x_s < x)    return temp + PI;
+    else            return temp;
 }
 
 void case_gerak(){
     // Rotasi
-    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;
+    if(modeauto){
+        if (!stick.L1 && stick.R1)        // Pivot Kanan
+            theta_s += 0.15;
+        else if (!stick.R1 && stick.L1)   // Pivot Kiri
+            theta_s -= 0.15;
+
+    }
+    else if(!modeauto){
+        if (!stick.L1 && stick.R1)        // Pivot Kanan
+            Vw = 0.3;
+        else if (!stick.R1 && stick.L1)   // Pivot Kiri
+            Vw = -0.3;
+        else
+            Vw = 0.0;
+    }
+    
+    if(stick.START_click){
+        modeauto = !modeauto;
+        theta = 0.0;
+        theta_prev = 0.0;
+        theta_s = 0;
+        theta_error_prev = 0;
+        sum_theta_error = 0;
+        theta_error;
+    }
     
     // Linier
     Vr = 0.5;
-    if ((stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(!stick.kiri))
+    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))
+    //    x_s = 0;// Maju
+    //    y_s = 10000;
+    }
+    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))
+    //    x_s = 0; // Mundur
+    //    y_s = -10000;
+    }
+    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))
+    //    x_s = 50000; // Maju+Kanan
+    //    y_s = 50000;
+    }
+    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))
+    //    x_s = 50000; // Mundur+Kanan
+    //    y_s = -50000;
+    }
+    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))
+    //    x_s = 50000; // Maju+Kiri
+    //    y_s = -50000;
+    }
+    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))
+    //    x_s = -50000; // Mundur+Kiri
+    //    y_s = -50000;
+    }
+    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))
+    //    x_s = 50000; // Kanan
+    //    y_s = 0;
+    }
+    else if ((!stick.atas)&&(!stick.bawah)&&(!stick.kanan)&&(stick.kiri)){
         a = 0/RAD_TO_DEG; // Kiri
+    //    x_s = -50000; // Kiri
+    //    y_s = 0;
+    }
     else {
         Vr = 0;
         a = 0;