Diff: main.cpp

Revision:

5:0b555929c5b2

Parent:

4:cd5de3b14797

--- a/main.cpp	Tue Dec 12 07:48:20 2017 +0000
+++ b/main.cpp	Sat Dec 16 10:25:27 2017 +0000
@@ -8,9 +8,34 @@
 #include <string>
 using namespace std;
 
-Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
+#define PI  3.14159265359
+#define RAD_TO_DEG  57.2957795131
+
+#define PULSE_TO_MM 0.1177    //rev/pulse * K_lingkaran_roda
+#define L 144.0           // lengan roda dari pusat robot (mm)
+#define Ts 2.0    // Time Sampling sistem 2ms
+
+#define MAX_SPEED 912.175  //Vresultan max (mm/s)
+#define MAX_W_SPEED 1314.72 //Vw max (mm/s)
+#define SPEED 1             //V robot
+
+#define BOUNDARY_TOLERANCE 70.0
+
+// konstanta PID untuk kendali Posisi (x y)
+#define Kp_s 10.0
+#define Ki_s 0.0
+#define Kd_s 1.6
+
+// konstanta PID untuk kendali arah (theta)
+#define Kp_w 0.2
+#define Ki_w 0.0
+#define Kd_w 0.01
+
 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
+Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
+Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL);
+Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL);
 
 TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
@@ -18,17 +43,41 @@
 encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
 
-    Motor motor3(PB_7, PA_14, PA_15); //motor4
-    Motor motor2(PA_11, PA_6, PA_5); //motor2
-    //Motor motor1(PA_10, PB_5, PB_4);  //motor_griper
-    Motor motor1(PB_6, PA_7, PB_12); //motor 3
-string a;
-    
+Motor motor3(PB_7, PA_14, PA_15); //motor4
+Motor motor2(PA_11, PA_6, PA_5); //motor2
+Motor motor1(PB_6, PA_7, PB_12); //motor 3
+//Motor motor1(PA_10, PB_5, PB_4);  //motor_griper
+
+Serial pc(USBTX,USBRX);
+
 void dataJoystick();
 void lcdPrint();
-void motorP();
+void self_localization();
+void motor_out();
+void PTP_movement();
+
+int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast);
+float moving_direction( float xs, float ys, float x, float y,float theta);
+
+/*------------buruk----------------*/
+float x = 0;
+float y = 0;
+float theta = 0;
 
