CLOSED LOOP BASE
Dependencies: Motor PID mbed millis
main.cpp
- Committer:
- gustavaditya
- Date:
- 2017-05-29
- Revision:
- 1:7d3041941b1b
- Parent:
- 0:57124be75a63
File content as of revision 1:7d3041941b1b:
/****************************************************************************/ /* PROGRAM UNTUK CLOSED LOOP BASE */ /* */ /* Last Update : 29 Mei 2017 */ /* */ /* - Digunakan encoder bawaan motor */ /* - Konfigurasi Motor dan Encoder sbb : */ /* ______________________ */ /* / \ Rode Depan Belakang: */ /* / 1 (Depan) \ Omniwheel */ /* | | */ /* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */ /* | | Omniwheel */ /* \ 2 (Belakang) / */ /* \______________________/ Putaran CW tampak depan */ /* positif */ /* SETTINGS (WAJIB!) : */ /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding */ /* 2. Deklarasi penggunaan library pada bagian deklarasi encoder */ /* */ /****************************************************************************/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #include "encoderKRAI.h" #include "millis.h" #define PI 3.14159265 #define D_ENCODER 10 // Diameter Roda Encoder #define D_ROBOT 55 // Diameter Roda Robot float K_enc = PI*D_ENCODER; float K_robot = PI*D_ROBOT; float speed1=0.6; float speed2=0.6; float speed3=0.6; float speed4=0.6; float errX, errY, errT, Vt, Vx, Vy, KpX=1.5, KpY=1.5, Kp_tetha=0.15; float V1, V2, V3, V4; // Variable Bawah float tuneDpn = 1.0; // Tunning PWM motor Depan float tuneBlk = 1.0; // Tunning PWM motor belakang /* Inisialisasi Pin TX-RX Joystik dan PC */ joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); /* Deklarasi Encoder Launcher */ encoderKRAI encKiri( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); encoderKRAI encKanan( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); /* Deklarasi Motor Base */ Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5); Motor motorBlk(PB_2, PB_15, PB_1); Motor motorL (PB_9, PA_12, PA_6); Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5);