main.cpp

00001 /****************************************************************************/
00002 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
00003 /*                                                                          */
00004 /*  - Digunakan encoder autonics                                            */
00005 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
00006 /*                      ______________________                              */
00007 /*                     /                      \    Rode Depan Belakang:     */
00008 /*                    /      2 (Belakang)      \   Omniwheel                */
00009 /*                   |                          |                           */
00010 /*                   | 3 (Encoder)            4 |  Roda Kiri Kanan:         */
00011 /*                   |                          |  Fixed Wheel              */
00012 /*                    \       1 (Depan)        /                            */
00013 /*                     \______________________/    Putaran berlawanan arah  */
00014 /*                                                 jarum jam positif        */
00015 /*   SETTINGS (WAJIB!) :                                                    */
00016 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h      */
00017 /*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
00018 /*                                                                          */
00019 /****************************************************************************/
00020 /*                                                                          */
00021 /*  Joystick                                                                */
00022 /*  Kanan => Posisi target x ditambah 'perpindahan'                         */
00023 /*  Kiri  => Posisi target x dikurang 'perpindahan'                         */
00024 /*                                                                          */
00025 /*  Tombol silang   => Kembali keposisi Awal                                */
00026 /*  Tombol segitiga => Aktif motor Launcher                                 */
00027 /*  Tombol lingkaran=> Aktif servo Launcher                                 */
00028 /*  Tombol L1       => Pivot Kiri                                           */
00029 /*  Tombol R1       => Pivot Kanan                                          */
00030 /*  Tombol L3       => PWM Launcher Belakang dikurangin                     */
00031 /*  Tombol R3       => PWM Launcher Belakang ditambahin                     */
00032 /*                                                                          */
00033 /*  Bismillahirahmanirrahim                                                 */
00034 /*  Jagalah Kebersihan Kodingan                                             */
00035 /****************************************************************************/
00036 
00037 #include "mbed.h"
00038 #include "JoystickPS3.h"
00039 #include "Motor.h"
00040 #include "Servo.h"
00041 #include "encoderKRAI.h"
00042 
00043 /***********************************************/
00044 /*          Konstanta dan Variabel             */
00045 /***********************************************/
00046 #define PI 3.14159265
00047 #define D_ENCODER 0.997     // Diameter Roda Encoder
00048 #define D_ROBOT             // Diameter Roda Robot
00049 #define VMAX 0.3            // Kiri Kanan
00050 #define PIVOT 0.4           // Pivka, Pivki
00051 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
00052 
00053 float k_enc     = PI*D_ENCODER;
00054 
00055 float speedT    = 0.2;
00056 float speedB    = 0.43;
00057 float speedL    = 0.4;
00058 
00059 float vpid = 0;
00060 
00061 float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03;
00062 
00063 /* Deklarasi encoder */
00064 encoderKRAI encoderKiri(      PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
00065 
00066 /* Deklarasi Motor Base */
00067 Motor motor1(PB_7, PA_14, PA_15);  // pwm, fwd, rev, Motor Depan
00068 Motor motor2(PB_8, PA_13, PB_0);   // pwm, fwd, rev, Motor Belakang
00069 
00070 /* Deklarasi Motor Launcher */
00071 Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
00072 Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
00073 
00074 /* Deklarasi Servo Launcher */
00075 Servo servoS(PB_2);
00076 Servo servoB(PA_5);
00077 
00078 /**
00079  *  posX dan posY berdasarkan arah robot
00080  *  encoder Depan & Belakang sejajar sumbu Y
00081  *  encoder Kanan & Kiri sejajar sumbu X 
00082  **/
00083 
00084 /* Variabel Encoder */
00085 float errT, Tetha;                  // Variabel yang didapatkan encoder
00086 
00087 /* Fungsi dan Procedur Encoder */
00088 void setCenter();                   // Fungsi reset agar robot di tengah
00089 float getTetha();                   // Fungsi mendapatkan jarak Tetha
00090 
00091 /* Inisialisasi  Pin TX-RX Joystik dan PC */
00092 joysticknucleo joystick(PA_0,PA_1);
00093 
00094 /* Variabel Stick */
00095 char case_ger;
00096 bool launcher = false, servoGo = false;
00097 
00098 /****************************************************/
00099 /*         Deklarasi Fungsi dan Procedure           */
00100 /****************************************************/
00101 int case_gerak(){
00102 /****************************************************/
00103  **  Gerak Motor Base
00104  **  Case 1  : Pivot kanan
00105  **  Case 2  : Pivot Kiri
00106  **  Case 3  : Kanan
00107  **  Case 4  : Kiri
00108  **  Case 5  : Break
00109  ****************************************************/ 
00110     
00111     int casegerak;
00112     if (!joystick.L1 && joystick.R1) {
00113         // Pivot Kanan
00114         casegerak = 1;
00115     } 
00116     else if (!joystick.R1 && joystick.L1) {
00117         // Pivot Kiri
00118         casegerak = 2;
00119     } 
00120     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
00121         // Kanan
00122         casegerak = 3;
00123     } 
00124     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
00125         // Kiri
00126         casegerak = 4;       
00127     } 
00128     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
00129         // Break
00130         casegerak = 5; 
00131     }
00132     return(casegerak);
00133 }
00134 
00135 void aktuator(){
00136 /* Fungsi untuk menggerakkan servo */
00137     // Servo
00138     if (servoGo){
00139         servoS.position(20);
00140         wait_ms(500);
00141         servoS.position(-28);
00142         wait_ms(500);
00143         servoS.position(20);
00144         wait_ms(500);
00145         for (int i = -0; i<=70; i++){
00146         servoB.position(i);
00147         wait_ms(10);
00148         }
00149         wait_ms(500);
00150         servoB.position(0);
00151         servoGo = false;
00152     }
00153     else{
00154         servoS.position(20);
00155         servoB.position(0);
00156     }
00157     
00158     // Motor Atas
00159     if (launcher) {
00160             motorld.speed(speedL);
00161             motorlb.speed(speedB);
00162     }else{
00163             motorld.speed(0);
00164             motorlb.speed(0);
00165     }
00166     
00167     // MOTOR Bawah
00168     switch (case_ger) {
00169         case (1): {       
00170             // Pivot Kanan
00171             motor1.speed(-PIVOT);
00172             motor2.speed(-PIVOT);
00173             break;
00174             }
00175         case (2): {
00176             // Pivot Kiri
00177             motor1.speed(PIVOT);
00178             motor2.speed(PIVOT);
00179             break;
00180             }
00181         case (3) : {
00182             // Kanan
00183             motor1.speed(-VMAX-vpid);
00184             motor2.speed(VMAX+vpid);
00185             break;
00186             }
00187         case (4) : {
00188             // Kiri
00189             motor1.speed(VMAX-vpid);
00190             motor2.speed(-VMAX+vpid);
00191             break;
00192             }
00193         default : {
00194             motor1.brake(1);
00195             motor2.brake(1);
00196             }   
00197     } // End Switch 
00198 }
00199 
00200 void setCenter(){
00201 /* Fungsi untuk menentukan center dari robot */
00202     encoderKiri.reset();
00203 }
00204 
00205 float getTetha(){
00206 /* Fungsi untuk mendapatkan nilai tetha */
00207     float busurKir;
00208     busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc);
00209     
00210     return -(busurKir);    
00211 }
00212 
00213 void gotoXYT(float Ta){
00214 /* Fungsi untuk bergerak ke target */
00215     float vt;
00216     
00217     errT = Ta-getTetha();
00218     Vt = kp_tetha*errT;
00219     
00220     if (vt>speed1)
00221     { vt = speed1; }
00222     else if (vt<-speed1)
00223     { vt = -speed1; }
00224     
00225     if ((((errT > 0.05) || (errT<-0.05))){ 
00226         vpid = vt;    
00227     }
00228     else{
00229         vpid = 0;
00230     }
00231 }
00232 
00233 void speedLauncher(){
00234 /* Fungsi untuk speed launcher */
00235     if (joystick.R3_click and speedL < 0.8){
00236         speedL = speedL + 0.01; // PWM++ Motor Belakang
00237     }
00238     if (joystick.L3_click and speedL > 0.1){
00239         speedL = speedL - 0.01; // PWM-- Motor Belakang
00240     } 
00241     if (joystick.R2_click and speedB < 0.8 ){
00242         speedB = speedB + 0.01; // PWM++ Motor Depan
00243     }  
00244     if (joystick.L2_click and speedB > 0.1 ){
00245         speedB = speedB - 0.01; // PWM-- Motor Depan
00246     }    
00247 }
00248    
00249 /*********************************************************/
00250 /*                  Main Function                        */
00251 /*********************************************************/
00252 
00253 int main (void){
00254     /* Set baud rate - 115200 */
00255     joystick.setup();
00256     wait_ms(1000);
00257     setCenter();
00258     wait_ms(500);
00259     
00260     /* Posisi Awal */
00261     Tetha = 0;
00262 
00263     /* Untuk mendapatkan serial dari Arduino */
00264     while(1)
00265     {
00266         // Interrupt Serial
00267         joystick.idle();        
00268         
00269         if(joystick.readable() ) {
00270             // Panggil fungsi pembacaan joystik
00271             joystick.baca_data();
00272             
00273             // Panggil fungsi pengolahan data joystik
00274             joystick.olah_data();
00275             
00276             // Masuk ke case gerak
00277             case_ger = case_gerak();
00278             aktuator();
00279 
00280             if (joystick.segitiga_click)    launcher    = !launcher;
00281             if (joystick.lingkaran_click)   servoGo     = true;
00282             speedLauncher();
00283         } 
00284         else {
00285             joystick.idle();    
00286         }
00287     }
00288 }