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