main.cpp

00001 #include "mbed.h"
00002 #include "JoystickPS3.h"
00003 #include "Motor.h"
00004 #include "encoderKRAI.h"
00005 #include "millis.h"
00006 
00007 #define PI 3.14159265
00008 #define D_ENCODER 10        // Diameter Roda Encoder
00009 #define D_ROBOT1 54         // Diameter Roda Robot dari kiri ke kanan
00010 #define D_ROBOT2 79         // Diameter Roda Robot dari depan ke belakang
00011 
00012 bool auto_rotate=false, flag_start=true, flag_buttonL1, flag_buttonR1, flag_select;
00013 bool reset_encoder = true;
00014 
00015 int mode=0;
00016 
00017 float V_x = 0.4, V_y = 0.3, V_pivot = 0.2;
00018 float rasio= (D_ROBOT2/D_ROBOT1);
00019 
00020 float K_enc = PI*D_ENCODER;
00021 float K_robot1 = PI*D_ROBOT1;
00022 float K_robot2 = PI*D_ROBOT2;
00023 
00024 float PIDSpeedX, PIDSpeedY, PIDSpeedT;
00025 float speedDpn, speedBlk, speedR, speedL;
00026 const float maxPIDSpeed = 0.3, minPIDSpeed = -0.3, Ts = 50;
00027 const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
00028 double current_error, previous_error1 = 0, previous_error2 = 0;
00029 unsigned long int previousMillis=0;
00030 float setpointX=0.0, setpointY=0.0, setpointT=0.0;
00031 
00032 /* Untuk PID */
00033 float errorX, previous_errorX=0, derivativeX, integralX=0;
00034 float errorY, previous_errorY=0, derivativeY, integralY=0;
00035 float errorT, previous_errorT=0, derivativeT, integralT=0;
00036 float KpX=0.11, KiX=0, KdX=0;
00037 float KpY=0.11, KiY=0, KdY=0;
00038 float KpT=0.08, KiT=0, KdT=3.33;
00039 
00040 /* Untuk joystick */
00041 int case_joy;
00042 
00043 /* Inisialisasi  Pin TX-RX Joystik dan PC */
00044 joysticknucleo joystick(PA_0,PA_1);
00045 Serial pc(USBTX,USBRX);
00046 
00047 /* Deklarasi Encoder Base */
00048 encoderKRAI encKanan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
00049 encoderKRAI encBlk( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
00050 encoderKRAI encDpn( PC_11, PC_10, 28, encoderKRAI::X4_ENCODING);
00051 encoderKRAI encKiri( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
00052 
00053 /* Deklarasi Motor Base */
00054 Motor motorDpn(PB_7, PC_3, PC_0);
00055 Motor motorBlk(PB_2, PB_15, PB_1);
00056 Motor motorL  (PB_9, PA_12, PA_6);  
00057 Motor motorR  (PB_8, PC_6, PC_5);
00058 
00059 void setCenter()
00060 {
00061 /* Fungsi untuk menentukan center dari robot */
00062     encDpn.reset();
00063     encBlk.reset();
00064     encKiri.reset();
00065     encKanan.reset();
00066 }
00067 
00068 int case_joystick()
00069 {    
00070     int caseJoystick;
00071     if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00072         // Pivot Kanan
00073         caseJoystick = 1;
00074     } 
00075     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00076         // Pivot Kiri
00077         caseJoystick = 2;
00078     }
00079     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00080         // Kanan + Rotasi kanan
00081         caseJoystick = 3;
00082     } 
00083     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00084         // Kanan + Rotasi kiri
00085         caseJoystick = 4;
00086     }
00087     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00088         // Kiri + Rotasi kanan
00089         caseJoystick = 5;
00090     } 
00091     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00092         // Kiri + Rotasi kiri
00093         caseJoystick = 6;
00094     }
00095     else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00096         // Maju + Rotasi kanan
00097         caseJoystick = 7;
00098     } 
00099     else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00100         // Maju + Rotasi kiri
00101         caseJoystick = 8;
00102     }
00103     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00104         // Mundur + Rotasi kanan
00105         caseJoystick = 9;
00106     } 
00107     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00108         // Mundur + Rotasi kiri
00109         caseJoystick = 10;
00110     }
00111     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00112         // Kanan
00113         caseJoystick = 11;
00114     }
00115     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00116         // Kiri
00117         caseJoystick = 12;
00118     }
00119     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00120         // Maju
00121         caseJoystick = 13;
00122     }
00123     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00124         // Mundur
00125         caseJoystick = 14;
00126     }
00127     
00128     return(caseJoystick);
00129 }
00130 
00131 void aktuator()
00132 {
00133     switch (case_joy) {
00134         case (1):        
00135             // Pivot Kanan
00136             motorDpn.speed(-V_pivot/rasio);
00137             motorBlk.speed(-V_pivot/rasio);
00138             motorR.speed(-V_pivot);
00139             motorL.speed(-V_pivot);
00140             break;
00141         case (2): 
00142             // Pivot Kiri
00143             motorDpn.speed(V_pivot/rasio);
00144             motorBlk.speed(V_pivot/rasio);
00145             motorR.speed(V_pivot);
00146             motorL.speed(V_pivot);
00147             break;
00148         case (11): 
00149             // Kanan
00150             /*speedDpn = -(V_x + PIDSpeedT);
00151             speedBlk = (V_x + PIDSpeedT);
00152             speedR   = rasio*PIDSpeedT + PIDSpeedY;
00153             speedL   = rasio*PIDSpeedT - PIDSpeedY;*/
00154             speedDpn = -(V_x);
00155             speedBlk = (V_x);
00156             //speedR   = PIDSpeedY;
00157             //speedL   = - PIDSpeedY;
00158             motorDpn.speed(speedDpn);
00159             motorBlk.speed(speedBlk);
00160             motorR.brake(1);
00161             motorL.brake(1);
00162             break;
00163         case (12): 
00164             // Kiri
00165             /*speedDpn = (V_x + PIDSpeedT);
00166             speedBlk = -(V_x + PIDSpeedT);
00167             speedR   = rasio*PIDSpeedT + PIDSpeedY;
00168             speedL   = rasio*PIDSpeedT - PIDSpeedY;*/
00169             speedDpn = (V_x);
00170             speedBlk = -(V_x);
00171             //speedR   = PIDSpeedY;
00172             //speedL   = -PIDSpeedY;
00173             motorDpn.speed(speedDpn);
00174             motorBlk.speed(speedBlk);
00175             motorR.brake(1);
00176             motorL.brake(1);
00177             break;
00178         case (13): 
00179             // Maju
00180             motorDpn.brake(1);
00181             motorBlk.brake(1);
00182             motorR.speed(0.3);
00183             motorL.speed(-0.3);
00184             break;
00185         case (14): 
00186             // Mundur
00187             motorDpn.brake(1);
00188             motorBlk.brake(1);
00189             motorR.speed(-0.3);
00190             motorL.speed(0.3);
00191             break;
00192         default : 
00193             motorDpn.brake(1);
00194             motorBlk.brake(1);
00195             motorR.brake(1);
00196             motorL.brake(1);
00197             break;
00198     } // End Switch
00199 }
00200 
00201 float getX()
00202 {
00203     float  jarakEncDpn, jarakEncBlk;
00204     jarakEncDpn = (encDpn.getPulses())/(float)(540.0)*K_enc;
00205     jarakEncBlk = (encBlk.getPulses())/(float)(540.0)*K_enc;
00206     return (jarakEncBlk-jarakEncDpn)/2;   
00207 }
00208 
00209 float getY()
00210 {
00211     float  jarakEncKir, jarakEncKan;
00212     jarakEncKir = (encKiri.getPulses())/(float)(540.0)*K_enc;
00213     jarakEncKan = (encKanan.getPulses())/(float)(540.0)*K_enc;
00214     return (jarakEncKan-jarakEncKir)/2;   
00215 }
00216 
00217 float getTetha()
00218 {
00219     float busurDpn, busurBlk, busurKir, busurKan, sudut;
00220     busurKir = ((encKiri.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
00221     busurKan = ((encKanan.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
00222     busurDpn = ((encDpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
00223     busurBlk = ((encBlk.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
00224     sudut = (busurDpn+busurBlk+busurKir+busurKan)/4;
00225     if (sudut>=360.0 || sudut<=-360.0)
00226     {
00227         sudut = 0.0;   
00228     }
00229     return sudut;    
00230 }
00231 
00232 void PIDcompute()
00233 {   
00234     errorX = setpointX - getX();
00235     errorY = setpointY - getY();
00236     errorT = setpointT - getTetha();
00237     
00238     integralX = integralX + errorX*Ts;
00239     integralY = integralY + errorY*Ts;
00240     integralT = integralT + errorT*Ts;
00241     
00242     derivativeX = (errorX - previous_errorX)/Ts;
00243     derivativeY = (errorY - previous_errorY)/Ts;
00244     derivativeT = (errorT - previous_errorT)/Ts;
00245     
00246     PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
00247     PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
00248     PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
00249     
00250     previous_errorX = errorX;
00251     previous_errorY = errorY;
00252     previous_errorT = errorT;
00253     
00254     if(PIDSpeedX > maxPIDSpeed){
00255         PIDSpeedX = maxPIDSpeed;
00256     }
00257     else if (PIDSpeedX < minPIDSpeed){
00258         PIDSpeedX = minPIDSpeed;
00259     }
00260     
00261     if(PIDSpeedY > maxPIDSpeed){
00262         PIDSpeedY = maxPIDSpeed;
00263     }
00264     else if (PIDSpeedY < minPIDSpeed){
00265         PIDSpeedY = minPIDSpeed;
00266     }
00267     
00268     if(PIDSpeedT > maxPIDSpeedT){
00269         PIDSpeedT = maxPIDSpeedT;
00270     }
00271     else if (PIDSpeedT < minPIDSpeedT){
00272         PIDSpeedT = minPIDSpeedT;
00273     }
00274 }
00275 
00276 void aturSetpoint()
00277 {
00278     switch (mode) 
00279     {
00280         case (-2):        
00281             setpointT = -45.0;
00282             break;
00283         case (-1): 
00284             setpointT = -20.0;
00285             break;
00286         case (0):        
00287             setpointT = 0.0;
00288             break;
00289         case (1): 
00290             setpointT = 20.0;
00291             break;
00292         case (2): 
00293             setpointT = 45.0;
00294             break;
00295     }
00296 }
00297 void cekMode()
00298 {
00299     if (mode<-2){mode = -2;}
00300     else if (mode>2){mode = 2;}   
00301 }
00302 
00303 void case_autoRotate()
00304 {
00305     if ((joystick.R1_click)&&(!joystick.L1_click)&&flag_buttonR1)  
00306     {  
00307         flag_buttonR1 = false;
00308         mode--;
00309         cekMode();
00310         pc.printf("masuk R1, mode = %d\n",mode);
00311     }
00312     else { flag_buttonR1 = true; } 
00313     
00314     if ((!joystick.R1_click)&&(joystick.L1_click)&&flag_buttonL1)
00315     {  
00316         flag_buttonL1 = false;
00317         mode++;
00318         cekMode();
00319         pc.printf("masuk L1, mode = %d\n",mode);
00320     }
00321     else { flag_buttonL1 = true; }
00322     
00323     if ((joystick.START_click)&&(!joystick.SELECT_click)&&flag_start)
00324     {   
00325         // Masuk auto rotate
00326         flag_start = false;
00327         auto_rotate = true;
00328         flag_buttonR1 = true;
00329         flag_buttonL1 = true;
00330         flag_select = true;
00331         pc.printf("MASUK AUTO ROTATE");     
00332     }
00333     else { flag_start = true; }
00334     
00335     if ((!joystick.START_click)&&(joystick.SELECT_click)&&flag_select)
00336     {   
00337         // Keluar auto rotate
00338         flag_select = false;
00339         auto_rotate = false;
00340         reset_encoder = true;
00341         setpointT = 0.0;
00342         mode = 0; 
00343         pc.printf("KELUAR AUTO ROTATE");      
00344     }
00345     else { flag_select = true; }
00346 }
00347 
00348 int main(void)
00349 {
00350     joystick.setup();
00351     pc.baud(115200);
00352     wait_ms(1000);
00353     startMillis();
00354     while(1)
00355     {   
00356         
00357         //COBA ROTASI
00358         joystick.idle();        
00359         if(joystick.readable()) 
00360         {
00361             // Panggil fungsi pembacaan joystik
00362             joystick.baca_data();
00363             // Panggil fungsi pengolahan data joystik
00364             joystick.olah_data();
00365             // Masuk ke case joystick
00366             case_joy = case_joystick();
00367             aktuator();
00368             
00369             case_autoRotate();
00370 
00371             while(auto_rotate)
00372             {
00373                 joystick.idle();        
00374                 if(joystick.readable()) 
00375                 {                    
00376                     joystick.baca_data();
00377                     joystick.olah_data();
00378                     
00379                     if (reset_encoder){ setCenter(); reset_encoder = false; }
00380                     
00381                     case_autoRotate();
00382                     aturSetpoint();
00383                     
00384                     while (millis()-previousMillis>=Ts)
00385                     { PIDcompute(); previousMillis = millis(); }
00386                     
00387                     //speedDpn = PIDSpeedT/rasio - PIDSpeedX;
00388                     //speedBlk = PIDSpeedT/rasio + PIDSpeedX;
00389                     //speedR   = PIDSpeedT + PIDSpeedY;
00390                     //speedL   = PIDSpeedT - PIDSpeedY;
00391                     
00392                     speedDpn = PIDSpeedT;
00393                     speedBlk = PIDSpeedT;
00394                     speedR   = PIDSpeedT*rasio;
00395                     speedL   = PIDSpeedT*rasio;
00396                     
00397                     if ((errorT > 0.5) || (errorT<-0.5))
00398                     {
00399                         motorDpn.speed(speedDpn);
00400                         motorBlk.speed(speedBlk);
00401                         motorR.speed(speedR);
00402                         motorL.speed(speedL);    
00403                     }
00404                     else
00405                     {
00406                         motorDpn.brake(1);
00407                         motorBlk.brake(1);
00408                         motorR.brake(1);
00409                         motorL.brake(1);    
00410                     }
00411                     pc.printf("SUDUT = %f\n",getTetha());
00412                     case_autoRotate();
00413                 }
00414                 else
00415                 {
00416                     joystick.idle();
00417                 }
00418             }
00419         }
00420         else
00421         {
00422             joystick.idle();
00423         }
00424     }
00425 }