main.cpp

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