![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
dear josh
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of DagonFly__RoadToJapan_15Mei by
main.cpp
- Committer:
- fanny868
- Date:
- 2015-05-28
- Revision:
- 0:9072e932503c
- Child:
- 1:56bd3e8f38c5
File content as of revision 0:9072e932503c:
/** Base 4 Nasional Case Gerak 1. Pivot Kanan 2. Pivot Kiri 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. Stop Bima Sahbani EL'12 Fanny Achmad Hindrarta EL'12 **/ #include "mbed.h" #include "JoystickPS3.h" #include "Motor.h" #define vmax 1 #define vmaxserong 0.9 #define vmaxpivot 0.7 #define ax 0.005 //#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; // Inisialisasi Pin TX-RX Joystik dan PC joysticknucleo joystick(PA_11,PA_12); //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 stop = true; bool t1, t2, pneu1, pneu2; int delay_pneumatik; double vcurr; int case_gerak() { int casegerak; 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)) ) { // 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))) { // Mundur casegerak = 4; } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((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))))) { // 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)))) { // 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)))) { // 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)) ) { // 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))) { // 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; } 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 : Break **/ 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): { 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=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=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; } s1 = (float)(-1*koef*vcurr); 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=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; } s1 = (float)(1*koef*vcurr); s2 = (float)(-1*koef*vcurr); s3 = (float)(-1*koef*vcurr); s4 = (float)(1*koef*vcurr); mundur=true; maju=kiri=kanan=saka=saki=sbka=sbki=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=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=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=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=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=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=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; } 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=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; stop = true; //s1 = 0;s2 =0; s3 =0; s4 =0; //pc.printf("Stop\n"); } } } 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; 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(); } else { joystick.idle(); } } }