main.cpp

00001 
00002 #include "mbed.h"
00003 #include "TextLCD.h"
00004 #include "PS_PAD.h"
00005 #include "Motor.h"
00006 #include "encoderKRAI.h"
00007 
00008 #include <string>
00009 using namespace std;
00010 
00011 #define PI  3.14159265359
00012 #define RAD_TO_DEG  57.2957795131
00013 
00014 #define PULSE_TO_MM 0.1177    //rev/pulse * K_lingkaran_roda
00015 #define L 144.0           // lengan roda dari pusat robot (mm)
00016 #define Ts 2.0    // Time Sampling sistem 2ms
00017 
00018 #define MAX_SPEED 912.175  //Vresultan max (mm/s)
00019 #define MAX_W_SPEED 1314.72 //Vw max (mm/s)
00020 #define SPEED 1
00021 
00022 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
00023 Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
00024 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
00025 Thread thread4(osPriorityNormal, OS_STACK_SIZE, NULL);
00026 Thread thread5(osPriorityNormal, OS_STACK_SIZE, NULL);
00027 
00028 TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
00029 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
00030 encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING);
00031 encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
00032 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
00033 
00034 Motor motor3(PB_7, PA_14, PA_15); //motor4
00035 Motor motor2(PA_11, PA_6, PA_5); //motor2
00036 Motor motor1(PB_6, PA_7, PB_12); //motor 3
00037 //Motor motor1(PA_10, PB_5, PB_4);  //motor_griper
00038 
00039 Serial pc(USBTX,USBRX);
00040 
00041 void dataJoystick();
00042 void lcdPrint();
00043 void self_localization();
00044 void motorP();
00045 void calculate_PID();
00046 
00047 float moving_direction( float xs, float ys, float x, float y,float theta);
00048 
00049 
00050 
00051 /*------------buruk----------------*/
00052 float x = 0;
00053 float y = 0;
00054 float theta = 0;
00055 
00056 float x_prev = 0;
00057 float y_prev = 0;
00058 float theta_prev = 0;
00059  
00060 float vr = 0;
00061 float vw = 0;
00062 float a = 0;
00063  
00064 float Vx = 0;
00065 float Vy = 0;
00066 float W = 0;
00067 
00068 string str;
00069 /*---------------------------------------*/
00070 
00071 int main()
00072 {    
00073     pc.baud(115200);
00074     ps2.init();
00075     thread1.start(dataJoystick);
00076     thread2.start(lcdPrint);
00077     thread3.start(self_localization);
00078     thread4.start(motorP);
00079     thread5.start(calculate_PID);
00080     
00081     while (1)
00082     {      
00083         // baca input
00084 /*      
00085         ps2.poll();
00086         if(ps2.read(PS_PAD::PAD_X)==1)                   a = "silang";
00087         else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         a = "lingkaran";
00088         else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       a = "segitiga";
00089         else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         a = "kotak";
00090         else                                             a = "NULL";
00091  */   
00092 /*        
00093         //tampilkan LCD
00094         lcd.locate(0,0);
00095         lcd.printf("input joystik :");
00096         lcd.locate(0,1);
00097         lcd.printf("%s",a); 
00098     
00099         wait_ms(10);
00100         lcd.cls();
00101 */
00102     }
00103 }
00104 
00105 void dataJoystick(){
00106     while(true){
00107     ps2.poll();
00108     if(ps2.read(PS_PAD::PAD_X)==1)                   str =  "silang";
00109     else if(ps2.read(PS_PAD::PAD_CIRCLE)==1)         str = "lingkaran";
00110     else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1)       str = "segitiga";
00111     else if(ps2.read(PS_PAD::PAD_SQUARE)==1)         str = "kotak";
00112     else                                             str = "NULL";    
00113     }
00114 }
00115 
00116 void lcdPrint(){
00117     while (true){
00118         lcd.cls();         
00119         lcd.locate(0,0);
00120         lcd.printf("input : %s", str);
00121         lcd.locate(0,1);
00122         //lcd.printf("Vr = %.2f", sqrt(Vx*Vx + Vy*Vy));
00123         lcd.printf("x = %.2f", x);
00124         lcd.locate(0,2);
00125         lcd.printf("y = %.2f", y);
00126         lcd.locate(0,3);
00127         lcd.printf("theta = %.2f", theta*RAD_TO_DEG);
00128         //lcd.printf("a = %.2f", a*RAD_TO_DEG);
00129         
00130         Thread::wait(100);
00131     }
00132 }
00133 
00134 
00135 void self_localization(){
00136     while(true){       
00137         float d1 = enc1.getPulses()*PULSE_TO_MM;
00138         float d2 = enc2.getPulses()*PULSE_TO_MM;
00139         float d3 = enc3.getPulses()*PULSE_TO_MM;
00140          
00141         x_prev = x;
00142         y_prev = y;
00143         theta_prev = theta;
00144         
00145         x = x_prev + (2*d1 - d2 - d3)/3*cos(theta_prev) - (-d2+d3)*0.5773*sin(theta_prev);
00146         y = y_prev + (2*d1 - d2 - d3)/3*sin(theta_prev) + (-d2+d3)*0.5773*cos(theta_prev);
00147         theta = theta_prev + (d1 + d2 + d3)/(3*L); //     //   0.132629 => 180 / (3. L. pi)
00148         
00149         Vx = (x - x_prev)/0.002;
00150         Vy = (y - y_prev)/0.002;
00151         W = (theta - theta_prev)/0.002;
00152         
00153         enc1.reset();
00154         enc2.reset();
00155         enc3.reset();
00156         
00157         Thread::wait(Ts); //frekuensi sampling = 500 Hz
00158     }
00159 }
00160 
00161 
00162 void calculate_PID(){
00163     // konstanta PID untuk kendali Posisi (x y)
00164     float Kp_s = 10.0;
00165     float Ki_s = 0.0;
00166     float Kd_s = 1.6;
00167     
00168     // konstanta PID untuk kendali arah (theta)
00169     float Kp_w = 0.2;
00170     float Ki_w = 0.0;
00171     float Kd_w = 0.01;
00172     
00173     // setpoint sistem
00174     float x_set = 400;
00175     float y_set = 700;
00176     float theta_set = 180;
00177     float S_set = sqrt(x_set*x_set + y_set*y_set);
00178     
00179     // variabel tambahan
00180     float S_error_prev = 0;
00181     float theta_error_prev = 0;
00182     
00183     float sum_S_error = 0;
00184     float sum_theta_error = 0;
00185     
00186     while(true){
00187         //menghitung error posisi
00188         //float S = sqrt(x*x + y*y);
00189         //float S_error = S_set - S;
00190         float S_error = sqrt((x_set-x)*(x_set-x) + (y_set-y)*(y_set-y));
00191         //menghitung error arah
00192         float theta_error = theta_set - theta*RAD_TO_DEG;
00193         
00194         sum_S_error += S_error;
00195         sum_theta_error += theta_error;
00196          
00197         float vs = Kp_s*S_error + Ki_s*Ts*sum_S_error + Kd_s*(S_error - S_error_prev)/Ts;
00198         float w = Kp_w*theta_error + Ki_w*Ts*sum_theta_error + Kd_w*(theta_error - theta_error_prev)/Ts;
00199        
00200         vr = vs/MAX_SPEED*0.5;
00201         vw = w*L/MAX_W_SPEED*0.5;
00202         a = moving_direction(x_set,y_set,x,y,theta);
00203         
00204         S_error_prev = S_error;
00205         theta_error_prev = theta_error;
00206                 
00207         Thread::wait(Ts);
00208     }   
00209 }
00210 
00211 
00212 float moving_direction( float xs, float ys, float x, float y,float theta){
00213     float temp = atan((ys - y)/(xs - x)) - theta;
00214     
00215     if (xs < x)    return temp + PI;
00216     else            return temp;
00217 }
00218 
00219 
00220 
00221 void motorP() {      
00222         Thread::wait(1500);   
00223             
00224         while(1){
00225            motor1.speed(SPEED*(vr*cos(a) + vw));
00226            motor2.speed(SPEED*(vr*(-0.5*cos(a) - 0.866*sin(a)) + vw));
00227            motor3.speed(SPEED*(vr*(-0.5*cos(a) + 0.866*sin(a)) + vw));
00228         }
00229 } 
00230