![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
ping + display on
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of MainProgram_BaseBaru_20Feb by
main.cpp
- Committer:
- rahmadirizki18
- Date:
- 2016-10-16
- Revision:
- 3:1287fccc11be
- Parent:
- 1:56bd3e8f38c5
- Child:
- 4:483c07cc22e1
File content as of revision 3:1287fccc11be:
/** 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 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 // 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; int jLX,jLY; double vcurr=0; float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi float errorx=0,errory=0; // Fungsi mapping 0-255 ke -1 sampai 1 float mapping(int x) { float hasil; hasil = ((x+1)/128)-1; return (hasil); } // simpan data analog void baca_analog() { jLX = joystick.LX; jLY = joystick.LY; // Pembacaan nilai Y terbalik x = mapping(jLX); y = -mapping(jLY); // Setelah di kalibrasi xk = x + errorx; yk = y + 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)) { // analog switch mode if (joystick.SELECT_click) {analog=!analog;} // 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; // 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 (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) { koef=0.5; } else { 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))); 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); 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"); } } } // Kalibrasi tombol analog kiri void kalibrasi() { errorx = 0-x; errory = 0-y; } int main (void) { // Set baud rate - 115200 joystick.setup(); pc.baud(115200); pc.printf("Ready...\n"); 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_click){ kalibrasi(); } } else { joystick.idle(); } } }