KRAI 2017 / Mbed 2 deprecated Encoder_Base_Nasioanal_awalv1

jalan kiri

Dependencies:   Motor PID mbed millis

Fork of Encoder_Base_NasioanalV1 by KRAI 2017

main.cpp@3:db2dc06c0686, 2017-05-30 (annotated)

Committer:
rahmadirizki18
Date:
Tue May 30 14:11:07 2017 +0000
Revision:
3:db2dc06c0686
Parent:
2:9cf609b74de9 
jalan kiri

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gustavaditya 0:57124be75a63 1 /****************************************************************************/
gustavaditya 0:57124be75a63 2 /* PROGRAM UNTUK CLOSED LOOP BASE */
gustavaditya 0:57124be75a63 3 /* */
gustavaditya 0:57124be75a63 4 /* Last Update : 29 Mei 2017 */
gustavaditya 0:57124be75a63 5 /* */
gustavaditya 0:57124be75a63 6 /* - Digunakan encoder bawaan motor */
gustavaditya 0:57124be75a63 7 /* - Konfigurasi Motor dan Encoder sbb : */
gustavaditya 0:57124be75a63 8 /* ______________________ */
gustavaditya 0:57124be75a63 9 /* / \ Rode Depan Belakang: */
gustavaditya 0:57124be75a63 10 /* / 1 (Depan) \ Omniwheel */
gustavaditya 0:57124be75a63 11 /* | | */
gustavaditya 0:57124be75a63 12 /* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */
gustavaditya 0:57124be75a63 13 /* | | Omniwheel */
gustavaditya 0:57124be75a63 14 /* \ 2 (Belakang) / */
gustavaditya 0:57124be75a63 15 /* \______________________/ Putaran CW tampak depan */
gustavaditya 0:57124be75a63 16 /* positif */
gustavaditya 0:57124be75a63 17 /* SETTINGS (WAJIB!) : */
gustavaditya 0:57124be75a63 18 /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding */
gustavaditya 0:57124be75a63 19 /* 2. Deklarasi penggunaan library pada bagian deklarasi encoder */
rahmadirizki18 3:db2dc06c0686 20 /* 3. !arah jarum jam positif 517 pulse 1 putaran enc Atas, 524 kiri */
gustavaditya 0:57124be75a63 21 /* */
gustavaditya 0:57124be75a63 22 /****************************************************************************/
gustavaditya 0:57124be75a63 23
gustavaditya 0:57124be75a63 24 #include "mbed.h"
gustavaditya 0:57124be75a63 25 #include "JoystickPS3.h"
gustavaditya 0:57124be75a63 26 #include "Motor.h"
gustavaditya 0:57124be75a63 27 #include "encoderKRAI.h"
gustavaditya 0:57124be75a63 28 #include "millis.h"
gustavaditya 0:57124be75a63 29
gustavaditya 0:57124be75a63 30 #define PI 3.14159265
gustavaditya 0:57124be75a63 31 #define D_ENCODER 10 // Diameter Roda Encoder
gustavaditya 1:7d3041941b1b 32 #define D_ROBOT 55 // Diameter Roda Robot
gustavaditya 1:7d3041941b1b 33
gustavaditya 1:7d3041941b1b 34 float K_enc = PI*D_ENCODER;
gustavaditya 1:7d3041941b1b 35 float K_robot = PI*D_ROBOT;
gustavaditya 1:7d3041941b1b 36
gustavaditya 1:7d3041941b1b 37 float speed1=0.6;
gustavaditya 1:7d3041941b1b 38 float speed2=0.6;
gustavaditya 1:7d3041941b1b 39 float speed3=0.6;
gustavaditya 1:7d3041941b1b 40 float speed4=0.6;
gustavaditya 1:7d3041941b1b 41
gustavaditya 1:7d3041941b1b 42 float errX, errY, errT, Vt, Vx, Vy, KpX=1.5, KpY=1.5, Kp_tetha=0.15;
gustavaditya 1:7d3041941b1b 43 float V1, V2, V3, V4;
gustavaditya 0:57124be75a63 44
gustavaditya 0:57124be75a63 45 // Variable Bawah
gustavaditya 0:57124be75a63 46 float tuneDpn = 1.0; // Tunning PWM motor Depan
gustavaditya 0:57124be75a63 47 float tuneBlk = 1.0; // Tunning PWM motor belakang
gustavaditya 0:57124be75a63 48
gustavaditya 0:57124be75a63 49 /* Inisialisasi Pin TX-RX Joystik dan PC */
gustavaditya 0:57124be75a63 50 joysticknucleo joystick(PA_0,PA_1);
gustavaditya 0:57124be75a63 51 Serial pc(USBTX,USBRX);
gustavaditya 0:57124be75a63 52
gustavaditya 0:57124be75a63 53 /* Deklarasi Encoder Launcher */
rahmadirizki18 3:db2dc06c0686 54 encoderKRAI encBawah( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
rahmadirizki18 3:db2dc06c0686 55 encoderKRAI encDepan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
gustavaditya 0:57124be75a63 56
gustavaditya 0:57124be75a63 57 /* Deklarasi Motor Base */
rahmadirizki18 2:9cf609b74de9 58 Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
gustavaditya 0:57124be75a63 59 Motor motorBlk(PB_2, PB_15, PB_1);
gustavaditya 0:57124be75a63 60 Motor motorL (PB_9, PA_12, PA_6);
rahmadirizki18 2:9cf609b74de9 61 Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5);
rahmadirizki18 2:9cf609b74de9 62
rahmadirizki18 2:9cf609b74de9 63 void setCenter(){
rahmadirizki18 2:9cf609b74de9 64 /* Fungsi untuk menentukan center dari robot */
rahmadirizki18 3:db2dc06c0686 65 encBawah.reset();
rahmadirizki18 3:db2dc06c0686 66 encDepan.reset();
rahmadirizki18 2:9cf609b74de9 67 }
rahmadirizki18 2:9cf609b74de9 68
rahmadirizki18 2:9cf609b74de9 69 int main(void){
rahmadirizki18 3:db2dc06c0686 70 double p_samp=0,error=0,kp=0.082;
rahmadirizki18 2:9cf609b74de9 71 pc.baud(115200);
rahmadirizki18 2:9cf609b74de9 72 setCenter();
rahmadirizki18 3:db2dc06c0686 73
rahmadirizki18 3:db2dc06c0686 74 while(encDepan.getPulses()<5740){
rahmadirizki18 3:db2dc06c0686 75 pc.printf("error %d, p_samp %lf \n",(int)encDepan.getPulses()+(int)encBawah.getPulses(),p_samp);
rahmadirizki18 3:db2dc06c0686 76 error = encDepan.getPulses()+encBawah.getPulses();
rahmadirizki18 3:db2dc06c0686 77 p_samp = kp * error;
rahmadirizki18 3:db2dc06c0686 78 motorDpn.speed(0.7-p_samp);
rahmadirizki18 3:db2dc06c0686 79 motorBlk.speed(-0.7);
rahmadirizki18 3:db2dc06c0686 80 wait_ms(12.5);
rahmadirizki18 3:db2dc06c0686 81 if(p_samp>1.5)p_samp = 1.5;
rahmadirizki18 3:db2dc06c0686 82 if(p_samp<-0.3)p_samp = -0.3;
rahmadirizki18 2:9cf609b74de9 83 }
rahmadirizki18 3:db2dc06c0686 84 motorDpn.speed(0);
rahmadirizki18 3:db2dc06c0686 85 motorBlk.speed(0);
rahmadirizki18 2:9cf609b74de9 86 }