KRAI 2017
aman semoga
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of
Joystick_ManualV2
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 18:23412e950394
- Parent:
- 17:703072f5dce1
- Child:
- 19:bb304933a9f8
--- 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);