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