main.cpp

00001 
00002 #include "mbed.h"
00003 #include "PS_PAD.h"
00004 #include "Motor.h"
00005 
00006 PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //mosi,miso,clk,ss
00007 Serial pc(USBTX,USBRX);
00008 Serial com(A0,A1);
00009 
00010 #define kec_max 0.5        // konstanta kecepatan maksimum
00011 #define kec_pivot 0.8   // konstanta  kecepatan pivot maksimum
00012 #define ax 0.1       // konstanta perceptan
00013 #define kec_gripper_naik 0.85
00014 #define kec_gripper_turun 0.15
00015 //Deklarasi pin motor
00016 //Motor NAME (PinName pwm, PinName fwd, PinName rev):
00017 
00018     Motor gripper(PB_6, PB_8, PB_9);
00019     Motor motor3(PC_6, PC_8, PC_9);
00020     Motor motor2(PB_3, PB_5, PB_4);
00021     Motor motor1(PA_8, PA_9, PC_7);
00022 
00023 bool maju=false;
00024 bool mundur=false;
00025 bool pivki=false;
00026 bool pivka=false;
00027 bool naik = false;
00028 bool turun = false;
00029 bool stop = true;
00030 char case_jalan, prev_case_jalan;
00031 
00032 
00033 // variabel kecepatan motor
00034 double kec_motor;
00035 
00036 void prosedurmotor()
00037 {
00038     double koef;
00039     double s1=0,s2=0;
00040     //Kondisi Motor 
00041     switch (case_jalan) 
00042     {
00043         case (1): // pivot kanan
00044             {       
00045                 if (pivka)
00046                 {
00047                     
00048                     if(kec_motor<0.1) 
00049                     {
00050                         kec_motor=0.1;
00051                     } 
00052                     else 
00053                     {
00054                         kec_motor+=ax;
00055                     }
00056                     
00057                 //perlambatan=0;
00058                 }
00059                 else 
00060                 { 
00061                     //perlambatan=1;
00062                 } 
00063             if (kec_motor>=kec_pivot) 
00064             {
00065                 kec_motor=kec_pivot; 
00066             }
00067             //mode lambat dan cepat pada tombol R2 dan L2
00068             if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
00069             {
00070                 koef=2;
00071             } 
00072             else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
00073             {
00074                 koef=0.5;
00075             }
00076             else 
00077             { 
00078                 koef=1;
00079             }
00080             
00081             s1 = (float)(-0.5*koef*kec_motor);
00082             s2 = (float)(0.5*koef*kec_motor);
00083               
00084             pivka=true;         
00085             maju=mundur=pivki=naik=turun=false;
00086             
00087             pc.printf("\rPivot Kanan Cuy\n");
00088             
00089             motor1.speed(s1);
00090             motor2.speed(s2);
00091             gripper.brake(1);
00092              
00093             break;
00094         }
00095     case (2): // pivot kiri
00096            {
00097             if (pivki)
00098             {
00099                 if(kec_motor<0.1) 
00100                 {
00101                     kec_motor=0.1;
00102                 } 
00103                 else 
00104                 {
00105                     kec_motor+=ax;
00106                 }
00107                 //perlambatan=0;
00108             } 
00109             else 
00110             { 
00111                 //perlambatan=1;
00112             } 
00113             
00114             if (kec_motor>=kec_pivot) 
00115             {
00116                 kec_motor=kec_pivot; 
00117             }
00118             //mode lambat dan cepat pada tombol R2 dan L2
00119             if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
00120             {
00121                 koef=2;
00122             } else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
00123             {
00124                 koef=0.5;
00125             } 
00126             else 
00127             {
00128                 koef=1;
00129             }
00130             
00131             s1 = (float)( 0.5*koef*kec_motor);
00132             s2 = (float)(-0.5*koef*kec_motor);
00133   
00134             
00135             pivki=true; 
00136             maju=mundur=pivka=naik=turun=false;
00137             
00138             pc.printf("\rPivot Kiri Cuy\n");
00139             
00140             motor1.speed(s1);
00141             motor2.speed(s2);
00142             gripper.brake(1);  
00143          
00144     
00145             break;
00146            }
00147     case (3): // maju
00148         {   
00149             if (maju) 
00150             {
00151                 if(kec_motor<0.1) 
00152                 {
00153                     kec_motor=0.1;
00154                 } 
00155                 else 
00156                 {
00157                     kec_motor+=ax;
00158                 }
00159                 //perlambatan=0;
00160             } 
00161             else 
00162             {
00163                 //perlambatan=1;
00164             } 
00165             
00166             if (kec_motor>=kec_max) 
00167             { 
00168                 kec_motor=kec_max; 
00169             }
00170             //mode lambat dan cepat pada tombol R2 dan L2
00171             if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
00172             {
00173                 koef=2;
00174             }  
00175             else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))  
00176             { 
00177                 koef=0.5;
00178             }
00179             else 
00180             {
00181                 koef=1;  
00182             }
00183             
00184             //Case s1 untuk mode L2 lebih lambat
00185             s1 = (float)(1*koef*(kec_motor));            
00186             s2 = (float)(1*koef*(kec_motor)); 
00187 
00188             
00189             maju=true;             
00190             mundur=pivka=pivki=naik=turun=false;
00191  
00192             pc.printf("\rMaju Cuy\n");
00193  
00194             motor1.speed(s1);
00195             motor2.speed(s2);
00196             gripper.brake(1);
00197         
00198     
00199             break;
00200         }
00201     case (4): // mundur
00202         { 
00203             if (mundur) 
00204             {
00205                 if(kec_motor<0.1) 
00206                 {
00207                     kec_motor=0.1;
00208                 } 
00209                 else 
00210                 {
00211                     kec_motor+=ax;
00212                 }
00213                 //perlambatan=0;
00214             } 
00215             else 
00216             {
00217                 //perlambatan=1;
00218             } 
00219             
00220             if (kec_motor>=kec_max) 
00221             {
00222                 kec_motor=kec_max; 
00223             }
00224             //mode lambat dan cepat pada tombol R2 dan L2
00225             if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0)) 
00226             { 
00227                 koef=2;
00228             } 
00229             else if ((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1)) 
00230             { 
00231                 koef=0.5;
00232             } 
00233             else 
00234             { 
00235                 koef=1;  
00236             }
00237             //Motor 4  telat mulai
00238             s1 = (float)(-1*koef*(kec_motor));
00239             s2 = (float)(-1*koef*(kec_motor)); 
00240  
00241  
00242             mundur=true; 
00243             maju=pivka=pivki=naik=turun=false;
00244  
00245             pc.printf("\rMundur Cuy\n");
00246  
00247             motor1.speed(s1);
00248             motor2.speed(s2);
00249             gripper.brake(1);
00250            
00251     
00252             break;
00253         }
00254     case (5) : //naik
00255         {
00256             //motor1.brake(1);
00257             //motor2.brake(1);
00258             gripper.speed(-kec_gripper_naik);
00259             break;
00260         }
00261      case (6) : //turun
00262         {
00263             //motor1.brake(1);
00264             //motor2.brake(1);
00265             gripper.speed(kec_gripper_turun);
00266             break;
00267         }  
00268         
00269         //TAMBAHAN POWER
00270         case (7) : //turun
00271         {
00272             motor1.speed(0.01);
00273             motor2.brake(0.01);
00274             //com.putc('1');
00275             //break;
00276         }
00277         case (8) : //turun
00278         {
00279             motor1.speed(0.01);
00280             motor2.speed(0.01);
00281             //com.putc('2');
00282             break;
00283         } 
00284          
00285          
00286     default :
00287         {
00288             
00289                 motor1.brake(1);
00290                 motor2.brake(1);
00291                 gripper.brake(1);
00292                
00293             //}
00294  
00295             maju=mundur=pivki=pivka=false;
00296             stop = true;
00297  
00298             //s1 = 0;s2 =0; 
00299  
00300             pc.printf("\rStop\n");
00301         }
00302     }
00303 }
00304 
00305 int mode_jalan ()
00306 {
00307     int jalan_ke;
00308     if ((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)) 
00309     {
00310         // Pivot Kanan
00311         jalan_ke = 1;
00312     } 
00313     else if ((ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1)) 
00314     {
00315         // Pivot Kiri
00316         jalan_ke = 2;
00317     } 
00318     else if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)) 
00319     {  
00320         // Maju
00321         jalan_ke = 3; 
00322     } 
00323     else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1))  
00324     {  
00325         // Mundur
00326         jalan_ke = 4;
00327     }
00328     else if((ps2.read(PS_PAD::PAD_X)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0))
00329     {
00330         jalan_ke = 5;
00331     }
00332     else if((ps2.read(PS_PAD::PAD_X)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1))
00333     {
00334         jalan_ke = 6;
00335     }
00336     
00337     //tambahan power
00338     else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0))
00339     {
00340         jalan_ke = 7;
00341     }
00342     else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==0))
00343     {
00344         jalan_ke = 8;
00345     }
00346     else
00347     {
00348         jalan_ke = 0;
00349     }
00350     return(jalan_ke);
00351 }
00352 
00353 int main ()
00354 {
00355     // Set baud rate - 115200
00356     pc.baud(115200);
00357     com.baud(9600);
00358     pc.printf("Ready...\n");
00359     ps2.init();
00360     
00361     while (1)
00362     {
00363             ps2.poll();
00364             case_jalan= mode_jalan(); 
00365             if (case_jalan != prev_case_jalan)
00366             {
00367                 kec_motor=0;
00368             }
00369             
00370             prosedurmotor();         
00371             prev_case_jalan = mode_jalan(); 
00372     }    
00373 }