![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Penyesuaian pin
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_ManualBaseBaruV1 by
main.cpp
- Committer:
- rahmadirizki18
- Date:
- 2017-01-27
- Revision:
- 17:e4229d77a5ab
- Parent:
- 16:90119f03c5d1
- Child:
- 18:1da121ddb7c1
File content as of revision 17:e4229d77a5ab:
/****************************************************************************/ /* 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 */ /* */ /****************************************************************************/ /* */ /* Joystick */ /* kanan => posisi target x ditambah 0.01 */ /* kiri => posisi target x dikurang 0.01 */ /* atas => posisi target y ditambah 0.01 */ /* bawah => posisi target y dikurang 0.01 */ /* */ /* Tombol silang => Kembali keposisi Awal */ /* Tombol segitiga => Aktif motor Launcher */ /* Tombol lingkaran=> Aktif servo Launcher */ /* Tombol L3 => PWM Launcher dikurangin */ /* Tombol R3 => PWM Launcher 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.058 #define D_ROBOT 0.64 #define VMAX 0.3 // Maju, Mundur, Kiri Kanan #define SAMPING 0.3 // Saka, Saki, Sbka, Sbki #define PIVOT 0.4 // Pivka, Pivki #define PERPINDAHAN 1 // Perpindahan ke kanan dan kiri 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 speedB =0.43; float speedL =0.4; float kpX=0.5, kpY=0.5, kp_tetha=0.03; /* Deklarasi encoder */ encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan 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 motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev /* 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 jarak, posX, posY; // float XT, YT, Tetha; // Jarak Target Robot float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder float v1, v2, v3, v4; // Variabel kecepatan motor dari encoder /* Fungsi dan Procedur Encoder */ void setCenter(); // Fungsi reset agar robot di tengah float getY(); // FUngsi mendapatkan jarak Y float getX(); // FUngsi mendapatkan jarak X 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, manual = true, caseTadi = false, caseSekarang = false; /***********************************************/ /* Deklarasi Fungsi dan Procedure */ /***********************************************/ int case_gerak(){ /***************************************************** ** Gerak Motor Base ** Case 1 : Pivot kanan ** Case 2 : Pivot Kiri ** Case 3 : Maju ** Case 4 : Mundur ** Case 5 : Serong Atas Kanan ** Case 6 : Serong Bawah Kanan ** Case 7 : Serong Atas Kiri ** Case 8 : Serong Bawah Kiri ** Case 9 : Kanan ** Case 10 : Kiri ** Case 12 : 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)) { // Maju casegerak = 3; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Mundur casegerak = 4; } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { // Serong Atas Kanan casegerak = 5; } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { // Serong Bawah Kanan casegerak = 6; } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { // Serong Atas Kiri casegerak = 7; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { // Serong Bawah Kiri casegerak = 8; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan casegerak = 9; caseSekarang = true; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri casegerak = 10; caseSekarang = true; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { casegerak = 12; } 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 if (manual) { // Mode Manual switch (case_ger) { case (1): { // Pivot Kanan motor1.speed(-PIVOT); motor2.speed(-PIVOT); motor3.speed(-PIVOT); motor4.speed(-PIVOT); break; } case (2): { // Pivot Kiri motor1.speed(PIVOT); motor2.speed(PIVOT); motor3.speed(PIVOT); motor4.speed(PIVOT); break; } case (3): { // Maju motor1.speed(-VMAX); motor2.speed(VMAX); motor3.speed(VMAX); motor4.speed(-VMAX); break; } case (4): { // Mundur motor1.speed(VMAX); motor2.speed(-VMAX); motor3.speed(-VMAX); motor4.speed(VMAX); break; } case (5) : { // Samping Atas Kanan motor1.speed(-SAMPING); motor2.brake(1); motor3.speed(SAMPING); motor4.brake(1); break; } case (6) : { // Samping Bawah Kanan motor1.brake(1); motor2.speed(-SAMPING); motor3.brake(1); motor4.speed(SAMPING); break; } case (7) : { // Samping Atas Kiri motor1.brake(1); motor2.speed(SAMPING); motor3.brake(1); motor4.speed(-SAMPING); break; } case (8) : { // Samping Bawah Kiri motor1.speed(SAMPING); motor2.brake(1); motor3.speed(-SAMPING); motor4.brake(1); break; } case (9) : { // Kanan motor1.speed(-VMAX); motor2.speed(-VMAX); motor3.speed(VMAX); motor4.speed(VMAX); break; } case (10) : { // Kiri motor1.speed(VMAX); motor2.speed(VMAX); motor3.speed(-VMAX); motor4.speed(-VMAX); break; } default : { motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); } } // End Switch } else { //Mode Encoder switch (case_ger) { case (1):{ Tetha = Tetha - 0.05; break; } case (2):{ Tetha = Tetha + 0.05; break; } case (3):{ YT = YT + 0.01; break; } case (4):{ YT = YT - 0.01; break; } case (5) :{ XT = XT + 0.01; YT = YT + 0.01; break; } case (6) :{ XT = XT + 0.01; YT = YT - 0.01; break; } case (7) :{ XT = XT - 0.01; YT = YT + 0.01; break; } case (8) :{ XT = XT - 0.01; YT = YT - 0.01; break; } case (9) :{ // Kanan if (caseSekarang != caseTadi) XT = XT + PERPINDAHAN; caseTadi = caseSekarang; break; } case (10) :{ // Kiri if (caseSekarang!= caseTadi) XT = XT - PERPINDAHAN; caseTadi = caseSekarang; break; } default :{} } //end of switch } } void setCenter(){ /* Fungsi untuk menentukan center dari robot */ encoderDepan.reset(); encoderBelakang.reset(); encoderKanan.reset(); encoderKiri.reset(); } float getX(){ /* Fungsi untuk mendapatkan jarak X */ float jarakEncDpn, jarakEncBlk; jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc; jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc; return (jarakEncDpn-jarakEncBlk)/2; } float getY(){ /* Fungsi untuk mendapatkan jarak Y */ float jarakEncKir, jarakEncKan; jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc; jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc; return (jarakEncKir-jarakEncKan)/2; } float getTetha(){ /* Fungsi untuk mendapatkan nilai tetha */ float busurDpn, busurBlk, busurKir, busurKan; busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0; busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0; busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc)/k_robot*360.0; busurKan = ((encoderKanan.getPulses())/(float)(720.0)*k_enc)/k_robot*360.0; return -(busurDpn+busurBlk+busurKir+busurKan)/4; } void gotoXYT(float xa, float ya, float Ta){ /* Fungsi untuk bergerat ke target */ errX = xa-getX(); Vx = kpX*errX; errY = ya-getY(); Vy = kpY*errY; errT = Ta-getTetha(); Vt = kp_tetha*errT; v1 = Vx+Vy-Vt; v2 = Vx-Vy-Vt; v3 = -Vx-Vy-Vt; v4 = -Vx+Vy-Vt; if (v1>speed1) { v1 = speed1; } else if (v1<-speed1) { v1 = -speed1; } if (v2>speed2) { v2 = speed2; } else if (v2<-speed2) { v2 = -speed2; } if (v3>speed3) { v3 = speed3; } else if (v3<-speed3) { v3 = -speed3; } if (v4>speed4) { v4 = speed4; } else if (v4<-speed4) { v4 = -speed4; } if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05))){ motor1.speed(v1); motor2.speed(v2); motor3.speed(v3); motor4.speed(v4); } else{ motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); } } void speedLauncher(){ /* Fungsi untuk speed launcher */ if (joystick.R3_click and speedL < 0.8){ speedL = speedL + 0.01; } if (joystick.L3_click and speedL > 0.1){ speedL = speedL - 0.01; } if (joystick.R2_click and speedB < 0.8 ){ speedB = speedB + 0.01; } if (joystick.L2_click and speedB > 0.1 ){ speedB = speedB - 0.01; } } /***********************************************/ /* Main Function */ /***********************************************/ int main (void){ /* Set baud rate - 115200 */ joystick.setup(); wait_ms(1000); setCenter(); wait_ms(500); /* Posisi Awal */ XT = 0; YT = 0; 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; if (joystick.SELECT_click) manual = !manual; if (joystick.silang) { XT = 0; YT = 0; Tetha = 0; } speedLauncher(); } else { joystick.idle(); } gotoXYT(XT,YT,Tetha); } }