main.cpp

00001 /****************************************************************************/
00002 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
00003 /*                                                                          */
00004 /*  - Digunakan encoder autonics                                            */
00005 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
00006 /*        _________________                                                 */
00007 /*        |     DEPAN      |                                                */
00008 /*        | 1.    e     .2 |    Angka ==> Motor                             */
00009 /*        | `            ` |    e     ==> Encoder                           */
00010 /*        | e            e |                                                */
00011 /*        | .            . |                                                */
00012 /*        | 4`    e     `3 |                                                */
00013 /*        |________________|                                                */
00014 /*                                                                          */
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 #include "mbed.h"
00022 #include "Motor.h"
00023 #include "omniPos.h"
00024 
00025 #define diameterRoda 10
00026 #define delta_t 0.001
00027 
00028 // Deklarasi variabel motor
00029 Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
00030 Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
00031 Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
00032 Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
00033 
00034 // Deklarasi variabel encoder
00035 //~ Dkiri untuk Motor 1
00036 //~ Dkanan untuk Motor 2        
00037 //~ Bkanan untuk Motor 3        
00038 //~ Bkiri untuk Motor 4
00039 omniPos omni1(Dkiri);
00040 omniPos omni2(Dkanan);
00041 omniPos omni3(Bkiri);
00042 omniPos omni4(Bkiri);
00043 
00044 // Inisialisasi  Pin TX-RX Joystik dan PC
00045 Serial pc(USBTX,USBRX);
00046 
00047 // Deklarasi Variabel Global
00048 /*
00049  * posX dan posY berdasarkan arah robot
00050  * encoder Depan & Belakang sejajar sumbu Y
00051  * encoder Kanan & Kiri sejajar sumbu X 
00052  */
00053 float jarak, posX, posY;
00054 float keliling = pi*diameterRoda;
00055 
00056 void detect_encoder()
00057 {
00058     int pv;
00059     // Motor1
00060     
00061     PID.setProcessValue(omni1.getVel(delta_t));
00062     PID.setSetPoint();
00063 }
00064 
00065 int main()
00066 {
00067     pc.baud(115200);
00068     
00069 }
00070 
00071 void gerakKanan()
00072 {
00073     if(vcurr<0.1) {
00074         vcurr=0.1;
00075     } else {
00076         vcurr+=ax;
00077     }
00078     //perlambatan=0;
00079             } else {
00080                 //perlambatan=1;
00081             } 
00082             
00083             if (vcurr>=vmax) { 
00084                 vcurr=vmax; 
00085             }
00086             
00087             if(joystick.R2==255 && joystick.L2==0) { 
00088                 koef=2;  
00089             } else if (joystick.L2==255 && joystick.R2==0)  { 
00090                 koef=0.5;
00091             } else { 
00092                 koef=1;  
00093             }
00094             
00095             s1 =(float)(-1*koef*vcurr);
00096             s2 =(float)(-1.0*koef*vcurr); 
00097             s3 =(float)(1*koef*vcurr); 
00098             s4 =(float)(1.0*koef*vcurr);           
00099             
00100             kanan=true; 
00101             maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
00102             
00103             pc.printf("Kanan\n");
00104             
00105             motor1.speed(s1);
00106             motor2.speed(s2);
00107             motor3.speed(s3);
00108             motor4.speed(s4);
00109             break;
00110         }    
00111 }
00112 
00113 void gerakKiri()
00114 {
00115     float revDepan, revBelakang, revKanan, revKiri;
00116     float tempX, tempY;
00117     
00118     revDepan    = encoderDepan.getRevolutions();
00119     revBelakang = encoderBelakang.getRevolutions();
00120     revKanan    = encoderKanan.getRevolutions();
00121     revKiri     = encoderKiri.getRevolutions();    
00122 }