Revision 7:f139bb3b2401, committed 2017-12-16

Comitter:

rizqicahyo

Date:

Sat Dec 16 21:08:40 2017 +0000

Parent:

6:f10c0d9f228d

Commit message:

perbaikan dan/atau penambahan tampilan LCD, input joystik, FSM, mode manual dan otomatis, 3 jenis mapping.

Changed in this revision

--- a/main.cpp	Sat Dec 16 11:10:56 2017 +0000
+++ b/main.cpp	Sat Dec 16 21:08:40 2017 +0000
@@ -1,13 +1,9 @@
-
 #include "mbed.h"
 #include "TextLCD.h"
 #include "PS_PAD.h"
 #include "Motor.h"
 #include "encoderKRAI.h"
 
-#include <string>
-using namespace std;
-
 #define PI  3.14159265359
 #define RAD_TO_DEG  57.2957795131
 
@@ -31,6 +27,15 @@
 #define Ki_w 0.0
 #define Kd_w 0.01
 
+//STATE dari Sistem
+#define state_Idle 1
+#define state_ManualControl 2
+#define state_AutoControl 3
+
+#define BUTTON_offAwal 6
+#define BUTTON_onAwal 7
+#define BUTTON_onEnd 8
+
 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
 Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
@@ -50,45 +55,72 @@
 
 Serial pc(USBTX,USBRX);
 
-void dataJoystick();
-void lcdPrint();
-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----------------*/
+/*------------Variabel Global----------------*/
 float x = 0;
 float y = 0;
 float theta = 0;
-
-float x_prev = 0;
-float y_prev = 0;
-float theta_prev = 0;
  
-float vr = 0;
-float vw = 0;
+float Vr = 0;
+float Vw = 0;
 float a = 0;
  
 float Vx = 0;
 float Vy = 0;
 float W = 0;
 
