aman semoga
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_ManualV2 by
Diff: main.cpp
- Revision:
- 18:23412e950394
- Parent:
- 17:703072f5dce1
- Child:
- 19:bb304933a9f8
diff -r 703072f5dce1 -r 23412e950394 main.cpp --- a/main.cpp Sat Jan 14 08:01:09 2017 +0000 +++ b/main.cpp Wed Jan 18 10:28:01 2017 +0000 @@ -214,7 +214,6 @@ motorlb.speed(0); } - double koef; double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0; // MOTOR BAWAH @@ -237,19 +236,11 @@ 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); + s1 = (float)(-0.5* vcurr); + s2 = (float)(-0.5* vcurr); + s3 = (float)(-0.5* vcurr); + s4 = (float)(-0.5* vcurr); pivka=true; maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -280,18 +271,11 @@ 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); + + s1 = (float)(0.5* vcurr); + s2 = (float)(0.5* vcurr); + s3 = (float)(0.5* vcurr); + s4 = (float)(0.5* vcurr); pivki=true; maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -322,23 +306,17 @@ 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)); - s2 = (float)(1.0*koef*vcurr); - s3 = (float)(1.0*koef*vcurr); - s4 = (float)(-1*koef*vcurr); + s1 = (float)(-1* (vcurr)); + s2 = (float)(1.0* vcurr); + s3 = (float)(1.0* vcurr); + s4 = (float)(-1* vcurr); - //s1 =-0.8*koef*vcurr; - //s2 =koef*vcurr; - //s3 =-koef*vcurr; - //s4 =koef*vcurr; + //s1 =-0.8* vcurr; + //s2 = vcurr; + //s3 =- vcurr; + //s4 = vcurr; maju=true; mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -369,18 +347,11 @@ 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)); + s1 = (float)(1* (vcurr)); + s2 = (float)(-1* (vcurr)); + s3 = (float)(-1* (vcurr)); + s4 = (float)(1* (vcurr)); mundur=true; maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -409,18 +380,13 @@ 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; + s1 = (float)(- vcurr); + s2 = (float)(0); // 0.1*vcurr; + s3 = (float)( vcurr); + s4 = (float)(0); //- 0.1*vcurr; saka=true; maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -453,18 +419,11 @@ 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); + s1 = (float)(0); // 0.1*vcurr; + s2 = (float)(- vcurr); + s3 = (float)(0); //- 0.1*vcurr; + s4 = (float)( vcurr); sbka=true; maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -497,18 +456,11 @@ 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); + s1 = (float)(0); //- 0.1*vcurr; + s2 = (float)( vcurr); + s3 = (float)(0); // 0.1*vcurr; + s4 = (float)(- vcurr); saki=true; maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -541,18 +493,11 @@ 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; + s1 = (float)( vcurr); + s2 = (float)(0); //- 0.1*vcurr; + s3 = (float)(- vcurr); + s4 = (float)(0); // 0.1*vcurr; sbki=true; maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -585,18 +530,11 @@ 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+0.005)); + s1 =(float)(-1* vcurr); + s2 =(float)(-1.0* vcurr); + s3 =(float)(1* vcurr); + s4 =(float)(1.0* (vcurr+0.005)); kanan=true; maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -626,18 +564,11 @@ 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+0.003)); - s3 =(float)(-1*koef*vcurr); - s4 =(float)(-1.0*koef*vcurr); + s1 =(float)(1* vcurr); + s2 =(float)(1* (vcurr+0.003)); + s3 =(float)(-1* vcurr); + s4 =(float)(-1.0* vcurr); kiri=true; maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; @@ -654,24 +585,16 @@ 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); + s1 = (float)(0.5* s1t); + s2 = (float)(0.5* s2t); + s3 = (float)(0.5* s3t); + s4 = (float)(0.5* s4t);