Versi omniwheel tanpa encoder
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_OrdoV6_2 by
Diff: main.cpp
- Revision:
- 16:89093194ccc2
- Parent:
- 15:98f0d56b14f0
- Child:
- 17:703072f5dce1
--- a/main.cpp Fri Dec 09 12:03:44 2016 +0000 +++ b/main.cpp Thu Jan 12 11:15:01 2017 +0000 @@ -1,21 +1,3 @@ -/****************************************************************************/ -/* PROGRAM UNTUK PID CLOSED LOOP */ -/* */ -/* - Digunakan encoder autonics */ -/* - Konfigurasi Motor dan Encoder sbb : */ -/* _________________ */ -/* | DEPAN | */ -/* | 1. e .2 | Angka ==> Motor */ -/* | ` ` | e ==> Encoder */ -/* | e e | */ -/* | . . | */ -/* | 4` e `3 | */ -/* |________________| */ -/* */ -/* SETTINGS (WAJIB!) : */ -/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ -/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ -/* */ /****************************************************************************/ /* */ /* Joystick */ @@ -41,19 +23,21 @@ #include "Servo.h" //#define koefperlambatan 0.8 -#include "encoderKRAI.h" - +//#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 -//PC 9 #define pinservo2 PC_9 -float K_enc = pi*diaEncoder; -float K_robot = pi*diaRobot; - float speed1=0.6; float speed2=0.6; float speed3=0.6; @@ -61,26 +45,7 @@ float speedB=0.43 ; float speedL=0.4; -float KpX=0.5, KpY=0.5, Kp_tetha=0.03; -Timer waktu; -//float jarakGlobalY; - -// Deklarasi variabel PID -//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling) -/* -// Deklarasi variabel encoder -encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan -encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); -encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING); -encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING); -*/ - -// Deklarasi variabel 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); -encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); -encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); // Deklarasi variabel motor Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev @@ -96,34 +61,12 @@ Servo servoS(PB_2); Servo servoB(PA_5); -// Deklarasi variabel posisi robot -//robotPos posisi(); - -// Inisialisasi Pin TX-RX Joystik dan PC -//Serial pc(PA_0,PA_1); -//Serial pc(USBTX,USBRX); -// Deklarasi Variabel Global -/* - * posX dan posY berdasarkan arah robot - * encoder Depan & Belakang sejajar sumbu Y - * encoder Kanan & Kiri sejajar sumbu X - */ float jarak, posX, posY; -//float keliling = pi*diameterRoda; - -void detect_encoder(float speed); -void setCenter(); -float getY(); -float getX(); -float getTetha(); // Inisialisasi Pin TX-RX Joystik dan PC joysticknucleo joystick(PA_0,PA_1); Serial pc(USBTX,USBRX); -// Posisi target -float XT, YT, Tetha; - //encoder variable float errX, errY, errT, Vt, Vx, Vy; float V1, V2, V3, V4; @@ -271,249 +214,497 @@ motorlb.speed(0); } - // MOTOR Bawah + 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): { - Tetha = Tetha - 0.05; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); + pc.printf("pivKa\n"); - + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); + break; } case (2): { - Tetha = Tetha + 0.05; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + pc.printf("pivKi\n"); - + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); + break; } case (3): { - YT = YT + 0.01; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); + pc.printf("maju\n"); + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); + break; } case (4): { - YT = YT - 0.01; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); + pc.printf("mundur\n"); + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); break; } case (5) : { - XT = XT + 0.01; - YT = YT + 0.01; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + 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) : { - XT = XT + 0.01; - YT = YT - 0.01; - + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + 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) : { - XT = XT - 0.01; - YT = YT + 0.01; - + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + 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) : { - XT = XT - 0.01; - YT = YT - 0.01; - - //pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + 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) : { - XT = XT + 0.01; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + pc.printf("Kanan\n"); + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); break; } case (10) : { - XT = XT - 0.01; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + pc.printf("Kiri\n"); + motor1.speed(s1); + motor2.speed(s2); + motor3.speed(s3); + motor4.speed(s4); break; } case (11): { - XT = XT + 0.01*x; - YT = YT + 0.01*y; + 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); + 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; - //s1 = 0;s2 =0; s3 =0; s4 =0; - // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4); - } + pc.printf("Stop\n"); + } } } -//Begin Encoder -void setCenter() -{ - encoderDepan.reset(); - encoderBelakang.reset(); - encoderKanan.reset(); - encoderKiri.reset(); -} - -float getX() -{ - 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() -{ - 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() -{ - 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; - - return -(busurDpn+busurBlk+busurKir+busurKan)/4; -} - -void gotoXYT(float xa, float ya, float Ta) -{ - - errX = xa-getX(); - Vx = KpX*errX; - - errY = ya-getY(); - Vy = KpY*errY; - - 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 (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); - // pc.printf("V1=%.2f \ ", V1); - } - else - { - motor1.brake(1); - motor2.brake(1); - motor3.brake(1); - motor4.brake(1); - //_ms(1000); - } - //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT); - // wait_ms(10); - -} -//End Encoder void speedLauncher() { @@ -535,17 +726,10 @@ // Set baud rate - 115200 joystick.setup(); pc.baud(115200); - wait_ms(1000); - setCenter(); wait_ms(500); - //Posisi Awal - XT = 0; - YT = 0; - Tetha = 0; pc.printf("Ready...\n"); kalibrasi(); - waktu.start(); while(1) { // Interrupt Serial @@ -570,12 +754,9 @@ if (joystick.segitiga_click) {Launcher = !Launcher;} if (joystick.lingkaran_click){ ServoGo = true; - waktu.reset(); } if (joystick.silang) { - XT = 0; - YT = 0; - Tetha = 0; + //pc.printf("x..\n"); } speedLauncher(); @@ -584,7 +765,7 @@ joystick.idle(); } - gotoXYT(XT,YT,Tetha); +// gotoXYT(XT,YT,Tetha); } }