-string str;
-/*---------------------------------------*/
+int state = state_Idle;
+int stateSelect = BUTTON_offAwal;
+int stateStart = BUTTON_offAwal; //state awal
+
+typedef struct map {
+    int n;
+    float x_pos[16];
+    float y_pos[16];
+    float theta_pos[16];
+} mapping;
+
+const mapping map_square =  {16,
+                            {  0,150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0},
+                            {  0,  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150},
+                            {  0,  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270}};
+const mapping map_triangle= {13,
+                            {  0,150,300,450,600,525,450,375,300,225,150, 75,  0},
+                            {  0,  0,  0,  0,  0,130,260,390,519,390,260,130,  0},
+                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}};
+const mapping map_circle =  {16,
+                            {0,114.88,212.17,277.17,300,277,211.83,114.44,0,-115.1,-212.34,-277.26,-300,-277,-212,-114.66},
+                            {0,22.68,87.57,184.78,299.52,414.77,512,577.13,600,577.22,512.25,415,300.23,185.44,88.07,22.96},
+                            {0,22.5,45,67.5, 90,112.5,135,157.5,180,202.5,225,247.5,270,292.5, 315, 337.5}};
+const mapping map_NULL =    {0,
+                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0},
+                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0},
+                            {  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0}};
+mapping map_data;
+
+/*------------- DEKLARASI FUNGSI ----------------*/
+void dataJoystick();
+void lcd_out();
+void self_localization();
+void motor_out();
+void PTP_movement();
+
+void FSM(int input1, int input2, int *state);
+int fsmButton(int input,int *stateButton);
+int calculate_PID(float x_s, float y_s, float theta_s, bool isLast);
+float moving_direction( float x_s, float y_s, float x, float y,float theta);
+/*-----------------------------------------------*/
 
 int main()
 {    
     pc.baud(115200);
     ps2.init();
+    
     thread1.start(dataJoystick);
-    thread2.start(lcdPrint);
+    thread2.start(lcd_out);
     thread3.start(self_localization);
     thread4.start(motor_out);
     thread5.start(PTP_movement);
-    
+       
     while (1)
     {      
         //do nothing
@@ -97,44 +129,152 @@
 
 void dataJoystick(){
     while(true){
-    ps2.poll();
-    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";    
+        ps2.poll();
+        
+        FSM(fsmButton(ps2.read(PS_PAD::PAD_SELECT),&stateSelect),fsmButton(ps2.read(PS_PAD::PAD_START),&stateStart), &state);
+        
+        if(state == state_ManualControl){
+            Vr = 0.5;
+            if(ps2.read(PS_PAD::PAD_LEFT))          a = 180/RAD_TO_DEG;
+            else if(ps2.read(PS_PAD::PAD_BOTTOM))   a = -90/RAD_TO_DEG;
+            else if(ps2.read(PS_PAD::PAD_RIGHT))    a = 0/RAD_TO_DEG;
+            else if(ps2.read(PS_PAD::PAD_TOP))      a = 90/RAD_TO_DEG;
+            else                                    Vr = 0;
+            
+            if(ps2.read(PS_PAD::PAD_L1))          Vw = 0.2;
+            else if(ps2.read(PS_PAD::PAD_R1))     Vw = -0.2;
+            else                                  Vw = 0.0;
+        }
+        else if(state == state_AutoControl){       
+            if(ps2.read(PS_PAD::PAD_X)){
+                map_data = map_NULL;
+                Vr = 0;
+                Vw = 0;
+            }
+            else if(ps2.read(PS_PAD::PAD_SQUARE)){
+                map_data = map_square;
+            }
+            else if(ps2.read(PS_PAD::PAD_TRIANGLE)){
+                map_data = map_triangle;
+            }
+            else if(ps2.read(PS_PAD::PAD_CIRCLE)){
+                map_data = map_circle;
+            }
+                
+        }
+        else if(state == state_Idle){
+        
+        }
+        
+        Thread::wait(1);
     }
 }
 
-void lcdPrint(){
+void FSM(int input1, int input2, int *state){
+    switch(*state){
+        case state_Idle :
+            if (input1 == 1)   *state = state_ManualControl;
+            else if (input2 == 1)   *state = state_AutoControl; 
+        break;
+        case state_ManualControl :
+            if (input1 == 1)    *state = state_Idle;
+        break;
+        case state_AutoControl :
+            if (input2 == 1)    *state = state_Idle;
+        break; 
+    }    
+}
+
+int fsmButton(int input,int *stateButton)
+{
+  switch (*stateButton)
+  {
+    case BUTTON_offAwal :
+      if (input ==  1)
+        *stateButton = BUTTON_onAwal;
+      else
+        *stateButton = BUTTON_offAwal;
+       return 0;
+    //break;
+    case BUTTON_onAwal :
+      if(input == 0)
+        *stateButton = BUTTON_onEnd;
+      else
+        *stateButton = BUTTON_onAwal;
+      return 0;
+    //break;
+    case BUTTON_onEnd :
+       *stateButton = BUTTON_offAwal;
+       return 1;
+    //break;
+  }
+}
+
+void lcd_out(){
+    lcd.locate(0,0);
+    lcd.printf("Sistem Pergerakan");
+    lcd.locate(0,1);
+    lcd.printf("Objek dengan 3 Roda");
+    lcd.locate(0,2);
+    lcd.printf("Berbasis Odometry"); 
+    Thread::wait(1500);
+    
+    lcd.cls();         
+    lcd.locate(0,0);
+    lcd.printf("Bryan Christy P");
+    lcd.locate(0,1);
+    lcd.printf("   13214073");
+    lcd.locate(0,2);
+    lcd.printf("Rizqi Cahyo Y"); 
+    lcd.locate(0,3);
+    lcd.printf("   13214090");
+    Thread::wait(1500);  
     while (true){
-        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);
+        lcd.cls();
+        if (state == state_ManualControl){
+            lcd.locate(0,0);
+            lcd.printf("Mode : Manual");
+            lcd.locate(0,1);
+            lcd.printf("Vx = %.2f", Vx);
+            lcd.locate(0,2);
+            lcd.printf("Vy = %.2f", Vy);
+            lcd.locate(0,3);
+            lcd.printf("W = %.2f", W*RAD_TO_DEG);
+        }
+        else if (state == state_AutoControl){
+            lcd.locate(0,0);
+            lcd.printf("Mode : Autonomous");
+            lcd.locate(0,1);
+            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);
+        } 
+        else if(state == state_Idle){
+            lcd.locate(0,0);
+            lcd.printf("PAD_START");
+            lcd.locate(0,1);
+            lcd.printf(" => Mode Auto");
+            lcd.locate(0,2);
+            lcd.printf("PAD_SELECT");
+            lcd.locate(0,3);
+            lcd.printf(" => Mode Manual");
+        }  
+        Thread::wait(50);
     }
 }
 
-
 void self_localization(){
+    static float x_prev = 0;
+    static float y_prev = 0;
+    static float theta_prev = 0;
+    
     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)
@@ -143,92 +283,100 @@
         Vy = (y - y_prev)/0.002;
         W = (theta - theta_prev)/0.002;
         
+        x_prev = x;
+        y_prev = y;
+        theta_prev = theta;
+        
         enc1.reset();
         enc2.reset();
         enc3.reset();
         
+        if((state == state_AutoControl) && (map_data.n == 0)){
+            x_prev = 0;
+            y_prev = 0;
+            theta_prev = 0;
+        }        
         Thread::wait(Ts); //frekuensi sampling = 500 Hz
     }
 }
 
 void PTP_movement(){
-    //mapping lokasi
-    float map_x[16]     = {  0,150,300,450,600,600,600,600,600,450,300,150,  0,  0,  0,  0};
-    float map_y[16]     = {  0,  0,  0,  0,  0,150,300,450,600,600,600,600,600,450,300,150};
-    //float map_theta[16] = {  0,  0,  0, 90, 90, 90, 90,180,180,180,180,270,270,270,270,360};
-    float thet=0;
-    float thet_prev = 0;
-    int i=0;
-
-    while(i < 16){
-        thet = thet_prev + 90*(i/4);
-        i += calculate_PID(&map_x[i],&map_y[i],&thet,false);
-        
-        if (i == 16){
+    int i = 0;
+    while(true){
+        if(state == state_AutoControl){
+            while(i < map_data.n){
+                i += calculate_PID(map_data.x_pos[i],map_data.y_pos[i],map_data.theta_pos[i],(i==(map_data.n-1)));
+                //if (i == map.n)    i = 0;
+                Thread::wait(Ts);
+            }
             i = 0;
-            thet_prev = thet;
         }
-
+        else    i = 0;
         Thread::wait(Ts);
     }
 }
 
-int calculate_PID(float *x_set, float *y_set, float *theta_set, bool isLast){
-    // variabel tambahan
+int calculate_PID(float x_s, float y_s, float theta_s, bool isLast){
+    //error_prev & sum_error
     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 jarak x,y terhaadap xs,ys
+    float S_error = sqrt((x_s-x)*(x_s-x) + (y_s-y)*(y_s-y));
     //menghitung error arah
-    float theta_error = *theta_set - theta*RAD_TO_DEG;
+    float theta_error = theta_s - 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 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);
+    Vr = Vs/MAX_SPEED*0.5;
+    Vw = w*L/MAX_W_SPEED*0.5;
+    a = moving_direction(x_s,y_s,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;
+        if ((abs(x_s - x) < 20) && (abs(y_s - 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;
+        if ((abs(x_s - x) < BOUNDARY_TOLERANCE) && (abs(y_s - 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;
+float moving_direction( float x_s, float y_s, float x, float y,float theta){
+    float temp = atan((y_s - y)/(x_s - x)) - theta;
     
-    if (xs < x)    return temp + PI;
+    if (x_s < 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));
+    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));
+        
+        if((Vr == 0) && (Vw == 0)){
+           motor1.brake(BRAKE_HIGH);
+           motor2.brake(BRAKE_HIGH);
+           motor3.brake(BRAKE_HIGH);
         }
+    }
 } 
      
\ No newline at end of file