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 = 16.0, target_rpm2 = 16.0; // selisih 4 bagus, sama bagus
00066 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
00067 
00068 const float pwmPowerUp        = 0.9;
00069 const float pwmPowerDown      = -0.9;
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.62;     // Tunning PWM motor Depan
00080 float tuneBlk           = 0.62;     // 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( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
00135 
00136 /* Deklarasi Motor Base */
00137 Motor motorDpn(PB_7, PC_3, PC_0); 
00138 Motor motorBlk(PB_6, PC_13, PC_14);
00139 Motor motorL  (PB_9, PA_12, PA_6);  
00140 Motor motorR  (PB_8, PC_5, PC_6); 
00141 
00142 /* Deklarasi Motor Launcher */
00143 Motor launcherDpn(PA_5,PA_11,PB_12);
00144 Motor launcherBlk(PB_3, PA_10, PC_4); 
00145 Motor powerScrew(PB_10, PB_13, PB_14); // pwm, fwd, rev                
00146 
00147 /* Deklarasi Penumatik Launcher */
00148 DigitalOut pneumatik(PA_4, PullUp);
00149 
00150 /*Dekalrasi Limit Switch */
00151 //DigitalIn infraAtas(PC_9, PullUp);
00152 DigitalIn limitTengah(PA_9, PullUp);
00153 DigitalIn limitBawah(PC_7, PullUp);
00154 DigitalIn limitBawah1(PC_2, PullUp);
00155 
00156 /*deklarasi PING ultrasonic*/
00157 Ping pingAtas(PC_15);
00158 
00159 /*Deklarasi Display*/
00160 DigitDisplay display (PA_8, PC_8);
00161 
00162 /****************************************************/
00163 /*         Deklarasi Fungsi dan Procedure           */
00164 /****************************************************/
00165 int case_joystick()
00166 {
00167 //---------------------------------------------------//
00168 //  Gerak Motor Base                                 //
00169 //   Case 1  : Pivot kanan                           //
00170 //   Case 2  : Pivot Kiri                            //
00171 //   Case 3  : Kanan                                 //
00172 //   Case 4  : Kiri                                  //
00173 //   Case 5  : Break                                 //
00174 //---------------------------------------------------//
00175     
00176     int caseJoystick;
00177     if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00178         // Pivot Kanan
00179         caseJoystick = 1;
00180     } 
00181     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00182         // Pivot Kiri
00183         caseJoystick = 2;
00184     }
00185     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00186         // Kanan + Rotasi kanan
00187         caseJoystick = 17;
00188     } 
00189     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {  
00190         // Kanan + Rotasi kiri
00191         caseJoystick = 18;
00192     }
00193     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00194         // Kiri + Rotasi kanan
00195         caseJoystick = 19;
00196     } 
00197     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))  {  
00198         // Kanan + Rotasi kiri
00199         caseJoystick = 20;
00200     }
00201     else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00202         // Maju + Rotasi kanan
00203         caseJoystick = 21;
00204     } 
00205     else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00206         // Maju + Rotasi kiri
00207         caseJoystick = 22;
00208     }
00209     else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00210         // Mundur + Rotasi kanan
00211         caseJoystick = 23;
00212     } 
00213     else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
00214         // Mundur + Rotasi kiri
00215         caseJoystick = 24;
00216     }
00217     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click))  {  
00218         // Kanan + segitiga
00219         caseJoystick = 25;
00220     }
00221     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click))  {  
00222         // Kiri + segitiga
00223         caseJoystick = 26;
00224     }
00225     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click))  {  
00226         // Kanan + lingkaran
00227         caseJoystick = 27;
00228     }
00229     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click))  {  
00230         // Kiri + lingkaran
00231         caseJoystick = 28;
00232     }
00233     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click))  {  
00234         // Kanan + kotak
00235         caseJoystick = 29;
00236     }
00237     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click))  {  
00238         // Kiri + kotak
00239         caseJoystick = 30;
00240     }
00241     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
00242         // Serong kanan maju
00243         caseJoystick = 13;     
00244     }
00245     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00246         // Serong kiri maju
00247         caseJoystick = 14;      
00248     }
00249     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {   
00250         // Serong kanan mundur
00251         caseJoystick = 15;       
00252     }
00253     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00254         // Serong kiri mundur
00255         caseJoystick = 16;       
00256     } 
00257     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
00258         // Kanan
00259         caseJoystick = 3;
00260     } 
00261     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00262         // Kiri
00263         caseJoystick = 4;
00264     }
00265     else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
00266         // Atas -- Maju
00267         caseJoystick = 8;       
00268     }
00269     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {   
00270         // Bawah -- Mundur
00271         caseJoystick = 9;       
00272     } 
00273     else if (joystick.segitiga_click){
00274         // Motor Launcher
00275         caseJoystick = 5;
00276     }
00277     else if (joystick.R2_click){
00278         // Target Pulse PID ++ Motor Depan
00279          caseJoystick = 6;
00280     }  
00281     else if (joystick.L2_click){
00282         // Target Pulse PID -- Motor 
00283          caseJoystick = 7;
00284     }
00285     else if (joystick.silang_click){
00286         // Pnemuatik ON
00287         caseJoystick = 10;
00288     }
00289     else if ((joystick.lingkaran_click)&&(!joystick.kotak_click))  {   
00290         // Power Screw Up
00291         caseJoystick = 11;
00292     } 
00293     else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {   
00294         // Power Screw Down
00295         caseJoystick = 12;       
00296     } else caseJoystick = 999;
00297     
00298     return(caseJoystick);
00299 }
00300 
00301 float getTetha(){
00302 // Fungsi untuk mendapatkan nilai tetha
00303     float busur, tetha;
00304    // busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
00305    // tetha = busur/keliling_robot*360;
00306     
00307    // return -(tetha);    
00308 }
00309 
00310 float pidBase(float Kp, float Ki, float Kd)
00311 {
00312     int errorP;
00313     errorP = getTetha();
00314     if (errorP<3.5 && errorP>(-3.5))
00315         errorP = 0;
00316     return (float)Kp*errorP;    
00317 }
00318 
00319 void setCenter(){
00320 // Fungsi untuk menentukan center dari robot
00321    // encoderBase.reset();
00322 }
00323 
00324 void init_rpm (){
00325     if (target_rpm>maxRPM-2){
00326         target_rpm = maxRPM-2;
00327     }
00328     if (target_rpm<minRPM){
00329         target_rpm = minRPM;
00330     }
00331     if (target_rpm2>maxRPM){
00332         target_rpm2 = maxRPM;
00333     }
00334     if (target_rpm2<minRPM+2){
00335         target_rpm2 = minRPM+2;
00336     }
00337 }
00338 
00339 void aktuator()
00340 {
00341     switch (case_joy) {
00342         case (1): 
00343         {       
00344             // Pivot Kanan
00345             motorDpn.speed(-PIVOT);
00346             motorBlk.speed(-PIVOT);
00347             motorR.speed(-rasio*PIVOT);
00348             motorL.speed(-rasio*PIVOT);
00349             break;
00350         }
00351         case (2): 
00352         {
00353             // Pivot Kiri
00354             motorDpn.speed(PIVOT);
00355             motorBlk.speed(PIVOT);
00356             motorR.speed(rasio*PIVOT);
00357             motorL.speed(rasio*PIVOT);
00358             break;
00359         }
00360         case (17): 
00361         {       
00362             // Kanan + Rotasi Kanan
00363             motorDpn.speed(-PIVOT);
00364             motorBlk.speed(-PIVOT);
00365             motorR.speed(-rasio*PIVOT);
00366             motorL.speed(-rasio*PIVOT);
00367             break;
00368         }
00369         case (18): 
00370         {
00371             // Kanan + Rotasi Kiri
00372             motorDpn.speed(PIVOT);
00373             motorBlk.speed(PIVOT);
00374             motorR.speed(rasio*PIVOT);
00375             motorL.speed(rasio*PIVOT);
00376             break;
00377         }
00378         case (19): 
00379         {       
00380             // Kiri + Rotasi Kanan
00381             motorDpn.speed(-PIVOT);
00382             motorBlk.speed(-PIVOT);
00383             motorR.speed(-rasio*PIVOT);
00384             motorL.speed(-rasio*PIVOT);
00385             break;
00386         }
00387         case (20): 
00388         {
00389             // Kiri + Rotasi Kiri
00390             motorDpn.speed(PIVOT);
00391             motorBlk.speed(PIVOT);
00392             motorR.speed(rasio*PIVOT);
00393             motorL.speed(rasio*PIVOT);
00394             break;
00395         }
00396         case (21): 
00397         {       
00398             // Maju + Rotasi Kanan
00399             motorDpn.speed(-PIVOT);
00400             motorBlk.speed(-PIVOT);
00401             motorR.speed(-rasio*PIVOT);
00402             motorL.speed(-rasio*PIVOT);
00403             break;
00404         }
00405         case (22): 
00406         {
00407             // Maju + Rotasi Kiri
00408             motorDpn.speed(PIVOT);
00409             motorBlk.speed(PIVOT);
00410             motorR.speed(rasio*PIVOT);
00411             motorL.speed(rasio*PIVOT);
00412             break;
00413         }
00414         case (23): 
00415         {       
00416             // Mundur + Rotasi Kanan
00417             motorDpn.speed(-PIVOT);
00418             motorBlk.speed(-PIVOT);
00419             motorR.speed(-rasio*PIVOT);
00420             motorL.speed(-rasio*PIVOT);
00421             break;
00422         }
00423         case (24): 
00424         {
00425             // Mundur + Rotasi Kiri
00426             motorDpn.speed(PIVOT);
00427             motorBlk.speed(PIVOT);
00428             motorR.speed(rasio*PIVOT);
00429             motorL.speed(rasio*PIVOT);
00430             break;
00431         }
00432         case (25): 
00433         {
00434             // Kanan + segitiga
00435             isLauncher = !isLauncher;
00436             break;
00437         }
00438         case (26): 
00439         {
00440             // Kiri + segitiga
00441             isLauncher = !isLauncher;
00442             break;
00443         }
00444         case (27): 
00445         {
00446             // Kanan + lingkaran
00447             ReloadOn = !ReloadOn;
00448             isReload = false;
00449             break;
00450         }
00451         case (28): 
00452         {
00453             // Kiri + lingkaran
00454             ReloadOn = !ReloadOn;
00455             isReload = false;
00456             break;
00457         }
00458         case (29): 
00459         {
00460             // Kanan + kotak
00461             ReloadOn = !ReloadOn;
00462             isReload = true;
00463             break;
00464         }
00465         case (30): 
00466         {
00467             // Kiri + kotak
00468             ReloadOn = !ReloadOn;
00469             isReload = true;
00470             break;
00471         }
00472         case (13) : 
00473         {
00474             // Serong kanan maju
00475             motorDpn.speed(-serong);
00476             motorL.speed(-serong);
00477             motorBlk.speed(serong);
00478             motorR.speed(serong);
00479             break;
00480         }
00481         case (14) : 
00482         {
00483             // Serong kiri maju
00484             motorDpn.speed(serong);
00485             motorR.speed(serong);
00486             motorBlk.speed(-serong);
00487             motorL.speed(-serong);
00488             break;
00489         }
00490         case (15) : 
00491         {
00492             // Serong kanan mundur 
00493             motorDpn.speed(-serong);
00494             motorR.speed(-serong);
00495             motorBlk.speed(serong);
00496             motorL.speed(serong);
00497             break;
00498         }
00499         case (16) : 
00500         {
00501             // Serong kiri mundur
00502             motorDpn.speed(serong);
00503             motorL.speed(serong);
00504             motorBlk.speed(-serong);
00505             motorR.speed(-serong);
00506             break;
00507         }
00508         case (3) : 
00509         {
00510             // Kanan
00511             motorDpn.speed(-tuneDpn);
00512             motorBlk.speed(tuneBlk);
00513             motorR.brake(1);
00514             motorL.brake(1);
00515             break;
00516         }
00517         case (4) : 
00518         {
00519             // Kiri
00520             motorDpn.speed(tuneDpn);
00521             motorBlk.speed(-tuneBlk);
00522             motorR.brake(1);
00523             motorL.brake(1);
00524             break;
00525         }
00526         case (8) : 
00527         {
00528             // Maju
00529             motorR.speed(tuneR);
00530             motorL.speed(-tuneL);
00531             motorDpn.brake(1);
00532             motorBlk.brake(1);
00533             break;
00534         }
00535         case (9) : 
00536         {
00537             // Mundur
00538             motorR.speed(-tuneR);
00539             motorL.speed(tuneL);
00540             motorDpn.brake(1);
00541             motorBlk.brake(1);
00542             break;
00543         }
00544         case (5) : 
00545         {
00546             // Aktifkan motor atas
00547             isLauncher = !isLauncher;
00548             break;
00549         }
00550         case (6) : 
00551         {
00552             // Target Pulse PID ++ Motor Depan
00553             target_rpm2 = target_rpm2+1.0;
00554             target_rpm = target_rpm+1.0;
00555             init_rpm();
00556             break;
00557         }
00558         case (7) : 
00559         {
00560             // Target Pulse PID -- Motor Depan
00561             target_rpm2 = target_rpm2-1.0;
00562             target_rpm = target_rpm-1.0;
00563             init_rpm();
00564             break;
00565         }
00566         case (10) : 
00567         {
00568             // Pneumatik
00569             if (ready)
00570             {
00571                 pneumatik = 0;
00572                 previousMillis3 = millis();
00573                 flag_Pneu = true;
00574                 ready = false;
00575             }
00576             break;
00577         }
00578         case (11) : 
00579         {
00580             // Power Screw Up
00581             ReloadOn = !ReloadOn;
00582             isReload = false;
00583             break;
00584         }
00585         case (12) : 
00586         {
00587             // Power Screw Down
00588             ReloadOn = !ReloadOn;
00589             isReload = true;
00590             break;
00591         }
00592         default : 
00593         {
00594             motorDpn.brake(1);
00595             motorBlk.brake(1);
00596             motorR.brake(1);
00597             motorL.brake(1);
00598         }   
00599     } // End Switch
00600  }
00601  
00602 void reloader()
00603 {
00604     if(ReloadOn){
00605         if(isReload){
00606             powerScrew.speed(pwmPowerDown);
00607             if((!limitBawah)||(limitBawah1)){
00608                 isReload = false;
00609                 ReloadOn = false;
00610             }
00611         }
00612         else if(!limitTengah){
00613             isReload = true;
00614         }
00615         else if((jarak_ping > 6.5) && !flag_Pneu){
00616             powerScrew.speed(pwmPowerUp);
00617             ready = false;
00618         }
00619         else if((jarak_ping < 6.0) && !flag_Pneu) {
00620             powerScrew.speed(-0.4);
00621             ready = false;
00622         }
00623         else{
00624             powerScrew.brake(1);
00625             ready = true;
00626         }
00627     }
00628     else{
00629         powerScrew.brake(1);
00630     }
00631 }
00632  
00633  
00634 void launcher()
00635 {
00636     if (isLauncher)
00637     {
00638         currentMillis  = millis();
00639         currentMillis2 = millis();
00640         
00641         // PID Launcher Depan
00642         if (currentMillis-previousMillis>=12.5)
00643         {
00644             rpm = (float)encLauncherBlk.getPulses();    
00645             current_error = target_rpm - rpm;
00646             sum_error = sum_error + current_error;
00647             p = current_error*kpA;
00648             d = (current_error-last_error)*kdA/12.5;
00649             i = sum_error*kiA*12.5;
00650             speed = p + d + i;
00651             //init_speed();
00652             if(speed > maxSpeed){
00653                 launcherBlk.speed(maxSpeed);
00654             }
00655             else if ( speed < minSpeed){
00656                 launcherBlk.speed(minSpeed);
00657             }
00658             else {
00659                 launcherBlk.speed(speed);
00660             }
00661             last_error = current_error;
00662             encLauncherBlk.reset();
00663           //pc.printf("%.04lf\n",rpm);
00664             previousMillis = currentMillis;
00665         }
00666         if (currentMillis2-previousMillis2>=12.5)
00667         {
00668             rpm2 = (float)encLauncherDpn.getPulses();    
00669             current_error2 = target_rpm2 - rpm2;
00670             sum_error2 = sum_error2 + current_error2;
00671             p2 = current_error2*kpA;
00672             d2 = (current_error2-last_error2)*kdA/12.5;
00673             i2 = sum_error2*kiA*12.5;
00674             speed2 = p2 + d2 + i2;
00675             //init_speed();
00676             if (speed2 > maxSpeed){
00677                 launcherDpn.speed(maxSpeed);
00678             }
00679             else if ( speed < minSpeed){
00680                 launcherDpn.speed(minSpeed);
00681             }
00682             else{
00683                 launcherDpn.speed(speed2);
00684             }
00685             last_error2 = current_error2;
00686             encLauncherDpn.reset();
00687             previousMillis2 = currentMillis2;
00688         }
00689     }
00690     else
00691     {
00692         launcherDpn.brake(1);
00693         launcherBlk.brake(1);
00694         sum_error = 0;
00695         sum_error2 = 0;
00696         current_error = 0;
00697         current_error2 = 0;
00698         last_error = 0;
00699         last_error2 = 0;
00700     }     
00701 }
00702   
00703 /*********************************************************/
00704 /*                  Main Function                        */
00705 /*********************************************************/
00706 
00707 int main (void)
00708 {
00709     // Set baud rate - 115200
00710     joystick.setup();
00711     pc.baud(115200);
00712     wait_ms(1000);
00713     
00714     // initializing encoder
00715     pneumatik =1;
00716     
00717     setCenter();
00718     
00719     wait_ms(500);
00720     
00721     //initializing PING
00722     pingAtas.Send();
00723     
00724     pc.printf("ready....");
00725     startMillis();
00726     while(1)
00727     {   
00728         // interupsi pembacaan PING setiap 30 ms
00729         if(millis() - previousMillis4 >= 5){    //30
00730             jarak_ping = (float)pingAtas.Read_cm()/2;
00731             
00732             pingAtas.Send();
00733             previousMillis4 = millis();
00734         }
00735         
00736         // Interrupt Serial
00737         joystick.idle();        
00738         if(joystick.readable()) 
00739         {
00740             // Panggil fungsi pembacaan joystik
00741             joystick.baca_data();
00742             // Panggil fungsi pengolahan data joystik
00743             joystick.olah_data();
00744             // Masuk ke case joystick
00745             case_joy = case_joystick();
00746             pc.printf("%d\n",case_joy);
00747             aktuator();
00748             launcher();
00749             reloader();
00750             if ((millis()-previousMillis3 >= 320)&&(flag_Pneu)){
00751                 pneumatik = 1;
00752                 flag_Pneu = false;
00753             }
00754         }
00755         else
00756         {
00757             joystick.idle();
00758         }
00759         
00760         if(millis() - previousMillis5 >= 400)
00761         {    
00762             display.write(0,((int) rpm2) / 10);
00763             display.write(1,((int)rpm2) % 10);
00764             display.write(2, (int)target_rpm2 / 10);
00765             display.write(3, (int)target_rpm2 % 10);
00766             display.setColon(true);
00767             
00768             previousMillis5 = millis();
00769         }
00770     }
00771 }