Gustav Aditya Permana
/
Riset-Odometry
Coba dulu
main.cpp
- Committer:
- gustavaditya
- Date:
- 2016-11-07
- Revision:
- 0:b455cd43929c
File content as of revision 0:b455cd43929c:
/****************************************************************************/ /* PROGRAM UNTUK PID CLOSED LOOP */ /* */ /* - Digunakan encoder autonics */ /* - Konfigurasi Motor dan Encoder sbb : */ /* _________________ */ /* | DEPAN | */ /* | 1. e .2 | Angka ==> Motor */ /* | ` ` | e ==> Encoder */ /* | e e | */ /* | . . | */ /* | 4` e `3 | */ /* |________________| */ /* */ /* SETTINGS (WAJIB!) : */ /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ /* */ /****************************************************************************/ #include "mbed.h" #include "Motor.h" #include "omniPos.h" #define diameterRoda 10 #define delta_t 0.001 // Deklarasi variabel motor Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev // Deklarasi variabel encoder //~ Dkiri untuk Motor 1 //~ Dkanan untuk Motor 2 //~ Bkanan untuk Motor 3 //~ Bkiri untuk Motor 4 omniPos omni1(Dkiri); omniPos omni2(Dkanan); omniPos omni3(Bkiri); omniPos omni4(Bkiri); // Inisialisasi Pin TX-RX Joystik dan PC Serial pc(USBTX,USBRX); // Deklarasi Variabel Global /* * posX dan posY berdasarkan arah robot * encoder Depan & Belakang sejajar sumbu Y * encoder Kanan & Kiri sejajar sumbu X */ float jarak, posX, posY; float keliling = pi*diameterRoda; void detect_encoder() { int pv; // Motor1 PID.setProcessValue(omni1.getVel(delta_t)); PID.setSetPoint(); } int main() { pc.baud(115200); } void gerakKanan() { if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmax) { vcurr=vmax; } if(joystick.R2==255 && joystick.L2==0) { koef=2; } else if (joystick.L2==255 && joystick.R2==0) { koef=0.5; } else { koef=1; } s1 =(float)(-1*koef*vcurr); s2 =(float)(-1.0*koef*vcurr); s3 =(float)(1*koef*vcurr); s4 =(float)(1.0*koef*vcurr); kanan=true; maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("Kanan\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } } void gerakKiri() { float revDepan, revBelakang, revKanan, revKiri; float tempX, tempY; revDepan = encoderDepan.getRevolutions(); revBelakang = encoderBelakang.getRevolutions(); revKanan = encoderKanan.getRevolutions(); revKiri = encoderKiri.getRevolutions(); }