main.cpp

00001 
00002 /****************************************************************************/
00003 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
00004 /*                                                                          */
00005 /*                  Last Update : 11 Maret 2017                          */
00006 /*                                                                          */
00007 /*  - Digunakan encoder autonics                                            */
00008 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
00009 /*                      ______________________                              */
00010 /*                     /                      \    Rode Depan Belakang:     */
00011 /*                    /      2 (Belakang)      \   Omniwheel                */
00012 /*                   |                          |                           */
00013 /*                   | 3 (kiri)       4 (kanan) |  Roda Kiri Kanan:         */
00014 /*                   |                          |  Omniwheel                */
00015 /*                    \       1 (Depan)        /                            */
00016 /*                     \______________________/    Putaran berlawanan arah  */
00017 /*                                                 jarum jam positif        */
00018 /*   SETTINGS (WAJIB!) :                                                    */
00019 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
00020 /*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
00021 /*                                                                          */
00022 /****************************************************************************/
00023 /*                                                                          */
00024 /*  Joystick                                                                */
00025 /*  Kanan =>                                                                */
00026 /*  Kiri  =>                                                                */
00027 /*                                                                          */
00028 /*  Tombol silang    => Pneumatik aktif                                     */
00029 /*  Tombol segitiga  => Aktif motor Launcher                                */
00030 /*  Tombol lingkaran => Reloader naik                                       */
00031 /*  Tombol kotak     => Reloader turun                                      */
00032 /*  Tombol L1        => Pivot Kiri                                          */
00033 /*  Tombol R1        => Pivot Kanan                                         */
00034 /*  Tombol L2        => Kurang PWM Motor Launcher                           */
00035 /*  Tombol R2        => Tambah PWM Motor Launcher                           */
00036 /*                                                                          */
00037 /*  Bismillahirahmanirrahim                                                 */
00038 /*  Jagalah Kebersihan Kodingan                                             */
00039 /****************************************************************************/
00040 
00041 #include "mbed.h"
00042 #include "JoystickPS3.h"
00043 #include "Motor.h"
00044 #include "encoderKRAI.h"
00045 #include "millis.h"
00046 #include "Ping.h"
00047 #include "DigitDisplay.h"
00048 
00049 /***********************************************/
00050 /*          Konstanta dan Variabel             */
00051 /***********************************************/
00052 #define PI 3.14159265
00053 #define D_ENCODER 10        // Diameter Roda Encoder
00054 #define D_ROBOT 80          // Diameter Roda Robot
00055 
00056 // Variable Atas
00057 double speed, speed2;
00058 const double maxSpeed = 0.95, minSpeed = 0.0;
00059 const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
00060 double p,i,d;
00061 double p2,i2,d2;
00062 double last_error = 0, current_error, sum_error = 0;
00063 double last_error2 = 0, current_error2, sum_error2 = 0;
00064 float rpm, rpm2;
00065 float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
00066 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
00067 
00068 const float pwmPowerUp        = 1.0;
00069 const float pwmPowerDown      = -1.0;
00070 
00071 float jarak_ping=0;
00072  
00073 // Variable Bawah
00074 float Vt;
00075 float keliling_enc      = PI*D_ENCODER;
00076 float keliling_robot    = PI*D_ROBOT;
00077 float speedT            = 0.2;
00078 float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
00079 float tuneDpn           = 0.80;     // Tunning PWM motor Depan
00080 float tuneBlk           = 0.80;     // Tunning PWM motor belakang
00081 float tuneR             = 0.78;
00082 float tuneL             = 0.72;
00083 float serong            = 0.4;
00084 float rasio             = 1.4545;
00085 
00086 /* variable tunning */
00087 float tunning_L;
00088 float tunning_R;
00089 float tunning_Dpn;
00090 float tunning_Blk;
00091 
00092 /* Variabel Encoder Bawah */
00093 float errTetha, Tetha;    // Variabel yang didapatkan encoder
00094 
00095 /* Deklarasi Variable Millis */
00096 unsigned long int previousMillis = 0;   // PID launcher
00097 unsigned long int currentMillis;
00098 unsigned long int previousMillis2 = 0;  // PID launcher
00099 unsigned long int currentMillis2;
00100 unsigned long int previousMillis3 = 0;  // Pneumatik
00101 unsigned long int previousMillis4 = 0;  // Ping
00102 unsigned long int previousMillis5 = 0;  // Display
00103 
00104 /* Variabel Stick */
00105 //Logic untuk masuk aktuator
00106 int case_joy;
00107 bool isLauncher = false;
00108 bool isReload = false;
00109 bool ReloadOn = false;
00110 bool flag_Pneu = false;
00111 bool ready = false;
00112 
00113 /*****************************************************/
00114 /*   Definisi Prosedur, Fungsi dan Setting Pinout    */
00115 /*****************************************************/
00116 
00117 /* Fungsi dan Procedur Encoder */
00118 void init_speed();                  // 
00119 void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
00120 int case_joystick();                // Mendapatkan case dari joystick
00121 //void speedKalibrasiMotor();       // Pertambahan target RPM motor atas melalui joystick
00122 void setCenter();                   // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
00123 float getTetha();                   // Fungsi mendapatkan error Tetha
00124 
00125 /* Inisialisasi  Pin TX-RX Joystik dan PC */
00126 joysticknucleo joystick(PA_0,PA_1);
00127 Serial pc(USBTX,USBRX);
00128 
00129 /* Deklarasi Encoder Base */
00130 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
00131 
00132 /* Deklarasi Encoder Launcher */
00133 encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
00134 encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
00135 
00136 /* Deklarasi Motor Base */
00137 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
00138 //Motor motorBlk(PB_6, PC_14, PC_13);  //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 
00139 Motor motorBlk(PB_2, PB_15, PB_1);
00140 Motor motorL  (PB_9, PA_12, PA_6);  
00141 Motor motorR  (PB_8, PC_5, PC_6);   //(PC_6, PB_4, PB_5);
00142 
00143 /* Deklarasi Motor Launcher */
00144 Motor launcherDpn(PA_5,PB_12,PA_11);    // pwm ,fwd, rev
00145 Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev 
00146 Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev                
00147 
00148 /* Deklarasi Penumatik Launcher */
00149 DigitalOut pneumatik(PA_4, PullUp);
00150 
00151 /*Dekalrasi Limit Switch */
00152 //DigitalIn infraAtas(PC_9, PullUp);
00153 DigitalIn limitTengah(PA_9, PullUp);
00154 DigitalIn limitBawah(PC_7, PullUp);
00155 DigitalIn limitBawah1(PA_7, PullUp);
00156 
00157 /*deklarasi PING ultrasonic*/
00158 Ping pingAtas(PC_15);
00159 
00160 /*Deklarasi Display*/
00161 DigitDisplay display (PA_8, PC_8);
00162 
00163 /****************************************************/
00164 /*         Deklarasi Fungsi dan Procedure           */
00165 /****************************************************/
00166 int case_joystick()
00167 {
00168 //---------------------------------------------------//
00169 //  Gerak Motor Base                                 //
00170 //   Case 1  : Pivot kanan                           //
00171 //   Case 2  : Pivot Kiri                            //
00172 //   Case 3  : Kanan                                 //
00173 //   Case 4  : Kiri                                  //
00174 //   Case 5  : Break                                 //
00175 //---------------------------------------------------//
00176     
00177     int caseJoystick;
00178     if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00179         // Pivot Kanan
00180         caseJoystick = 1;
00181     } 
00182     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00183         // Pivot Kiri
00184         caseJoystick = 2;
00185     }
00186     else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {   
00187         // tambah rpm dengan nilai tertentu
00188         caseJoystick = 31;     
00189     }
00190     else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {   
00191         // kurangi rpm dengan nilai tertentu
00192         caseJoystick = 32;     
00193     }
00194     else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {   
00195         // kurangi rpm dengan nilai tertentu
00196         caseJoystick = 33;     
00197     }
00198     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00199         // Kanan + Rotasi kanan
00200         caseJoystick = 17;
00201     } 
00202     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00203         // Kanan + Rotasi kiri
00204         caseJoystick = 18;
00205     }
00206     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00207         // Kiri + Rotasi kanan
00208         caseJoystick = 19;
00209     } 
00210     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00211         // Kanan + Rotasi kiri
00212         caseJoystick = 20;
00213     }
00214     else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00215         // Maju + Rotasi kanan
00216         caseJoystick = 21;
00217     } 
00218     else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00219         // Maju + Rotasi kiri
00220         caseJoystick = 22;
00221     }
00222     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00223         // Mundur + Rotasi kanan
00224         caseJoystick = 23;
00225     } 
00226     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00227         // Mundur + Rotasi kiri
00228         caseJoystick = 24;
00229     }
00230     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click))  {  
00231         // Kanan + segitiga
00232         caseJoystick = 25;
00233     }
00234     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click))  {  
00235         // Kiri + segitiga
00236         caseJoystick = 26;
00237     }
00238     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click))  {  
00239         // Kanan + lingkaran
00240         caseJoystick = 27;
00241     }
00242     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click))  {  
00243         // Kiri + lingkaran
00244         caseJoystick = 28;
00245     }
00246     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click))  {  
00247         // Kanan + kotak
00248         caseJoystick = 29;
00249     }
00250     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click))  {  
00251         // Kiri + kotak
00252         caseJoystick = 30;
00253     }
00254     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
00255         // Serong kanan maju
00256         caseJoystick = 13;     
00257     }
00258     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00259         // Serong kiri maju
00260         caseJoystick = 14;      
00261     }
00262     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
00263         // Serong kanan mundur
00264         caseJoystick = 15;       
00265     }
00266     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00267         // Serong kiri mundur
00268         caseJoystick = 16;       
00269     } 
00270     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
00271         // Kanan
00272         caseJoystick = 3;
00273     } 
00274     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00275         // Kiri
00276         caseJoystick = 4;
00277     }
00278     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
00279         // Atas -- Maju
00280         caseJoystick = 8;       
00281     }
00282     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
00283         // Bawah -- Mundur
00284         caseJoystick = 9;       
00285     } 
00286     else if (joystick.segitiga_click){
00287         // Motor Launcher
00288         caseJoystick = 5;
00289     }
00290     else if (joystick.R2_click){
00291         // Target Pulse PID ++ Motor Depan
00292          caseJoystick = 6;
00293     }  
00294     else if (joystick.L2_click){
00295         // Target Pulse PID -- Motor 
00296          caseJoystick = 7;
00297     }
00298     else if (joystick.silang_click){
00299         // Pnemuatik ON
00300         caseJoystick = 10;
00301     }
00302     else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
00303         // Power Screw Up
00304         caseJoystick = 11;
00305     } 
00306     else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
00307         // Power Screw Down
00308         caseJoystick = 12;       
00309     } 
00310     
00311     return(caseJoystick);
00312 }
00313 
00314 float getTetha(){
00315 // Fungsi untuk mendapatkan nilai tetha
00316     float busur, tetha;
00317     busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
00318     tetha = busur/keliling_robot*360;
00319     
00320     return -(tetha);    
00321 }
00322 
00323 float pidBase(float Kp, float Ki, float Kd)
00324 {
00325     int errorP;
00326     errorP = getTetha();
00327     if (errorP<3.5 && errorP>(-3.5))
00328         errorP = 0;
00329     return (float)Kp*errorP;    
00330 }
00331 
00332 void setCenter(){
00333 // Fungsi untuk menentukan center dari robot
00334     encoderBase.reset();
00335 }
00336 
00337 void init_rpm (){
00338     if (target_rpm>maxRPM-2){
00339         target_rpm = maxRPM-2;
00340     }
00341     if (target_rpm<minRPM){
00342         target_rpm = minRPM;
00343     }
00344     if (target_rpm2>maxRPM){
00345         target_rpm2 = maxRPM;
00346     }
00347     if (target_rpm2<minRPM+2){
00348         target_rpm2 = minRPM+2;
00349     }
00350 }
00351 
00352 void aktuator()
00353 {
00354     switch (case_joy) {
00355         case (1): 
00356         {       
00357             // Pivot Kanan
00358             motorDpn.speed(-PIVOT);
00359             motorBlk.speed(-PIVOT);
00360             motorR.speed(-rasio*PIVOT);
00361             motorL.speed(-rasio*PIVOT);
00362             break;
00363         }
00364         case (2): 
00365         {
00366             // Pivot Kiri
00367             motorDpn.speed(PIVOT);
00368             motorBlk.speed(PIVOT);
00369             motorR.speed(rasio*PIVOT);
00370             motorL.speed(rasio*PIVOT);
00371             break;
00372         }
00373         case (17): 
00374         {       
00375             // Kanan + Rotasi Kanan
00376             motorDpn.speed(-PIVOT);
00377             motorBlk.speed(-PIVOT);
00378             motorR.speed(-rasio*PIVOT);
00379             motorL.speed(-rasio*PIVOT);
00380             break;
00381         }
00382         case (18): 
00383         {
00384             // Kanan + Rotasi Kiri
00385             motorDpn.speed(PIVOT);
00386             motorBlk.speed(PIVOT);
00387             motorR.speed(rasio*PIVOT);
00388             motorL.speed(rasio*PIVOT);
00389             break;
00390         }
00391         case (19): 
00392         {       
00393             // Kiri + Rotasi Kanan
00394             motorDpn.speed(-PIVOT);
00395             motorBlk.speed(-PIVOT);
00396             motorR.speed(-rasio*PIVOT);
00397             motorL.speed(-rasio*PIVOT);
00398             break;
00399         }
00400         case (20): 
00401         {
00402             // Kiri + Rotasi Kiri
00403             motorDpn.speed(PIVOT);
00404             motorBlk.speed(PIVOT);
00405             motorR.speed(rasio*PIVOT);
00406             motorL.speed(rasio*PIVOT);
00407             break;
00408         }
00409         case (21): 
00410         {       
00411             // Maju + Rotasi Kanan
00412             motorDpn.speed(-PIVOT);
00413             motorBlk.speed(-PIVOT);
00414             motorR.speed(-rasio*PIVOT);
00415             motorL.speed(-rasio*PIVOT);
00416             break;
00417         }
00418         case (22): 
00419         {
00420             // Maju + Rotasi Kiri
00421             motorDpn.speed(PIVOT);
00422             motorBlk.speed(PIVOT);
00423             motorR.speed(rasio*PIVOT);
00424             motorL.speed(rasio*PIVOT);
00425             break;
00426         }
00427         case (23): 
00428         {       
00429             // Mundur + Rotasi Kanan
00430             motorDpn.speed(-PIVOT);
00431             motorBlk.speed(-PIVOT);
00432             motorR.speed(-rasio*PIVOT);
00433             motorL.speed(-rasio*PIVOT);
00434             break;
00435         }
00436         case (24): 
00437         {
00438             // Mundur + Rotasi Kiri
00439             motorDpn.speed(PIVOT);
00440             motorBlk.speed(PIVOT);
00441             motorR.speed(rasio*PIVOT);
00442             motorL.speed(rasio*PIVOT);
00443             break;
00444         }
00445         case (25): 
00446         {
00447             // Kanan + segitiga
00448             isLauncher = !isLauncher;
00449             break;
00450         }
00451         case (26): 
00452         {
00453             // Kiri + segitiga
00454             isLauncher = !isLauncher;
00455             break;
00456         }
00457         case (27): 
00458         {
00459             // Kanan + lingkaran
00460             ReloadOn = !ReloadOn;
00461             isReload = false;
00462             break;
00463         }
00464         case (28): 
00465         {
00466             // Kiri + lingkaran
00467             ReloadOn = !ReloadOn;
00468             isReload = false;
00469             break;
00470         }
00471         case (29): 
00472         {
00473             // Kanan + kotak
00474             ReloadOn = !ReloadOn;
00475             isReload = true;
00476             break;
00477         }
00478         case (30): 
00479         {
00480             // Kiri + kotak
00481             ReloadOn = !ReloadOn;
00482             isReload = true;
00483             break;
00484         }
00485         case (13) : 
00486         {
00487             // Serong kanan maju
00488             motorDpn.speed(-serong);
00489             motorL.speed(-serong);
00490             motorBlk.speed(serong);
00491             motorR.speed(serong);
00492             break;
00493         }
00494         case (14) : 
00495         {
00496             // Serong kiri maju
00497             motorDpn.speed(serong);
00498             motorR.speed(serong);
00499             motorBlk.speed(-serong);
00500             motorL.speed(-serong);
00501             break;
00502         }
00503         case (15) : 
00504         {
00505             // Serong kanan mundur 
00506             motorDpn.speed(-serong);
00507             motorR.speed(-serong);
00508             motorBlk.speed(serong);
00509             motorL.speed(serong);
00510             break;
00511         }
00512         case (16) : 
00513         {
00514             // Serong kiri mundur
00515             motorDpn.speed(serong);
00516             motorL.speed(serong);
00517             motorBlk.speed(-serong);
00518             motorR.speed(-serong);
00519             break;
00520         }
00521         case (3) : 
00522         {
00523             // Kanan
00524             motorDpn.speed(-tuneDpn);
00525             motorBlk.speed(tuneBlk);
00526             motorR.brake(1);
00527             motorL.brake(1);
00528             break;
00529         }
00530         case (4) : 
00531         {
00532             // Kiri
00533             motorDpn.speed(tuneDpn);
00534             motorBlk.speed(-tuneBlk);
00535             motorR.brake(1);
00536             motorL.brake(1);
00537             break;
00538         }
00539         case (8) : 
00540         {
00541             // Maju
00542             motorR.speed(tuneR);
00543             motorL.speed(-tuneL);
00544             motorDpn.brake(1);
00545             motorBlk.brake(1);
00546             break;
00547         }
00548         case (9) : 
00549         {
00550             // Mundur
00551             motorR.speed(-tuneR);
00552             motorL.speed(tuneL);
00553             motorDpn.brake(1);
00554             motorBlk.brake(1);
00555             break;
00556         }
00557         case (5) : 
00558         {
00559             // Aktifkan motor atas
00560             isLauncher = !isLauncher;
00561             break;
00562         }
00563         case (6) : 
00564         {
00565             // Target Pulse PID ++ Motor Depan
00566             target_rpm2 = target_rpm2+1.0;
00567             target_rpm = target_rpm+1.0;
00568             init_rpm();
00569             break;
00570         }
00571         case (7) : 
00572         {
00573             // Target Pulse PID -- Motor Depan
00574             target_rpm2 = target_rpm2-1.0;
00575             target_rpm = target_rpm-1.0;
00576             init_rpm();
00577             break;
00578         }
00579         case (10) : 
00580         {
00581             // Pneumatik
00582             if (ready)
00583             {
00584                 pneumatik = 0;
00585                 previousMillis3 = millis();
00586                 flag_Pneu = true;
00587                 ready = false;
00588             }
00589             break;
00590         }
00591         case (11) : 
00592         {
00593             // Power Screw Up
00594             ReloadOn = !ReloadOn;
00595             isReload = false;
00596             break;
00597         }
00598         case (31) : 
00599         {
00600             // start
00601             target_rpm2 = 22;
00602             target_rpm = 22;
00603             init_rpm();
00604             break;
00605         }
00606         case (32) : 
00607         {
00608             // select
00609             target_rpm2 = 10;
00610             target_rpm = 10;
00611             init_rpm();
00612             break;
00613         }
00614         case (33) : 
00615         {
00616             // R3
00617             target_rpm2 = 17;
00618             target_rpm = 17;
00619             init_rpm();
00620             break;
00621         }
00622         case (12) : 
00623         {
00624             // Power Screw Down
00625             ReloadOn = !ReloadOn;
00626             isReload = true;
00627             break;
00628         }
00629         default : 
00630         {
00631             motorDpn.brake(1);
00632             motorBlk.brake(1);
00633             motorR.brake(1);
00634             motorL.brake(1);
00635         }   
00636     } // End Switch
00637  }
00638  
00639 void reloader()
00640 {
00641     if(ReloadOn){
00642         if(isReload){
00643             powerScrew.speed(pwmPowerDown);
00644             if((!limitBawah)||(!limitBawah1)){
00645                 isReload = false;
00646                 ReloadOn = false;
00647             }
00648         }
00649         else if(!limitTengah){
00650             isReload = true;
00651         }
00652         else if((jarak_ping > 6.5) && !flag_Pneu){
00653             powerScrew.speed(pwmPowerUp);
00654             ready = false;
00655         }
00656         else if((jarak_ping < 6.0) && !flag_Pneu) {
00657             powerScrew.speed(-0.85);
00658             ready = false;
00659         }
00660         else{
00661             powerScrew.brake(1);
00662             ready = true;
00663         }
00664     }
00665     else{
00666         powerScrew.brake(1);
00667     }
00668 }
00669  
00670  
00671 void launcher()
00672 {
00673     if (isLauncher)
00674     {
00675         currentMillis  = millis();
00676         currentMillis2 = millis();
00677         
00678         // PID Launcher Depan
00679         if (currentMillis-previousMillis>=12.5)
00680         {
00681             rpm = (float)encLauncherBlk.getPulses();    
00682             current_error = target_rpm - rpm;
00683             sum_error = sum_error + current_error*12.5;
00684             p = current_error*kpA;
00685             d = (current_error-last_error)*kdA/12.5;
00686             i = sum_error*kiA;
00687             speed = p + d + i;
00688             //init_speed();
00689             if(speed > maxSpeed){
00690                 launcherBlk.speed(maxSpeed);
00691             }
00692             else if ( speed < minSpeed){
00693                 launcherBlk.speed(minSpeed);
00694             }
00695             else {
00696                 launcherBlk.speed(speed);
00697             }
00698             last_error = current_error;
00699             encLauncherBlk.reset();
00700           //pc.printf("%.04lf\n",rpm);
00701             previousMillis = currentMillis;
00702         }
00703         if (currentMillis2-previousMillis2>=12.5)
00704         {
00705             rpm2 = (float)encLauncherDpn.getPulses();    
00706             current_error2 = target_rpm2 - rpm2;
00707             sum_error2 = sum_error2 + current_error2*12.5;
00708             p2 = current_error2*kpA;
00709             d2 = (current_error2-last_error2)*kdA/12.5;
00710             i2 = sum_error2*kiA;
00711             speed2 = p2 + d2 + i2;
00712             //init_speed();
00713             if (speed2 > maxSpeed){
00714                 launcherDpn.speed(maxSpeed);
00715             }
00716             else if ( speed < minSpeed){
00717                 launcherDpn.speed(minSpeed);
00718             }
00719             else{
00720                 launcherDpn.speed(speed2);
00721             }
00722             last_error2 = current_error2;
00723             encLauncherDpn.reset();
00724             previousMillis2 = currentMillis2;
00725         }
00726         pc.printf("%.2f\t%.2f\n",rpm,rpm2);
00727     }
00728     else
00729     {
00730         launcherDpn.brake(1);
00731         launcherBlk.brake(1);
00732         sum_error = 0;
00733         sum_error2 = 0;
00734         current_error = 0;
00735         current_error2 = 0;
00736         last_error = 0;
00737         last_error2 = 0;
00738     }     
00739 }
00740   
00741 /*********************************************************/
00742 /*                  Main Function                        */
00743 /*********************************************************/
00744 
00745 int main (void)
00746 {
00747     // Set baud rate - 115200
00748     joystick.setup();
00749     pc.baud(115200);
00750     wait_ms(1000);
00751     
00752     // initializing encoder
00753     pneumatik =1;
00754     
00755     setCenter();
00756     
00757     wait_ms(500);
00758     
00759     //initializing PING
00760     pingAtas.Send();
00761     
00762     pc.printf("ready....");
00763     startMillis();
00764     while(1)
00765     {   
00766         // interupsi pembacaan PING setiap 30 ms
00767         if(millis() - previousMillis4 >= 5){    //30
00768             jarak_ping = (float)pingAtas.Read_cm()/2;
00769             
00770             pingAtas.Send();
00771             previousMillis4 = millis();
00772         }
00773         
00774         // Interrupt Serial
00775         joystick.idle();        
00776         if(joystick.readable()) 
00777         {
00778             // Panggil fungsi pembacaan joystik
00779             joystick.baca_data();
00780             // Panggil fungsi pengolahan data joystik
00781             joystick.olah_data();
00782             // Masuk ke case joystick
00783             case_joy = case_joystick();
00784             //pc.printf("%d\n",case_joy);
00785             aktuator();
00786             launcher();
00787             reloader();
00788             if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
00789                 pneumatik = 1;
00790                 flag_Pneu = false;
00791             }
00792         }
00793         else
00794         {
00795             joystick.idle();
00796         }
00797         
00798         if(millis() - previousMillis5 >= 400)
00799         {    
00800             display.write(0,((int) rpm2) / 10);
00801             display.write(1,((int)rpm2) % 10);
00802             display.write(2, (int)target_rpm2 / 10);
00803             display.write(3, (int)target_rpm2 % 10);
00804             display.setColon(true);
00805             
00806             previousMillis5 = millis();
00807         }
00808     }
00809 }