KRAI 2017
ping + display on
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_20Feb
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 15:98f0d56b14f0
- Parent:
- 14:6d389e99981c
- Child:
- 16:90119f03c5d1
--- a/main.cpp Fri Dec 09 10:50:40 2016 +0000
+++ b/main.cpp Fri Dec 09 12:03:44 2016 +0000
@@ -68,17 +68,24 @@
// Deklarasi variabel PID
//PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
-
+/*
// Deklarasi variabel encoder
encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
+*/
+
+// Deklarasi variabel encoder
+encoderKRAI encoderDepan( PB_13, PB_14, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderBelakang( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
+encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING);
+encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
// 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_3, PA_12 , PC_5); // 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 Atas
@@ -269,119 +276,119 @@
{
case (1):
{
- Tetha = Tetha - 0.5;
+ Tetha = Tetha - 0.05;
pivka=true;
maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-// pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
+ //pc.printf("pivKa Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
break;
}
case (2):
{
- Tetha = Tetha + 0.5;
+ Tetha = Tetha + 0.05;
pivki=true;
maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("pivKi Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (3):
{
- YT = YT + 0.1;
+ YT = YT + 0.01;
maju=true;
mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
-// pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
+ //pc.printf("maju Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
break;
}
case (4):
{
- YT = YT - 0.1;
+ YT = YT - 0.01;
mundur=true;
maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
+ //pc.printf("mundur Xt =%.2f x=%.2f YT=%.2f y=%.2f errx=%.2f erry=%.2f \n",XT,x,YT,y,errX,errY);
break;
}
case (5) :
{
- XT = XT + 0.1;
- YT = YT + 0.1;
+ XT = XT + 0.01;
+ YT = YT + 0.01;
saka=true;
maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("saka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (6) :
{
- XT = XT + 0.1;
- YT = YT - 0.1;
+ XT = XT + 0.01;
+ YT = YT - 0.01;
sbka=true;
maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("sbka Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (7) :
{
- XT = XT - 0.1;
- YT = YT + 0.1;
+ XT = XT - 0.01;
+ YT = YT + 0.01;
saki=true;
maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("saki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (8) :
{
- XT = XT - 0.1;
- YT = YT - 0.1;
+ XT = XT - 0.01;
+ YT = YT - 0.01;
- // pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (9) :
{
- XT = XT + 0.1;
+ XT = XT + 0.01;
kanan=true;
maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("Kanan Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
case (10) :
{
- XT = XT - 0.1;
+ XT = XT - 0.01;
kiri=true;
maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("Kiri Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
@@ -389,13 +396,13 @@
case (11):
{
- XT = XT + 0.1*x;
- YT = YT + 0.1*y;
+ XT = XT + 0.01*x;
+ YT = YT + 0.01*y;
analog=true;
maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
- // pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ //pc.printf("analog Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
break;
}
@@ -518,7 +525,7 @@
speedB = speedB + 0.01;}
if (joystick.L2_click and speedB > 0.1 ){
speedB = speedB - 0.01;}
- pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
+ //pc.printf("Pwm depan = %.3f\t Pwm belakang = %.3f\n", speedL, speedB);
}
@@ -569,7 +576,7 @@
XT = 0;
YT = 0;
Tetha = 0;
- pc.printf("x..\n");
+ //pc.printf("x..\n");
}
speedLauncher();