Versi omniwheel tanpa encoder
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_OrdoV6_2 by
main.cpp
- Committer:
- rahmadirizki18
- Date:
- 2017-01-12
- Revision:
- 16:89093194ccc2
- Parent:
- 15:98f0d56b14f0
- Child:
- 17:703072f5dce1
File content as of revision 16:89093194ccc2:
/****************************************************************************/ /* */ /* 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 */ /* */ /* */ /* */ /****************************************************************************/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #include "Servo.h" //#define koefperlambatan 0.8 //#include "encoderKRAI.h" /* #define pi 22/7 #define diaEncoder 0.058 #define PPR 1000 #define diaRobot 0.64 */ #define vmax 1 #define vmaxserong 0.9 #define vmaxpivot 0.7 #define vmaxanalog 0.9 #define ax 0.005 #define pinservo1 PC_8 #define pinservo2 PC_9 float speed1=0.6; float speed2=0.6; float speed3=0.6; float speed4=0.6; float speedB=0.43 ; float speedL=0.4; // Deklarasi variabel motor 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 Atas Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev //Servo Atas Servo servoS(PB_2); Servo servoB(PA_5); float jarak, posX, posY; // Inisialisasi Pin TX-RX Joystik dan PC joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); //encoder variable float errX, errY, errT, Vt, Vx, Vy; float V1, V2, V3, V4; //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; bool Launcher = false,ServoGo = false; float jLX,jLY; double vcurr; 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() { //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); } double koef; double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0; // MOTOR BAWAH 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 : { 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; pc.printf("Stop\n"); } } } void speedLauncher() { 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;} //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB); } int main (void) { // Set baud rate - 115200 joystick.setup(); pc.baud(115200); wait_ms(500); 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("%3d %3d\n\r",joystick.R2, joystick.L2); //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(); //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read()); //kalibrasi if (joystick.START){ kalibrasi();} // analog switch mode if (joystick.SELECT_click) {analog = !analog;} if (joystick.segitiga_click) {Launcher = !Launcher;} if (joystick.lingkaran_click){ ServoGo = true; } if (joystick.silang) { //pc.printf("x..\n"); } speedLauncher(); } else { joystick.idle(); } // gotoXYT(XT,YT,Tetha); } }