main.cpp

00001 /****************************************************************************/
00002 /*                  PROGRAM UNTUK CLOSED LOOP BASE                          */
00003 /*                                                                          */
00004 /*                  Last Update : 29 Mei 2017                               */
00005 /*                                                                          */
00006 /*  - Digunakan encoder bawaan motor                                        */
00007 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
00008 /*                      ______________________                              */
00009 /*                     /                      \    Rode Depan Belakang:     */
00010 /*                    /      1 (Depan)         \   Omniwheel                */
00011 /*                   |                          |                           */
00012 /*                   | 3 (kiri)       4 (kanan) |  Roda Kiri Kanan:         */
00013 /*                   |                          |  Omniwheel                */
00014 /*                    \       2 (Belakang)     /                            */
00015 /*                     \______________________/    Putaran CW tampak depan  */
00016 /*                                                 positif                  */
00017 /*   SETTINGS (WAJIB!) :                                                    */
00018 /*   1. Settings Pin Encoder, Resolusi, dan Tipe encoding                   */
00019 /*   2. Deklarasi penggunaan library pada bagian deklarasi encoder          */
00020 /*   3. !arah jarum jam positif 517 pulse 1 putaran enc Atas, 524 kiri      */
00021 /*                                                                          */
00022 /****************************************************************************/
00023 
00024 #include "mbed.h"
00025 #include "JoystickPS3.h"
00026 #include "Motor.h"
00027 #include "encoderKRAI.h"
00028 #include "millis.h"
00029 
00030 #define PI 3.14159265
00031 #define D_ENCODER 10        // Diameter Roda Encoder
00032 #define D_ROBOT 55          // Diameter Roda Robot
00033 
00034 float K_enc = PI*D_ENCODER;
00035 float K_robot = PI*D_ROBOT;
00036 
00037 float speed1=0.6;
00038 float speed2=0.6;
00039 float speed3=0.6;
00040 float speed4=0.6;
00041 
00042 float errX, errY, errT, Vt, Vx, Vy, KpX=1.5, KpY=1.5, Kp_tetha=0.15;
00043 float V1, V2, V3, V4;
00044 
00045 // Variable Bawah
00046 float tuneDpn           = 1.0;     // Tunning PWM motor Depan
00047 float tuneBlk           = 1.0;     // Tunning PWM motor belakang
00048 
00049 /* Inisialisasi  Pin TX-RX Joystik dan PC */
00050 joysticknucleo joystick(PA_0,PA_1);
00051 Serial pc(USBTX,USBRX);
00052 
00053 /* Deklarasi Encoder Launcher */
00054 encoderKRAI encBawah( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
00055 encoderKRAI encDepan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
00056 
00057 /* Deklarasi Motor Base */
00058 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); 
00059 Motor motorBlk(PB_2, PB_15, PB_1);
00060 Motor motorL  (PB_9, PA_12, PA_6);  
00061 Motor motorR  (PB_8, PC_6, PC_5);   //(PC_6, PB_4, PB_5);
00062 
00063 void setCenter(){
00064 /* Fungsi untuk menentukan center dari robot */
00065     encBawah.reset();
00066     encDepan.reset();
00067 }
00068 
00069 int main(void){
00070     double p_samp=0,error=0,kp=0.082;
00071     pc.baud(115200);
00072     setCenter();
00073     
00074     while(encDepan.getPulses()<5740){
00075         pc.printf("error %d, p_samp %lf \n",(int)encDepan.getPulses()+(int)encBawah.getPulses(),p_samp);
00076         error = encDepan.getPulses()+encBawah.getPulses();
00077         p_samp = kp * error;
00078         motorDpn.speed(0.7-p_samp);
00079         motorBlk.speed(-0.7);
00080         wait_ms(12.5); 
00081         if(p_samp>1.5)p_samp = 1.5;
00082         if(p_samp<-0.3)p_samp = -0.3;
00083     }
00084     motorDpn.speed(0);
00085     motorBlk.speed(0);
00086 }