![](/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:
- 5:3aa203218306
- Parent:
- 4:483c07cc22e1
- Child:
- 6:68293bed71ea
--- a/main.cpp Tue Oct 25 13:21:57 2016 +0000 +++ b/main.cpp Tue Nov 22 15:30:31 2016 +0000 @@ -1,24 +1,23 @@ -/** -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 +/****************************************************************************/ +/* 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 */ +/* */ +/****************************************************************************/ -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" @@ -29,23 +28,80 @@ #define vmaxanalog 0.9 #define ax 0.005 //#define koefperlambatan 0.8 +#include "encoderKRAI.h" + +#define pi 22/7 +#define diaEncoder 0.058 +#define PPR 1000 +#define diaRobot 0.64 + +float K_enc = pi*diaEncoder; +float K_robot = pi*diaRobot; + +float speed1=0.6; +float speed2=0.6; +float speed3=0.6; +float speed4=0.6; + +float KpX=0.5, KpY=0.5, Kp_tetha=0.03; +//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( D3, D4, 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 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 +Motor motor1(PB_8, PB_1 , PA_13); // pwm, fwd, rev +Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev +Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev +Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev + +// 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; + //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; +double vcurr; float x,y; // untuk analoghat kiri float errorx=0,errory=0; @@ -147,525 +203,269 @@ **/ + // 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); - + Tetha = Tetha - 0.5; pivka=true; maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("pivKa\n"); + pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); - 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); + Tetha = Tetha + 0.5; pivki=true; maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("pivKi\n"); + pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - 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; + YT = YT + 0.01; maju=true; mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("maju\n"); + pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); - 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)); + YT = YT - 0.01; mundur=true; maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("mundur\n"); + pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); - 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; + XT = XT + 0.01; + YT = YT + 0.01; saka=true; maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("saka\n"); + pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - 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); + XT = XT + 0.01; + YT = YT - 0.01; + sbka=true; maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("sbka\n"); + pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - //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); + XT = XT - 0.01; + YT = YT + 0.01; + saki=true; maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("saki\n"); + pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - //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; - } + 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(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); - + XT = XT + 0.01; kanan=true; maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("Kanan\n"); + pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - 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); + XT = XT - 0.01; kiri=true; maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("Kiri\n"); + pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); - 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); - - + XT = XT + 0.01*x; + YT = YT + 0.01*y; 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); + pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,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"); + // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4); } - } + } +} + +//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 + int main (void) { // 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(); @@ -687,9 +487,19 @@ // analog switch mode if (joystick.SELECT_click) {analog=!analog;} //pc.printf(" X =%.2f Y =%.2f \n ",x,y); + if (joystick.silang) { + XT = 0; + YT = 0; + Tetha = 0; + pc.printf("x..\n"); + } + + } else { joystick.idle(); } + gotoXYT(XT,YT,Tetha); + } }