baca kiri dan atas
Dependencies: Motor PID mbed millis
Fork of Encoder_Base_Nasioanal by
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
Baca kiri dan kanan
Who changed what in which revision?
User | Revision | Line number | New 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 | } |