![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Penyesuaian pin
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_ManualBaseBaruV1 by
Diff: main.cpp
- Revision:
- 22:4632f58461e0
- Parent:
- 21:da2f3d04468f
- Child:
- 23:023b522977b2
--- a/main.cpp Sat Jan 28 07:24:07 2017 +0000 +++ b/main.cpp Sat Jan 28 10:19:36 2017 +0000 @@ -3,15 +3,15 @@ /* */ /* - Digunakan encoder autonics */ /* - Konfigurasi Motor dan Encoder sbb : */ -/* _________________ */ -/* | DEPAN | */ -/* | 1. e .2 | Angka ==> Motor */ -/* | ` ` | e ==> Encoder */ -/* | e e | */ -/* | . . | */ -/* | 4` e `3 | */ -/* |________________| */ -/* */ +/* ______________________ */ +/* / \ 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 */ @@ -21,14 +21,14 @@ /* Joystick */ /* Kanan => Posisi target x ditambah 'perpindahan' */ /* Kiri => Posisi target x dikurang 'perpindahan' */ -/* 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 */ +/* Tombol L1 => Pivot Kiri */ +/* Tombol R1 => Pivot Kanan */ +/* Tombol L3 => PWM Launcher Belakang dikurangin */ +/* Tombol R3 => PWM Launcher Belakang ditambahin */ /* */ /* Bismillahirahmanirrahim */ /* Jagalah Kebersihan Kodingan */ @@ -44,36 +44,28 @@ /* 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 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 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 speedT = 0.2; +float speedB = 0.43; +float speedL = 0.4; -float kpX=0.5, kpY=0.5, kp_tetha=0.03; +float vpid = 0; + +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 +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 @@ -90,15 +82,10 @@ **/ /* 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 +float errT, Tetha; // Variabel yang didapatkan 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 */ @@ -106,27 +93,21 @@ /* Variabel Stick */ char case_ger; -bool launcher = false, servoGo = false, manual = true; -int caseSebelum; +bool launcher = false, servoGo = false; -/***********************************************/ -/* Deklarasi Fungsi dan Procedure */ -/***********************************************/ +/****************************************************/ +/* 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 + ** Case 3 : Kanan + ** Case 4 : Kiri + ** Case 5 : Break ****************************************************/ + int casegerak; if (!joystick.L1 && joystick.R1) { // Pivot Kanan @@ -136,40 +117,17 @@ // 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; + casegerak = 3; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri - casegerak = 10; + casegerak = 4; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { - casegerak = 12; + // Break + casegerak = 5; } return(casegerak); } @@ -207,257 +165,90 @@ } // MOTOR Bawah - if (manual) { - // Mode Manual - switch (case_ger) { + 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); + } + case (3) : { + // Kanan + motor1.speed(-VMAX-vpid); + motor2.speed(VMAX+vpid); 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) : { + } + case (4) : { // Kiri - motor1.speed(VMAX); - motor2.speed(VMAX); - motor3.speed(-VMAX); - motor4.speed(-VMAX); + motor1.speed(VMAX-vpid); + motor2.speed(-VMAX+vpid); break; } default : { - motor1.brake(1); - motor2.brake(1); - motor3.brake(1); - motor4.brake(1); + motor1.brake(1); + motor2.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 (case_ger != caseSebelum) - XT = XT + PERPINDAHAN; - break; - } - case (10) :{ - // Kiri - if (case_ger != caseSebelum) - XT = XT - PERPINDAHAN; - break; - } - default :{} - } // End of Switch - caseSebelum = case_ger; - } + } // End 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; + float busurKir; + busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*k_enc); - return -(busurDpn+busurBlk+busurKir+busurKan)/4; + return -(busurKir); } -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; +void gotoXYT(float Ta){ +/* Fungsi untuk bergerak ke target */ + float vt; 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 (vt>speed1) + { vt = speed1; } + else if (vt<-speed1) + { vt = -speed1; } - 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); + if ((((errT > 0.05) || (errT<-0.05))){ + vpid = vt; } else{ - motor1.brake(1); - motor2.brake(1); - motor3.brake(1); - motor4.brake(1); + vpid = 0; } } void speedLauncher(){ /* Fungsi untuk speed launcher */ if (joystick.R3_click and speedL < 0.8){ - speedL = speedL + 0.01; + speedL = speedL + 0.01; // PWM++ Motor Belakang } if (joystick.L3_click and speedL > 0.1){ - speedL = speedL - 0.01; + speedL = speedL - 0.01; // PWM-- Motor Belakang } if (joystick.R2_click and speedB < 0.8 ){ - speedB = speedB + 0.01; + speedB = speedB + 0.01; // PWM++ Motor Depan } if (joystick.L2_click and speedB > 0.1 ){ - speedB = speedB - 0.01; + speedB = speedB - 0.01; // PWM-- Motor Depan } } -/***********************************************/ -/* Main Function */ -/***********************************************/ +/*********************************************************/ +/* Main Function */ +/*********************************************************/ int main (void){ /* Set baud rate - 115200 */ @@ -467,8 +258,6 @@ wait_ms(500); /* Posisi Awal */ - XT = 0; - YT = 0; Tetha = 0; /* Untuk mendapatkan serial dari Arduino */ @@ -490,18 +279,10 @@ 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(); } - if (!manual) - gotoXYT(XT,YT,Tetha); } }