![](/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
Diff: main.cpp
- Revision:
- 3:1287fccc11be
- Parent:
- 1:56bd3e8f38c5
- Child:
- 4:483c07cc22e1
--- a/main.cpp Sat Jun 27 07:37:28 2015 +0000 +++ b/main.cpp Sun Oct 16 06:27:39 2016 +0000 @@ -1,9 +1,7 @@ /** -Base 4 Nasional - Case Gerak -1. Pivot Kanan -2. Pivot Kiri +1. cw +2. ccw 3. Maju 4. Mundur 5. Serong Atas Kanan @@ -12,9 +10,12 @@ 8. Serong Bawah Kiri 9. Kanan 10. Kiri -11. Stop +11. Analog kiri base +12. stop +Urutan motor 1234 searah jarum jam +Source Code dari Bima Sahbani EL'12 Fanny Achmad Hindrarta EL'12 **/ @@ -29,80 +30,93 @@ //#define koefperlambatan 0.8 // Deklarasi variabel motor -Motor motor1(PA_15, PA_13, PA_14); // pwm, fwd, rev -Motor motor2(PA_0, PC_14, PC_15); // pwm, fwd, rev -Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev -Motor motor4(PC_6, PC_9, PC_8); // pwm, fwd, rev - -// Deklarasi Register Pneumatik -DigitalOut pneumatik1(PC_11); -DigitalOut pneumatik2(PD_2); - -// Deklarasi Timer Pneumatik -Timer timer_pneu; +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_11,PA_12); -//Serial pc(USBTX,USBRX); +joysticknucleo joystick(PA_0,PA_1); +Serial pc(USBTX,USBRX); //bool perlambatan=0; char case_ger; -bool maju=false,mundur=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,pivki=false,pivka=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false; +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 t1, t2, pneu1, pneu2; -int delay_pneumatik; -double vcurr; +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; - if (!joystick.L1 && joystick.R1) { + baca_analog(); + if (!joystick.L1 && joystick.R1) { // Pivot Kanan casegerak = 1; } else if (!joystick.R1 && joystick.L1) { // Pivot Kiri casegerak = 2; - } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>110 && joystick.LX<190) && (joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) ) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Maju - casegerak = 3; - } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>90 && joystick.LX<190) && (joystick.LY>=200) )|| ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))) { + casegerak = 3; + } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { // Mundur casegerak = 4; - } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)))) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { // Serong Atas Kanan casegerak = 5; - } else if(((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))) { + } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) { // Serong Bawah Kanan casegerak = 6; - } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { // Serong Atas Kiri casegerak = 7; - } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) { // Serong Bawah Kiri casegerak = 8; - } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>=210) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) ) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { // Kanan casegerak = 9; - } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX<=50) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))) { + } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { // Kiri - casegerak = 10; - } else { - // Stop - casegerak = 11; - } - - if(joystick.silang_click && t1==0 && t2==0) { - pneu1 = 1; - } else { - pneu1 = 0; - } - - if(joystick.kotak_click && t1==0 && t2==0) { - pneu2 = 1; - } else { - pneu2 = 0; - } + 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); } @@ -110,7 +124,7 @@ /** -** Case 1 : Pivot Kanan +** Case 1 : Pivot kanan ** Case 2 : Pivot Kiri ** Case 3 : Maju ** Case 4 : Mundur @@ -120,47 +134,21 @@ ** Case 8 : Serong Bawah Kiri ** Case 9 : Kanan ** Case 10 : Kiri -** Case 11 : Break +** Case 11 : Analog +** Case 11 : break **/ + +// Pergerakan dari base void aktuator() { double koef; double s1=0,s2=0,s3=0,s4=0; - // PNEUMATIK - if(t1==1) { - if(timer_pneu.read_ms() - delay_pneumatik > 800) { - pneumatik1 = 1; - t1=0; - } - } - if(t2==1) { - if(timer_pneu.read_ms() - delay_pneumatik > 800) { - pneumatik2 = 1; - t2=0; - } - } - - if (pneu1 == 1 || pneu2==1) { - timer_pneu.reset(); - delay_pneumatik = timer_pneu.read_ms(); - if(pneu1 == 1) { - pneumatik1 = 0; - t1 = 1; - pneu1 = 0; - } else if(pneu2 == 1) { - pneumatik2 = 0; - t2 = 1; - pneu2 = 0; - } - - } - // MOTOR switch (case_ger) { - case (1): +case (1): { if (pivka) { if(vcurr<0.1) { @@ -187,14 +175,14 @@ } s1 = (float)(-0.5*koef*vcurr); - s2 = (float)(0.5*koef*vcurr); + s2 = (float)(-0.5*koef*vcurr); s3 = (float)(-0.5*koef*vcurr); - s4 = (float)(0.5*koef*vcurr); + s4 = (float)(-0.5*koef*vcurr); pivka=true; - maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("pivKa\n"); + pc.printf("pivKa\n"); motor1.speed(s1); motor2.speed(s2); @@ -229,14 +217,14 @@ } s1 = (float)(0.5*koef*vcurr); - s2 = (float)(-0.5*koef*vcurr); + s2 = (float)(0.5*koef*vcurr); s3 = (float)(0.5*koef*vcurr); - s4 = (float)(-0.5*koef*vcurr); + s4 = (float)(0.5*koef*vcurr); pivki=true; - maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("pivKi\n"); + pc.printf("pivKi\n"); motor1.speed(s1); motor2.speed(s2); @@ -282,9 +270,9 @@ //s4 =koef*vcurr; maju=true; - mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("maju\n"); + pc.printf("maju\n"); motor1.speed(s1); motor2.speed(s2); @@ -324,9 +312,9 @@ s4 = (float)(1*koef*(vcurr+0.005)); mundur=true; - maju=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("mundur\n"); + pc.printf("mundur\n"); motor1.speed(s1); motor2.speed(s2); @@ -364,9 +352,9 @@ s4 = (float)(0); //-koef*0.1*vcurr; saka=true; - maju=mundur=kiri=kanan=sbka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("saka\n"); + pc.printf("saka\n"); motor1.speed(s1); motor2.brake(1); @@ -408,9 +396,9 @@ s4 = (float)(koef*vcurr); sbka=true; - maju=mundur=kiri=kanan=saka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("sbka\n"); + pc.printf("sbka\n"); //motor1.speed(s1); motor1.brake(1); @@ -452,9 +440,9 @@ s4 = (float)(-koef*vcurr); saki=true; - maju=kiri=kanan=saka=mundur=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("saki\n"); + pc.printf("saki\n"); //motor1.speed(s1); motor1.brake(1); @@ -496,9 +484,9 @@ s4 = (float)(0); //koef*0.1*vcurr; sbki=true; - maju=kiri=kanan=saka=saki=sbka=mundur=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("sbki\n"); + pc.printf("sbki\n"); motor1.speed(s1); //motor2.speed(s2); @@ -540,9 +528,9 @@ s4 =(float)(1.0*koef*vcurr); kanan=true; - maju=kiri=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("Kanan\n"); + pc.printf("Kanan\n"); motor1.speed(s1); motor2.speed(s2); @@ -581,9 +569,9 @@ s4 =(float)(-1.0*koef*vcurr); kiri=true; - maju=kanan=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - //pc.printf("Kiri\n"); + pc.printf("Kiri\n"); motor1.speed(s1); motor2.speed(s2); @@ -592,7 +580,52 @@ break; } - default : + 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); @@ -618,28 +651,32 @@ motor4.brake(1); //} - maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; + 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\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"); - timer_pneu.start(); - pneumatik1=1; - pneumatik2=1; - t1=0; - t2=0; + pc.baud(115200); + pc.printf("Ready...\n"); + while(1) { // Interrupt Serial @@ -651,7 +688,11 @@ 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(); + aktuator(); + //kalibrasi + if (joystick.START_click){ + kalibrasi(); + } } else { joystick.idle();