![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
aman semoga
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_ManualV2 by
main.cpp
- Committer:
- franshendri
- Date:
- 2016-11-29
- Revision:
- 10:f0f0dc3904e0
- Parent:
- 9:5a50782510fb
- Child:
- 12:e07c59c28c29
File content as of revision 10:f0f0dc3904e0:
/****************************************************************************/ /* 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 */ /* 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 kotak => Aktif servo Launcher */ /* */ /****************************************************************************/ #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 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; float speed4=0.6; float speedL=0.6; 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 motor Motor motor1(PA_8, PB_0 , PC_15); // pwm, fwd, rev Motor motor2(PA_11, PA_6 ,PA_5); // pwm, fwd, rev Motor motor3(PA_9, PC_2 , PC_3); // pwm, fwd, rev Motor motor4(PA_10, PB_5 ,PB_4); // pwm, fwd, rev //Motor Atas Motor motorld(PB_8, PB_1 , PA_13); // pwm, fwd, rev Motor motorlb(PB_9, PA_12, PC_5 ); // pwm, fwd, rev //Servo Atas Servo servoS(PC_9); Servo servoB(PC_7); // 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; 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(10); wait_ms(500); servoS.position(-70); wait_ms(500); servoS.position(10); wait_ms(500); for (int i = 0; i<=90; i++){ servoB.position(i); wait_ms(10); } wait_ms(500); servoB.position(0); ServoGo = false; }else{ servoS.position(10); servoB.position(0); } // Motor Atas if (Launcher) { motorld.speed(speedL); motorlb.speed(speedL); }else{ motorld.speed(0); motorlb.speed(0); } // MOTOR Bawah switch (case_ger) { case (1): { 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY); break; } case (2): { 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 Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); break; } case (3): { YT = YT + 0.1; 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); break; } case (4): { YT = YT - 0.1; 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); break; } case (5) : { XT = XT + 0.1; YT = YT + 0.1; 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); break; } case (6) : { XT = XT + 0.1; YT = YT - 0.1; 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); break; } case (7) : { XT = XT - 0.1; YT = YT + 0.1; 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); break; } case (8) : { XT = XT - 0.1; YT = YT - 0.1; pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y); break; } case (9) : { XT = XT + 0.1; 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); break; } case (10) : { XT = XT - 0.1; 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); break; } case (11): { XT = XT + 0.1*x; YT = YT + 0.1*y; 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); break; } default : { 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); } } } //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() { if (joystick.R3_click and speedL < 0.7){ speedL = speedL + 0.1;} if (joystick.L3_click and speedL > 0.1){ speedL = speedL - 0.1;} } 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(); servoS.position(90); servoB.position(0); waktu.start(); 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(); 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; waktu.reset(); } if (joystick.silang) { XT = 0; YT = 0; Tetha = 0; pc.printf("x..\n"); } speedLauncher(); } else { joystick.idle(); } gotoXYT(XT,YT,Tetha); } }