main.cpp

00001 /****************************************************************************/
00002 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
00003 /*                                                                          */
00004 /*                  Last Update : 18 Februari 2017                          */
00005 /*                                                                          */
00006 /*  - Digunakan encoder autonics                                            */
00007 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
00008 /*                      ______________________                              */
00009 /*                     /                      \    Rode Depan Belakang:     */
00010 /*                    /      2 (Belakang)      \   Omniwheel                */
00011 /*                   |                          |                           */
00012 /*                   | 3 (Encoder)            4 |  Roda Kiri Kanan:         */
00013 /*                   |                          |  Fixed Wheel              */
00014 /*                    \       1 (Depan)        /                            */
00015 /*                     \______________________/    Putaran berlawanan arah  */
00016 /*                                                 jarum jam positif        */
00017 /*   SETTINGS (WAJIB!) :                                                    */
00018 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
00019 /*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
00020 /*                                                                          */
00021 /****************************************************************************/
00022 /*                                                                          */
00023 /*  Joystick                                                                */
00024 /*  Kanan =>                                                                */
00025 /*  Kiri  =>                                                                */
00026 /*                                                                          */
00027 /*  Tombol silang    => Power Screw Aktif                                   */
00028 /*  Tombol segitiga  => Aktif motor Launcher                                */
00029 /*  Tombol lingkaran => Aktif Pneumatik Launcher                            */
00030 /*  Tombol L1        => Pivot Kiri                                          */
00031 /*  Tombol R1        => Pivot Kanan                                         */
00032 /*  Tombol L3        => PWM Motor Belakang Dikurangi                        */
00033 /*  Tombol R3        => PWM Motor Belakang Ditambah                         */
00034 /*  Tombol L2        => PWM Motor Depan Dikurangi                           */
00035 /*  Tombol R2        => PWM Motor Depan Ditambah                            */
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 /***********************************************/
00048 /*          Konstanta dan Variabel             */
00049 /***********************************************/
00050 #define PI 3.14159265
00051 #define D_ENCODER 10        // Diameter Roda Encoder
00052 #define D_ROBOT 80          // Diameter Roda Robot
00053 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
00054 
00055 // Variable Atas
00056 double speed, speed2;
00057 const double maxSpeed = 0.95, minSpeed = 0.0;
00058 const double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
00059 double p,i,d;
00060 double p2,i2,d2;
00061 double last_error = 0, current_error, sum_error = 0;
00062 double last_error2 = 0, current_error2, sum_error2 = 0;
00063 float rpm, rpm2;
00064 float target_rpm = 15.0, target_rpm2 = 17.0; 
00065 const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
00066 
00067 const float pwmPowerUp        = 0.8;
00068 const float pwmPowerDown      = -0.9;
00069 
00070 float jarak_ping=0;
00071  
00072 // Variable Bawah
00073 float Vt;
00074 float keliling_enc      = PI*D_ENCODER;
00075 float keliling_robot    = PI*D_ROBOT;
00076 float speedT            = 0.2;
00077 float vpid              = 0;
00078 float PIVOT             = 0.27;      // PWM Pivot Kanan, Pivot Kiri
00079 float tuneDpn           = 0.35;    // Tunning PWM motor Depan
00080 float tuneBlk           = 0.3;      // Tunning PWM motor belakang
00081 
00082 /* Variabel Encoder Bawah */
00083 float errTetha, Tetha;    // Variabel yang didapatkan encoder
00084 
00085 /* Deklarasi Variable Millis */
00086 unsigned long int previousMillis = 0;   // PID launcher
00087 unsigned long int currentMillis;
00088 unsigned long int previousMillis2 = 0;  // PID launcher
00089 unsigned long int currentMillis2;
00090 unsigned long int previousMillis3 = 0;  // Pneumatik
00091 unsigned long int previousMillis4 = 0;  // Ping
00092 
00093 /* Variabel Stick */
00094 //Logic untuk masuk aktuator
00095 int case_joy;
00096 bool isLauncher = false;
00097 bool isReload = false;
00098 bool ReloadOn = false;
00099 bool flag_Pneu = false;
00100 
00101 /*****************************************************/
00102 /*   Definisi Prosedur, Fungsi dan Setting Pinout    */
00103 /*****************************************************/
00104 
00105 /* Fungsi dan Procedur Encoder */
00106 void init_speed();                  // 
00107 void aktuator();                    // Pergerakan aktuator berdasarkan case joystick
00108 int case_joystick();                // Mendapatkan case dari joystick
00109 //void speedKalibrasiMotor();       // Pertambahan target RPM motor atas melalui joystick
00110 void setCenter();                   // Prosedur reset encoder, posisi saat itu diset jadi titik (0,0)
00111 float getTetha();                   // Fungsi mendapatkan error Tetha
00112 
00113 /* Inisialisasi  Pin TX-RX Joystik dan PC */
00114 joysticknucleo joystick(PA_0,PA_1);
00115 Serial pc(USBTX,USBRX);
00116 
00117 /* Deklarasi Encoder Base */
00118 encoderKRAI encoderBase(PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), 2xresolusi, mode pembacaan
00119 
00120 /* Deklarasi Encoder Launcher */
00121 encoderKRAI encLauncherBlk( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
00122 encoderKRAI encLauncherDpn( PD_2, PC_12, 28, encoderKRAI::X4_ENCODING);
00123 
00124 /* Deklarasi Motor Base */
00125 Motor motorDpn(PB_9, PA_12, PC_5);    // pwm, fwd, rev
00126 Motor motorBlk(PB_6, PB_1, PB_12);    // pwm, fwd, rev
00127 
00128 /* Deklarasi Motor Launcher */
00129 Motor launcherDpn(PA_8,PC_2,PC_1);    // pwm ,fwd, rev
00130 Motor launcherBlk(PA_10, PC_3, PC_0); // pwm, fwd, rev 
00131 Motor powerScrew(PB_7, PA_14, PA_15); // pwm, fwd, rev                
00132 
00133 /* Deklarasi Penumatik Launcher */
00134 DigitalOut pneumatik(PB_3, PullUp);
00135 
00136 /*Dekalrasi Limit Switch */
00137 //DigitalIn infraAtas(PC_9, PullUp);
00138 DigitalIn limitTengah(PB_10, PullUp);
00139 DigitalIn limitBawah(PC_8, PullUp);
00140 
00141 /*deklarasi PING ultrasonic*/
00142 Ping pingAtas(PC_9);
00143 
00144 /****************************************************/
00145 /*         Deklarasi Fungsi dan Procedure           */
00146 /****************************************************/
00147 int case_joystick()
00148 {
00149 //---------------------------------------------------//
00150 //  Gerak Motor Base                                 //
00151 //   Case 1  : Pivot kanan                           //
00152 //   Case 2  : Pivot Kiri                            //
00153 //   Case 3  : Kanan                                 //
00154 //   Case 4  : Kiri                                  //
00155 //   Case 5  : Break                                 //
00156 //---------------------------------------------------//
00157     
00158     int caseJoystick;
00159     if (!joystick.L1 && joystick.R1) {
00160         // Pivot Kanan
00161         caseJoystick = 1;
00162     } 
00163     else if (!joystick.R1 && joystick.L1) {
00164         // Pivot Kiri
00165         caseJoystick = 2;
00166     } 
00167     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
00168         // Kanan
00169         caseJoystick = 3;
00170          //pc.printf("kanan");
00171     } 
00172     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00173         // Kiri
00174         caseJoystick = 4;
00175          //pc.printf("kiri");       
00176     } 
00177     else if (joystick.segitiga_click){
00178         // Motor Launcher
00179         caseJoystick = 5;
00180     }
00181     else if (joystick.R2_click){
00182         // Target Pulse PID ++ Motor Depan
00183          caseJoystick = 6;
00184     }  
00185     else if (joystick.L2_click){
00186         // Target Pulse PID -- Motor Depan
00187          caseJoystick = 7;
00188     }
00189     /*else if (joystick.R3_click){
00190         // Target Pulse PID ++ Motor Belakang
00191          caseJoystick = 8;
00192     }
00193     else if (joystick.L3_click){
00194         // Target Pulse PID -- Motor Belakang
00195          caseJoystick = 9;
00196     }*/
00197     else if (joystick.silang_click){
00198         // Pnemuatik ON
00199         caseJoystick = 10;
00200     }
00201     else if ((joystick.atas)&&(!joystick.bawah))  {   
00202         // Power Screw Up
00203         caseJoystick = 11;
00204     } 
00205     else if ((!joystick.atas)&&(joystick.bawah)) {   
00206         // Power Screw Down
00207         caseJoystick = 12;       
00208     } 
00209     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
00210         // Break
00211         caseJoystick = 13; 
00212     }
00213     
00214     return(caseJoystick);
00215 }
00216 
00217 float getTetha(){
00218 // Fungsi untuk mendapatkan nilai tetha
00219     float busur, tetha;
00220     busur = ((encoderBase.getPulses())/(float)(2000.0)*keliling_enc);
00221     tetha = busur/keliling_robot*360;
00222     
00223     return -(tetha);    
00224 }
00225 
00226 float pidBase(float Kp, float Ki, float Kd)
00227 {
00228     int errorP;
00229     errorP = getTetha();
00230     if (errorP<3.5 && errorP>(-3.5))
00231         errorP = 0;
00232     return (float)Kp*errorP;    
00233 }
00234 
00235 void setCenter(){
00236 // Fungsi untuk menentukan center dari robot
00237     encoderBase.reset();
00238 }
00239 
00240 void init_rpm (){
00241     if (target_rpm>maxRPM-2){
00242         target_rpm = maxRPM-2;
00243     }
00244     if (target_rpm<minRPM+8){
00245         target_rpm = minRPM;
00246     }
00247     if (target_rpm2>maxRPM){
00248         target_rpm2 = maxRPM;
00249     }
00250     if (target_rpm2<minRPM+2+8){
00251         target_rpm2 = minRPM+2;
00252     }
00253 }
00254 
00255 void aktuator()
00256 {
00257     switch (case_joy) {
00258         case (1): 
00259         {       
00260             // Pivot Kanan
00261             motorDpn.speed(-PIVOT);
00262             motorBlk.speed(-PIVOT);
00263             setCenter();
00264             break;
00265         }
00266         case (2): 
00267         {
00268             // Pivot Kiri
00269             motorDpn.speed(PIVOT);
00270             motorBlk.speed(PIVOT);
00271             setCenter();
00272             break;
00273         }
00274         case (3) : 
00275         {
00276             // Kanan
00277             motorDpn.speed(-tuneDpn + pidBase(0.009,0,0));
00278             motorBlk.speed(tuneBlk + pidBase(0.009,0,0));
00279             //speedDpn = tuneDpn + pidBase(0.009,0,0)
00280             //speedBlk = tuneBlk + pidBase(0.009,0,0)
00281             //motorDpn.speed(-tuneDpn);
00282             //motorBlk.speed(tuneBlk);
00283             break;
00284         }
00285         case (4) : 
00286         {
00287             // Kiri
00288             motorDpn.speed(tuneDpn + pidBase(0.009,0,0));
00289             motorBlk.speed(-tuneBlk + pidBase(0.009,0,0));
00290             //motorDpn.speed(tuneDpn);
00291             //motorBlk.speed(-tuneBlk);
00292             break;
00293         }
00294         case (5) : 
00295         {
00296             // Aktifkan motor atas
00297             isLauncher = !isLauncher;
00298             break;
00299         }
00300         case (6) : 
00301         {
00302             // Target Pulse PID ++ Motor Depan
00303             target_rpm2 = target_rpm2+1.0;
00304             target_rpm = target_rpm+1.0;
00305             init_rpm();
00306             break;
00307         }
00308         case (7) : 
00309         {
00310             // Target Pulse PID -- Motor Depan
00311             target_rpm2 = target_rpm2-1.0;
00312             target_rpm = target_rpm-1.0;
00313             init_rpm();
00314             break;
00315         }
00316         /*case (8) : 
00317         {
00318             // Target Pulse PID ++ Motor Belakang=
00319             //init_rpm();
00320             break;
00321         }
00322         case (9) : 
00323         {
00324             // Target Pulse PID -- Motor Belakang
00325             //init_rpm();
00326             break;
00327         }*/
00328         case (10) : 
00329         {
00330             // Pneumatik
00331             pneumatik = 0;
00332             previousMillis3 = millis();
00333             flag_Pneu = true;
00334             break;
00335         }
00336         case (11) : 
00337         {
00338             // Power Screw Up
00339             //powerScrew.speed(pwmPowerUp);
00340             ReloadOn = !ReloadOn;
00341             //powerScrew.speed(pwmPowerUp);
00342             break;
00343         }
00344         case (12) : 
00345         {
00346             // Power Screw Down
00347             //powerScrew.speed(pwmPowerDown);
00348             break;
00349         }
00350         default : 
00351         {
00352             motorDpn.brake(1);
00353             motorBlk.brake(1);
00354            /* if(isReload){
00355                 powerScrew.speed(pwmPowerDown);
00356                 if(!limitBawah){
00357                     isReload = false;
00358                     ReloadOn = false;
00359                 }
00360             }
00361             else if(!limitTengah){
00362                 isReload = true;
00363             }
00364             else{
00365                 powerScrew.brake(1);
00366             }*/
00367         }   
00368     } // End Switch
00369  }
00370  
00371 void reloader()
00372 {
00373     if(ReloadOn){
00374         if(isReload){
00375             powerScrew.speed(pwmPowerDown);
00376             if(!limitBawah){
00377                 isReload = false;
00378                 ReloadOn = false;
00379             }
00380         }
00381         else if(!limitTengah){
00382             isReload = true;
00383         }
00384         else if((jarak_ping > 4) && !flag_Pneu){
00385             powerScrew.speed(pwmPowerUp);
00386         }
00387         else if((jarak_ping < 3.5 ) && !flag_Pneu) {
00388             powerScrew.speed(-0.1);
00389         }
00390         else{
00391             powerScrew.brake(1);
00392         }
00393     }
00394     else{
00395         powerScrew.brake(1);
00396     }
00397 }
00398  
00399  
00400 void launcher()
00401 {
00402     if (isLauncher)
00403     {
00404         currentMillis  = millis();
00405         currentMillis2 = millis();
00406         
00407         // PID Launcher Depan
00408         if (currentMillis-previousMillis>=12.5)
00409         {
00410             rpm = (float)encLauncherBlk.getPulses();    
00411             current_error = target_rpm - rpm;
00412             sum_error = sum_error + current_error;
00413             p = current_error*kpA;
00414             d = (current_error-last_error)*kdA/12.5;
00415             i = sum_error*kiA*12.5;
00416             speed = p + d + i;
00417             //init_speed();
00418             if(speed > maxSpeed){
00419                 launcherBlk.speed(maxSpeed);
00420             }
00421             else if ( speed < minSpeed){
00422                 launcherBlk.speed(minSpeed);
00423             }
00424             else {
00425                 launcherBlk.speed(speed);
00426             }
00427             last_error = current_error;
00428             encLauncherBlk.reset();
00429           //pc.printf("%.04lf\n",rpm);
00430             previousMillis = currentMillis;
00431         }
00432         if (currentMillis2-previousMillis2>=12.5)
00433         {
00434             rpm2 = (float)encLauncherDpn.getPulses();    
00435             current_error2 = target_rpm2 - rpm2;
00436             sum_error2 = sum_error2 + current_error2;
00437             p2 = current_error2*kpA;
00438             d2 = (current_error2-last_error2)*kdA/12.5;
00439             i2 = sum_error2*kiA*12.5;
00440             speed2 = p2 + d2 + i2;
00441             //init_speed();
00442             if (speed2 > maxSpeed){
00443                 launcherDpn.speed(maxSpeed);
00444             }
00445             else if ( speed < minSpeed){
00446                 launcherDpn.speed(minSpeed);
00447             }
00448             else{
00449                 launcherDpn.speed(speed2);
00450             }
00451             last_error2 = current_error2;
00452             encLauncherDpn.reset();
00453             previousMillis2 = currentMillis2;
00454         }
00455     }
00456     else
00457     {
00458         launcherDpn.brake(1);
00459         launcherBlk.brake(1);
00460     }     
00461 }
00462   
00463 /*********************************************************/
00464 /*                  Main Function                        */
00465 /*********************************************************/
00466 
00467 int main (void)
00468 {
00469     // Set baud rate - 115200
00470     joystick.setup();
00471     pc.baud(115200);
00472     wait_ms(1000);
00473     
00474     // initializing encoder
00475     pneumatik =1;
00476     
00477     setCenter();
00478     
00479     wait_ms(500);
00480     
00481     //initializing PING
00482     pingAtas.Send();
00483     
00484     pc.printf("ready....");
00485     startMillis();
00486     while(1)
00487     {   
00488         // interupsi pembacaan PING setiap 30 ms
00489         if(millis() - previousMillis4 >= 5){    //30
00490             jarak_ping = (float)pingAtas.Read_cm()/2;
00491             
00492             pingAtas.Send();
00493             previousMillis4 = millis();
00494         }
00495         
00496         // Interrupt Serial
00497         joystick.idle();        
00498         if(joystick.readable()) 
00499         {
00500             // Panggil fungsi pembacaan joystik
00501             joystick.baca_data();
00502             // Panggil fungsi pengolahan data joystik
00503             joystick.olah_data();
00504             // Masuk ke case joystick
00505             case_joy = case_joystick();
00506             aktuator();
00507             launcher();
00508             reloader();
00509             if ((millis()-previousMillis3 >= 370)&&(flag_Pneu)){
00510                 pneumatik = 1;
00511                 flag_Pneu = false;
00512             }
00513         }
00514         else
00515         {
00516             joystick.idle();
00517         }
00518     }
00519 }