-Serial pc(USBTX,USBRX);
+float x_prev = 0;
+float y_prev = 0;
+float theta_prev = 0;
+ 
+float vr = 0;
+float vw = 0;
+float a = 0;
+ 
+float Vx = 0;
+float Vy = 0;
+float W = 0;
+
+string str;
+/*---------------------------------------*/
 
 int main()
 {    
@@ -36,69 +85,144 @@
     ps2.init();
     thread1.start(dataJoystick);
     thread2.start(lcdPrint);
-    thread3.start(motorP);
+    thread3.start(self_localization);
+    thread4.start(motor_out);
+    thread5.start(PTP_movement);
+    
     while (1)
     {      
-        // baca input
-/*      
-        ps2.poll();
-        if(ps2.read(PS_PAD::PAD_X)==1)                   a = "silang";
-        else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         a = "lingkaran";
-        else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       a = "segitiga";
-        else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         a = "kotak";
-        else                                             a = "NULL";
- */   
-/*        
-        //tampilkan LCD
-        lcd.locate(0,0);
-        lcd.printf("input joystik :");
-        lcd.locate(0,1);
-        lcd.printf("%s",a); 
-    
-        wait_ms(10);
-        lcd.cls();
-*/
+        //do nothing
     }
 }
 
 void dataJoystick(){
     while(true){
     ps2.poll();
-    if(ps2.read(PS_PAD::PAD_X)==1)                   a =  "silang";
-    else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         a = "lingkaran";
-    else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       a = "segitiga";
-    else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         a = "kotak";
-    else                                             a = "NULL";    
+    if(ps2.read(PS_PAD::PAD_X)==1)                   str =  "silang";
+    else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         str = "lingkaran";
+    else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       str = "segitiga";
+    else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         str = "kotak";
+    else                                             str = "NULL";    
     }
 }
 
 void lcdPrint(){
     while (true){
-    lcd.cls(); 
-    int pulse1 = enc1.getPulses();
-    int pulse2 = enc2.getPulses();
-    int pulse3 = enc3.getPulses();
-    lcd.locate(0,0);
-    lcd.printf("input : %s", a);
-    lcd.locate(0,1);
-    lcd.printf("enc1 = %d", pulse1);
-    lcd.locate(0,2);
-    lcd.printf("enc2 = %d", pulse2);
-    lcd.locate(0,3);
-    lcd.printf("enc3 = %d", pulse3);
-    Thread::wait(20);
+        lcd.cls();         
+        lcd.locate(0,0);
+        lcd.printf("input : %s", str);
+        lcd.locate(0,1);
+        //lcd.printf("Vr = %.2f", sqrt(Vx*Vx + Vy*Vy));
+        lcd.printf("x = %.2f", x);
+        lcd.locate(0,2);
+        lcd.printf("y = %.2f", y);
+        lcd.locate(0,3);
+        lcd.printf("theta = %.2f", theta*RAD_TO_DEG);
+        //lcd.printf("a = %.2f", a*RAD_TO_DEG);
+        
+        Thread::wait(100);
+    }
+}
+
+
+void self_localization(){
+    while(true){       
+        float d1 = enc1.getPulses()*PULSE_TO_MM;
+        float d2 = enc2.getPulses()*PULSE_TO_MM;
+        float d3 = enc3.getPulses()*PULSE_TO_MM;
+         
+        x_prev = x;
+        y_prev = y;
+        theta_prev = theta;
+        
+        x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev);
+        y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev);
+        theta = theta_prev + (d1 + d2 + d3)/(3*L); //     //   0.132629 => 180 / (3. L. pi)
+        
+        Vx = (x - x_prev)/0.002;
+        Vy = (y - y_prev)/0.002;
+        W = (theta - theta_prev)/0.002;
+        
+        enc1.reset();
+        enc2.reset();
+        enc3.reset();
+        
+        Thread::wait(Ts); //frekuensi sampling = 500 Hz
+    }
+}
+
+void PTP_movement(){
+    //mapping lokasi
+    float map_x[16]     = {150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0,  0};
+    float map_y[16]     = {  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150,  0};
+    float map_theta[16] = {  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360};
+    
+    int i=0;
+    
+    while(i < 16){
+        i += calculate_PID(&map_x[i],&map_y[i],&map_theta[i],false);
+        
+        if (i == 16)    i = 0;
+        Thread::wait(Ts);
     }
 }
 
-void motorP() {
-        while(true){
-            motor1.speed(0.5);
-            motor2.speed(0.5);
-            motor3.speed(0.5);
-            Thread::wait(2000);
-            motor1.speed(-0.5);
-            motor2.speed(-0.5);
-            motor3.speed(-0.5);
-            Thread::wait(2000);
-        }  
-}      
\ No newline at end of file
+int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast){
+    // variabel tambahan
+    static float S_error_prev = 0;
+    static float theta_error_prev = 0;
+    
+    static float sum_S_error = 0;
+    static float sum_theta_error = 0;
+    
+    //menghitung error posisi
+    float S_error = sqrt((*x_set-x)*(*x_set-x) + (*y_set-y)*(*y_set-y));
+    //menghitung error arah
+    float theta_error = *theta_set - theta*RAD_TO_DEG;
+    
+    sum_S_error += S_error;
+    sum_theta_error += theta_error;
+     
+    float vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts;
+    float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts;
+    
+    vr = vs/MAX_SPEED*0.5;
+    vw = w*L/MAX_W_SPEED*0.5;
+    a = moving_direction(*x_set,*y_set,x,y,theta);
+    
+    S_error_prev = S_error;
+    theta_error_prev = theta_error;
+    
+    if(isLast == true){
+        if ((abs(*x_set - x) < 20) && (abs(*y_set - y) < 20)){
+               vw = 0;
+               vr = 0;
+                return 1;
+        }
+        else return 0;
+    }
+    else{
+        if ((abs(*x_set - x) < BOUNDARY_TOLERANCE) && (abs(*y_set - y) < BOUNDARY_TOLERANCE))   return 1;
+        else return 0;
+    }      
+}
+
+
+float moving_direction( float xs, float ys, float x, float y,float theta){
+    float temp = atan((ys - y)/(xs - x)) - theta;
+    
+    if (xs < x)    return temp + PI;
+    else            return temp;
+}
+
+
+void motor_out() {      
+        Thread::wait(1500);   
+            
+        while(1){
+           motor1.speed(SPEED*(vr*cos(a) + vw));
+           motor2.speed(SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + vw));
+           motor3.speed(SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + vw));
+        }
+} 
+     
\ No newline at end of file