Versi omniwheel tanpa encoder
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of Joystick_OrdoV6_2 by
Revision 17:703072f5dce1, committed 2017-01-14
- Comitter:
- rahmadirizki18
- Date:
- Sat Jan 14 08:01:09 2017 +0000
- Parent:
- 16:89093194ccc2
- Commit message:
- yoi;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Jan 12 11:15:01 2017 +0000 +++ b/main.cpp Sat Jan 14 08:01:09 2017 +0000 @@ -30,10 +30,10 @@ #define PPR 1000 #define diaRobot 0.64 */ -#define vmax 1 -#define vmaxserong 0.9 -#define vmaxpivot 0.7 -#define vmaxanalog 0.9 +#define vmax 0.3 //0.4 +#define vmaxserong 0.3 //0.3 +#define vmaxpivot 0.3 //0.3 +#define vmaxanalog 0.3 //0.3 #define ax 0.005 #define pinservo1 PC_8 #define pinservo2 PC_9 @@ -48,10 +48,10 @@ // Deklarasi variabel motor -Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev -Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev -Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev -Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev +Motor motor1(PB_7, PA_15 , PA_14); // pwm, fwd, rev +Motor motor2(PB_8, PB_0 ,PA_13); // pwm, fwd, rev +Motor motor3(PB_9, PC_5 ,PA_12); // pwm, fwd, rev +Motor motor4(PB_6, PB_12 ,PB_1); // pwm, fwd, rev //Motor Atas Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev @@ -254,7 +254,7 @@ 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\n"); motor1.speed(s1); motor2.speed(s2); @@ -296,7 +296,7 @@ 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\n"); motor1.speed(s1); motor2.speed(s2); @@ -330,8 +330,7 @@ } //Case s1 untuk mode L2 lebih lambat - s1 = (float)(-1*koef*(vcurr+0.005)); - + s1 = (float)(-1*koef*(vcurr)); s2 = (float)(1.0*koef*vcurr); s3 = (float)(1.0*koef*vcurr); s4 = (float)(-1*koef*vcurr); @@ -344,7 +343,7 @@ 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\n"); motor1.speed(s1); motor2.speed(s2); @@ -377,16 +376,16 @@ } 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)); + + 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=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false; - pc.printf("mundur\n"); + //pc.printf("mundur\n"); motor1.speed(s1); motor2.speed(s2); @@ -426,7 +425,7 @@ 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\n"); motor1.speed(s1); motor2.brake(1); @@ -470,7 +469,7 @@ 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\n"); //motor1.speed(s1); motor1.brake(1); @@ -514,7 +513,7 @@ 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\n"); //motor1.speed(s1); motor1.brake(1); @@ -558,7 +557,7 @@ sbki=true; 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); @@ -597,12 +596,12 @@ s1 =(float)(-1*koef*vcurr); s2 =(float)(-1.0*koef*vcurr); s3 =(float)(1*koef*vcurr); - s4 =(float)(1.0*koef*vcurr); + s4 =(float)(1.0*koef*(vcurr+0.005)); 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\n"); motor1.speed(s1); motor2.speed(s2); @@ -636,14 +635,14 @@ } s1 =(float)(1*koef*vcurr); - s2 =(float)(1*koef*vcurr); + s2 =(float)(1*koef*(vcurr+0.003)); s3 =(float)(-1*koef*vcurr); s4 =(float)(-1.0*koef*vcurr); 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\n"); motor1.speed(s1); motor2.speed(s2); @@ -679,7 +678,7 @@ 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 X =%.2f Y =%.2f \n ",x,y); motor1.speed(s1); @@ -699,7 +698,7 @@ stop = true; - pc.printf("Stop\n"); + //pc.printf("Stop\n"); } } } @@ -755,10 +754,6 @@ if (joystick.lingkaran_click){ ServoGo = true; } - if (joystick.silang) { - - //pc.printf("x..\n"); - } speedLauncher(); } else {