![](/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:
- 4:483c07cc22e1
- Parent:
- 3:1287fccc11be
- Child:
- 5:3aa203218306
--- a/main.cpp Sun Oct 16 06:27:39 2016 +0000 +++ b/main.cpp Tue Oct 25 13:21:57 2016 +0000 @@ -26,14 +26,15 @@ #define vmax 1 #define vmaxserong 0.9 #define vmaxpivot 0.7 +#define vmaxanalog 0.9 #define ax 0.005 //#define koefperlambatan 0.8 // Deklarasi variabel motor -Motor motor1(PB_6, PA_7 , PB_12); // pwm, fwd, rev -Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev -Motor motor3(PB_7, PA_15, PA_14); // pwm, fwd, rev -Motor motor4(PB_8, PB_1 , PA_13); // pwm, fwd, rev +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 // Inisialisasi Pin TX-RX Joystik dan PC joysticknucleo joystick(PA_0,PA_1); @@ -43,20 +44,33 @@ char case_ger; bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false; bool stop = true; -int jLX,jLY; +float jLX,jLY; double vcurr=0; -float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi +float x,y; // untuk analoghat kiri float errorx=0,errory=0; -// Fungsi mapping 0-255 ke -1 sampai 1 -float mapping(int x) +// Fungsi mapping 0-255 ke -128 sampai 127 +float mapping(float a,float error) { - float hasil; - hasil = ((x+1)/128)-1; - + float hasil,b; + b = (float)((a-128)/128); + if (b>(error - 0.2) && b<(error + 0.2)) + { + hasil = 0; + } else { + hasil = b; + } return (hasil); } +// Kalibrasi tombol analog kiri +void kalibrasi() +{ + errorx = (jLX - 128)/128; + errory = (jLY - 128)/128; + +} + // simpan data analog void baca_analog() { @@ -64,12 +78,8 @@ jLY = joystick.LY; // Pembacaan nilai Y terbalik - x = mapping(jLX); - y = -mapping(jLY); - - // Setelah di kalibrasi - xk = x + errorx; - yk = y + errory; + x = mapping(jLX,errorx); + y = -mapping(jLY,errory); } // Gerak dari Motor base @@ -108,8 +118,6 @@ // Kiri casegerak = 10; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { - // analog switch mode - if (joystick.SELECT_click) {analog=!analog;} // case gerak analog on off if (analog){ casegerak = 11; @@ -143,7 +151,7 @@ void aktuator() { double koef; - double s1=0,s2=0,s3=0,s4=0; + double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0; // MOTOR switch (case_ger) @@ -235,7 +243,7 @@ } case (3): { - if (maju) { + if (maju) { if(vcurr<0.1) { vcurr=0.1; } else { @@ -582,21 +590,7 @@ } case (11): { - if (analog) { - 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) { @@ -606,17 +600,22 @@ koef=1; } - s1 = (float)(0.5*koef*vcurr*((-xk)+(yk))); - s2 = (float)(0.5*koef*vcurr*((-xk)+(-yk))); - s3 = (float)(0.5*koef*vcurr*((xk)+(-yk))); - s4 = (float)(0.5*koef*vcurr*((xk)+(yk))); + s1t = (vmaxanalog*(-x+y)); + s2t = (vmaxanalog*(-x-y)); + s3t = (vmaxanalog*(x-y)); + s4t = (vmaxanalog*(x+y)); + + s1 = (float)(0.5*koef*s1t); + s2 = (float)(0.5*koef*s2t); + s3 = (float)(0.5*koef*s3t); + s4 = (float)(0.5*koef*s4t); analog=true; maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("analog x= %d y= %d /n ",x,y); + pc.printf("analog X =%.2f Y =%.2f \n ",x,y); motor1.speed(s1); @@ -661,14 +660,6 @@ } } -// Kalibrasi tombol analog kiri -void kalibrasi() -{ - errorx = 0-x; - errory = 0-y; - -} - int main (void) { @@ -676,6 +667,7 @@ joystick.setup(); pc.baud(115200); pc.printf("Ready...\n"); + kalibrasi(); while(1) { @@ -690,10 +682,11 @@ case_ger = case_gerak(); aktuator(); //kalibrasi - if (joystick.START_click){ - kalibrasi(); - } - + if (joystick.START){ + kalibrasi();} + // analog switch mode + if (joystick.SELECT_click) {analog=!analog;} + //pc.printf(" X =%.2f Y =%.2f \n ",x,y); } else { joystick.idle();