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@2:9cf609b74de9, 2017-05-29 (annotated)

Committer:
rahmadirizki18
Date:
Mon May 29 17:01:57 2017 +0000
Revision:
2:9cf609b74de9
Parent:
1:7d3041941b1b
Child:
3:db2dc06c0686
Baca kiri dan kanan

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 2:9cf609b74de9 20 /* 3. Searah 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 2:9cf609b74de9 54 encoderKRAI encKiri( PB_5, PB_4, 28, encoderKRAI::X4_ENCODING);
rahmadirizki18 2:9cf609b74de9 55 encoderKRAI encAtas( PA_15, PA_14, 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 2:9cf609b74de9 65 encKiri.reset();
rahmadirizki18 2:9cf609b74de9 66 encAtas.reset();
rahmadirizki18 2:9cf609b74de9 67 }
rahmadirizki18 2:9cf609b74de9 68
rahmadirizki18 2:9cf609b74de9 69 int main(void){
rahmadirizki18 2:9cf609b74de9 70 pc.baud(115200);
rahmadirizki18 2:9cf609b74de9 71 setCenter();
rahmadirizki18 2:9cf609b74de9 72 while(1){
rahmadirizki18 2:9cf609b74de9 73 pc.printf("kiri %d, atas %d \n",(int)encKiri.getPulses(),(int)encAtas.getPulses());
rahmadirizki18 2:9cf609b74de9 74 //wait_ms(500);
rahmadirizki18 2:9cf609b74de9 75 }
rahmadirizki18 2:9cf609b74de9 76 }