![](/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:
- 2016-10-25
- Revision:
- 4:483c07cc22e1
- Parent:
- 3:1287fccc11be
- Child:
- 5:3aa203218306
File content as of revision 4:483c07cc22e1:
/** Case Gerak 1. cw 2. ccw 3. Maju 4. Mundur 5. Serong Atas Kanan 6. Serong Bawah Kanan 7. Serong Atas Kiri 8. Serong Bawah Kiri 9. Kanan 10. Kiri 11. Analog kiri base 12. stop Urutan motor 1234 searah jarum jam Source Code dari Bima Sahbani EL'12 Fanny Achmad Hindrarta EL'12 **/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #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, 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); Serial pc(USBTX,USBRX); //bool perlambatan=0; 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; float jLX,jLY; double vcurr=0; float x,y; // untuk analoghat kiri float errorx=0,errory=0; // Fungsi mapping 0-255 ke -128 sampai 127 float mapping(float a,float error) { 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() { jLX = joystick.LX; jLY = joystick.LY; // Pembacaan nilai Y terbalik x = mapping(jLX,errorx); y = -mapping(jLY,errory); } // Gerak dari Motor base int case_gerak() { int casegerak; baca_analog(); 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; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri casegerak = 10; } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // case gerak analog on off if (analog){ casegerak = 11; } else { casegerak = 12; } } return(casegerak); } /** ** 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 11 : Analog ** Case 11 : break **/ // Pergerakan dari base void aktuator() { double koef; double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0; // MOTOR switch (case_ger) { case (1): { if (pivka) { if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmaxpivot) { vcurr=vmaxpivot; } 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)(-0.5*koef*vcurr); s2 = (float)(-0.5*koef*vcurr); s3 = (float)(-0.5*koef*vcurr); s4 = (float)(-0.5*koef*vcurr); pivka=true; maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("pivKa\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } case (2): { if (pivki){ if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmaxpivot) { vcurr=vmaxpivot; } 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)(0.5*koef*vcurr); s2 = (float)(0.5*koef*vcurr); s3 = (float)(0.5*koef*vcurr); s4 = (float)(0.5*koef*vcurr); pivki=true; maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("pivKi\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } case (3): { if (maju) { 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; } //Case s1 untuk mode L2 lebih lambat s1 = (float)(-1*koef*(vcurr+0.005)); s2 = (float)(1.0*koef*vcurr); s3 = (float)(1.0*koef*vcurr); s4 = (float)(-1*koef*vcurr); //s1 =-0.8*koef*vcurr; //s2 =koef*vcurr; //s3 =-koef*vcurr; //s4 =koef*vcurr; maju=true; mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("maju\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } case (4): { if (mundur) { 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; } //Motor 4 telat mulai s1 = (float)(1*koef*(vcurr-0.008)); s2 = (float)(-1*koef*(vcurr-0.005)); s3 = (float)(-1*koef*(vcurr-0.005)); s4 = (float)(1*koef*(vcurr+0.005)); mundur=true; maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("mundur\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } case (5) : { if (saka) { 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)(-koef*vcurr); s2 = (float)(0); //koef*0.1*vcurr; s3 = (float)(koef*vcurr); s4 = (float)(0); //-koef*0.1*vcurr; saka=true; maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("saka\n"); motor1.speed(s1); motor2.brake(1); //motor2.speed(s2); motor3.speed(s3); motor4.brake(1); //motor4.speed(s4); break; } case (6) : { if (sbka){ if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmaxserong) { vcurr=vmaxserong; } 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)(0); //koef*0.1*vcurr; s2 = (float)(-koef*vcurr); s3 = (float)(0); //-koef*0.1*vcurr; s4 = (float)(koef*vcurr); sbka=true; maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("sbka\n"); //motor1.speed(s1); motor1.brake(1); motor2.speed(s2); //motor3.speed(s3); motor3.brake(1); motor4.speed(s4); break; } case (7) : { if (saki) { if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmaxserong) { vcurr=vmaxserong; } 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)(0); //-koef*0.1*vcurr; s2 = (float)(koef*vcurr); s3 = (float)(0); //koef*0.1*vcurr; s4 = (float)(-koef*vcurr); saki=true; maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("saki\n"); //motor1.speed(s1); motor1.brake(1); motor2.speed(s2); //motor3.speed(s3); motor3.brake(1); motor4.speed(s4); break; } case (8) : { if (sbki) { if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=0; } else { //perlambatan=1; } if (vcurr>=vmaxserong) { vcurr=vmaxserong; } 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)(koef*vcurr); s2 = (float)(0); //-koef*0.1*vcurr; s3 = (float)(-koef*vcurr); s4 = (float)(0); //koef*0.1*vcurr; sbki=true; maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("sbki\n"); motor1.speed(s1); //motor2.speed(s2); motor2.brake(1); motor3.speed(s3); //motor4.speed(s4); motor4.brake(1); break; } case (9) : { if (kanan) { 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; } case (10) : { if (kiri) { if(vcurr<0.1) { vcurr=0.1; } else { vcurr+=ax; } //perlambatan=1; } 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*koef*vcurr); s3 =(float)(-1*koef*vcurr); s4 =(float)(-1.0*koef*vcurr); kiri=true; maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; pc.printf("Kiri\n"); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } case (11): { if(joystick.R2==255 && joystick.L2==0) { koef=2; } else if (joystick.L2==255 && joystick.R2==0) { koef=0.5; } else { koef=1; } 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 =%.2f Y =%.2f \n ",x,y); motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); break; } default : { //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100); //if (maju && (vcurr>=0.5)) wait_ms(100); //else if (maju && (vcurr<0.5)) wait_ms(50); /* if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) { s1 = koefperlambatan * s1; s2 = koefperlambatan * s2; s3 = koefperlambatan * s3; s4 = koefperlambatan * s4; motor1.speed(s1); motor2.speed(s2); motor3.speed(s3); motor4.speed(s4); } else { */ motor1.brake(1); motor2.brake(1); motor3.brake(1); motor4.brake(1); //} maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; stop = true; //s1 = 0;s2 =0; s3 =0; s4 =0; pc.printf("Stop\n"); } } } int main (void) { // Set baud rate - 115200 joystick.setup(); pc.baud(115200); pc.printf("Ready...\n"); kalibrasi(); while(1) { // Interrupt Serial joystick.idle(); if(joystick.readable() ) { // Panggil fungsi pembacaan joystik joystick.baca_data(); // Panggil fungsi pengolahan data joystik joystick.olah_data(); //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY); case_ger = case_gerak(); aktuator(); //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(); } } }