Update 12 Maret 2017. Tambahan kombinasi tombol
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of MainProgram_BaseBaru_20Feb_malam by
main.cpp
- Committer:
- MarchioKevin
- Date:
- 2017-01-28
- Revision:
- 22:4632f58461e0
- Parent:
- 21:da2f3d04468f
- Child:
- 23:023b522977b2
File content as of revision 22:4632f58461e0:
/****************************************************************************/ /* PROGRAM UNTUK PID CLOSED LOOP */ /* */ /* - Digunakan encoder autonics */ /* - Konfigurasi Motor dan Encoder sbb : */ /* ______________________ */ /* / \ Rode Depan Belakang: */ /* / 2 (Belakang) \ Omniwheel */ /* | | */ /* | 3 (Encoder) 4 | Roda Kiri Kanan: */ /* | | Fixed Wheel */ /* \ 1 (Depan) / */ /* \______________________/ Putaran berlawanan arah */ /* jarum jam positif */ /* SETTINGS (WAJIB!) : */ /* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ /* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ /* */ /****************************************************************************/ /* */ /* Joystick */ /* Kanan => Posisi target x ditambah 'perpindahan' */ /* Kiri => Posisi target x dikurang 'perpindahan' */ /* */ /* Tombol silang => Kembali keposisi Awal */ /* Tombol segitiga => Aktif motor Launcher */ /* Tombol lingkaran=> Aktif servo Launcher */ /* Tombol L1 => Pivot Kiri */ /* Tombol R1 => Pivot Kanan */ /* Tombol L3 => PWM Launcher Belakang dikurangin */ /* Tombol R3 => PWM Launcher Belakang ditambahin */ /* */ /* Bismillahirahmanirrahim */ /* Jagalah Kebersihan Kodingan */ /****************************************************************************/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #include "Servo.h" #include "encoderKRAI.h" /***********************************************/ /* Konstanta dan Variabel */ /***********************************************/ #define PI 3.14159265 #define D_ENCODER 0.997 // Diameter Roda Encoder #define D_ROBOT // Diameter Roda Robot #define VMAX 0.3 // Kiri Kanan #define PIVOT 0.4 // Pivka, Pivki #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri float k_enc = PI*D_ENCODER; float speedT = 0.2; float speedB = 0.43; float speedL = 0.4; float vpid = 0; float kpX = 0.5, kpY = 0.5, kp_tetha = 0.03; /* Deklarasi encoder */ encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan /* Deklarasi Motor Base */ Motor motor1(PB_7, PA_14, PA_15); // pwm, fwd, rev, Motor Depan Motor motor2(PB_8, PA_13, PB_0); // pwm, fwd, rev, Motor Belakang /* Deklarasi Motor Launcher */ Motor motorld(PA_8, PC_1, PC_2); // pwm, fwd, rev Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev /* Deklarasi Servo Launcher */ Servo servoS(PB_2); Servo servoB(PA_5); /** * posX dan posY berdasarkan arah robot * encoder Depan & Belakang sejajar sumbu Y * encoder Kanan & Kiri sejajar sumbu X **/ /* Variabel Encoder */ float errT, Tetha; // Variabel yang didapatkan encoder /* Fungsi dan Procedur Encoder */ void setCenter(); // Fungsi reset agar robot di tengah float getTetha(); // Fungsi mendapatkan jarak Tetha /* Inisialisasi Pin TX-RX Joystik dan PC */ joysticknucleo joystick(PA_0,PA_1); /* Variabel Stick */ char case_ger; bool launcher = false, servoGo = false; /****************************************************/ /* Deklarasi Fungsi dan Procedure */ /****************************************************/ int case_gerak(){ /****************************************************/ ** Gerak Motor Base ** Case 1 : Pivot kanan ** Case 2 : Pivot Kiri ** Case 3 : Kanan ** Case 4 : Kiri ** Case 5 : Break ****************************************************/ int casegerak; if (!joystick.L1 && joystick.R1) { // Pivot Kanan casegerak = 1; } else if (!joystick.R1 && joystick.L1) { // Pivot Kiri casegerak = 2; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan casegerak = 3; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri casegerak = 4; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Break casegerak = 5; } return(casegerak); } void aktuator(){ /* Fungsi untuk menggerakkan servo */ // Servo if (servoGo){ servoS.position(20); wait_ms(500); servoS.position(-28); wait_ms(500); servoS.position(20); wait_ms(500); for (int i = -0; i<=70; i++){ servoB.position(i); wait_ms(10); } wait_ms(500); servoB.position(0); servoGo = false; } else{ servoS.position(20); servoB.position(0); } // Motor Atas if (launcher) { motorld.speed(speedL); motorlb.speed(speedB); }else{ motorld.speed(0); motorlb.speed(0); } // MOTOR Bawah switch (case_ger) { case (1): { // Pivot Kanan motor1.speed(-PIVOT); motor2.speed(-PIVOT); break; } case (2): { // Pivot Kiri motor1.speed(PIVOT); motor2.speed(PIVOT); break; } case (3) : { // Kanan motor1.speed(-VMAX-vpid); motor2.speed(VMAX+vpid); break; } case (4) : { // Kiri motor1.speed(VMAX-vpid); motor2.speed(-VMAX+vpid); break; } default : { motor1.brake(1); motor2.brake(1); } } // End Switch } void setCenter(){ /* Fungsi untuk menentukan center dari robot */ encoderKiri.reset(); } float getTetha(){ /* Fungsi untuk mendapatkan nilai tetha */ float busurKir; busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); return -(busurKir); } void gotoXYT(float Ta){ /* Fungsi untuk bergerak ke target */ float vt; errT = Ta-getTetha(); Vt = kp_tetha*errT; if (vt>speed1) { vt = speed1; } else if (vt<-speed1) { vt = -speed1; } if ((((errT > 0.05) || (errT<-0.05))){ vpid = vt; } else{ vpid = 0; } } void speedLauncher(){ /* Fungsi untuk speed launcher */ if (joystick.R3_click and speedL < 0.8){ speedL = speedL + 0.01; // PWM++ Motor Belakang } if (joystick.L3_click and speedL > 0.1){ speedL = speedL - 0.01; // PWM-- Motor Belakang } if (joystick.R2_click and speedB < 0.8 ){ speedB = speedB + 0.01; // PWM++ Motor Depan } if (joystick.L2_click and speedB > 0.1 ){ speedB = speedB - 0.01; // PWM-- Motor Depan } } /*********************************************************/ /* Main Function */ /*********************************************************/ int main (void){ /* Set baud rate - 115200 */ joystick.setup(); wait_ms(1000); setCenter(); wait_ms(500); /* Posisi Awal */ Tetha = 0; /* Untuk mendapatkan serial dari Arduino */ while(1) { // Interrupt Serial joystick.idle(); if(joystick.readable() ) { // Panggil fungsi pembacaan joystik joystick.baca_data(); // Panggil fungsi pengolahan data joystik joystick.olah_data(); // Masuk ke case gerak case_ger = case_gerak(); aktuator(); if (joystick.segitiga_click) launcher = !launcher; if (joystick.lingkaran_click) servoGo = true; speedLauncher(); } else { joystick.idle(); } } }