CLOSED LOOP BASE
Dependencies: Motor PID mbed millis
main.cpp@1:7d3041941b1b, 2017-05-29 (annotated)
- Committer:
- gustavaditya
- Date:
- Mon May 29 15:46:48 2017 +0000
- Revision:
- 1:7d3041941b1b
- Parent:
- 0:57124be75a63
CLOSED LOOP BASE
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 */ |
gustavaditya | 0:57124be75a63 | 20 | /* */ |
gustavaditya | 0:57124be75a63 | 21 | /****************************************************************************/ |
gustavaditya | 0:57124be75a63 | 22 | |
gustavaditya | 0:57124be75a63 | 23 | #include "mbed.h" |
gustavaditya | 0:57124be75a63 | 24 | #include "JoystickPS3.h" |
gustavaditya | 0:57124be75a63 | 25 | #include "Motor.h" |
gustavaditya | 0:57124be75a63 | 26 | #include "encoderKRAI.h" |
gustavaditya | 0:57124be75a63 | 27 | #include "millis.h" |
gustavaditya | 0:57124be75a63 | 28 | |
gustavaditya | 0:57124be75a63 | 29 | #define PI 3.14159265 |
gustavaditya | 0:57124be75a63 | 30 | #define D_ENCODER 10 // Diameter Roda Encoder |
gustavaditya | 1:7d3041941b1b | 31 | #define D_ROBOT 55 // Diameter Roda Robot |
gustavaditya | 1:7d3041941b1b | 32 | |
gustavaditya | 1:7d3041941b1b | 33 | float K_enc = PI*D_ENCODER; |
gustavaditya | 1:7d3041941b1b | 34 | float K_robot = PI*D_ROBOT; |
gustavaditya | 1:7d3041941b1b | 35 | |
gustavaditya | 1:7d3041941b1b | 36 | float speed1=0.6; |
gustavaditya | 1:7d3041941b1b | 37 | float speed2=0.6; |
gustavaditya | 1:7d3041941b1b | 38 | float speed3=0.6; |
gustavaditya | 1:7d3041941b1b | 39 | float speed4=0.6; |
gustavaditya | 1:7d3041941b1b | 40 | |
gustavaditya | 1:7d3041941b1b | 41 | float errX, errY, errT, Vt, Vx, Vy, KpX=1.5, KpY=1.5, Kp_tetha=0.15; |
gustavaditya | 1:7d3041941b1b | 42 | float V1, V2, V3, V4; |
gustavaditya | 0:57124be75a63 | 43 | |
gustavaditya | 0:57124be75a63 | 44 | // Variable Bawah |
gustavaditya | 0:57124be75a63 | 45 | float tuneDpn = 1.0; // Tunning PWM motor Depan |
gustavaditya | 0:57124be75a63 | 46 | float tuneBlk = 1.0; // Tunning PWM motor belakang |
gustavaditya | 0:57124be75a63 | 47 | |
gustavaditya | 0:57124be75a63 | 48 | /* Inisialisasi Pin TX-RX Joystik dan PC */ |
gustavaditya | 0:57124be75a63 | 49 | joysticknucleo joystick(PA_0,PA_1); |
gustavaditya | 0:57124be75a63 | 50 | Serial pc(USBTX,USBRX); |
gustavaditya | 0:57124be75a63 | 51 | |
gustavaditya | 0:57124be75a63 | 52 | /* Deklarasi Encoder Launcher */ |
gustavaditya | 0:57124be75a63 | 53 | encoderKRAI encKiri( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); |
gustavaditya | 0:57124be75a63 | 54 | encoderKRAI encKanan( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); |
gustavaditya | 0:57124be75a63 | 55 | |
gustavaditya | 0:57124be75a63 | 56 | /* Deklarasi Motor Base */ |
gustavaditya | 0:57124be75a63 | 57 | Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); |
gustavaditya | 0:57124be75a63 | 58 | Motor motorBlk(PB_2, PB_15, PB_1); |
gustavaditya | 0:57124be75a63 | 59 | Motor motorL (PB_9, PA_12, PA_6); |
gustavaditya | 0:57124be75a63 | 60 | Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5); |