KRAI 2017
aman semoga
Dependencies: Motor PID Joystick_OrdoV5 mbed
Fork of
Joystick_ManualV2
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 16:89093194ccc2
- Parent:
- 15:98f0d56b14f0
- Child:
- 17:703072f5dce1
--- a/main.cpp Fri Dec 09 12:03:44 2016 +0000
+++ b/main.cpp Thu Jan 12 11:15:01 2017 +0000
@@ -1,21 +1,3 @@
-/****************************************************************************/
-/* PROGRAM UNTUK PID CLOSED LOOP */
-/* */
-/* - Digunakan encoder autonics */
-/* - Konfigurasi Motor dan Encoder sbb : */
-/* _________________ */
-/* | DEPAN | */
-/* | 1. e .2 | Angka ==> Motor */
-/* | ` ` | e ==> Encoder */
-/* | e e | */
-/* | . . | */
-/* | 4` e `3 | */
-/* |________________| */
-/* */
-/* SETTINGS (WAJIB!) : */
-/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
-/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
-/* */
/****************************************************************************/
/* */
/* Joystick */
@@ -41,19 +23,21 @@
#include "Servo.h"
//#define koefperlambatan 0.8
-#include "encoderKRAI.h"
-
+//#include "encoderKRAI.h"
+/*
#define pi 22/7
#define diaEncoder 0.058
#define PPR 1000
#define diaRobot 0.64
+*/
+#define vmax 1
+#define vmaxserong 0.9
+#define vmaxpivot 0.7
+#define vmaxanalog 0.9
+#define ax 0.005
#define pinservo1 PC_8
-//PC 9
#define pinservo2 PC_9
-float K_enc = pi*diaEncoder;
-float K_robot = pi*diaRobot;
-
float speed1=0.6;
float speed2=0.6;
float speed3=0.6;
@@ -61,26 +45,7 @@
float speedB=0.43 ;
float speedL=0.4;
-float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
-Timer waktu;
-//float jarakGlobalY;
-
-// 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
@@ -96,34 +61,12 @@
Servo servoS(PB_2);
Servo servoB(PA_5);
-// Deklarasi variabel posisi robot
-//robotPos posisi();
-
-// Inisialisasi Pin TX-RX Joystik dan PC
-//Serial pc(PA_0,PA_1);
-//Serial pc(USBTX,USBRX);
-// Deklarasi Variabel Global
-/*
- * posX dan posY berdasarkan arah robot
- * encoder Depan & Belakang sejajar sumbu Y
- * encoder Kanan & Kiri sejajar sumbu X
- */
float jarak, posX, posY;
-//float keliling = pi*diameterRoda;
-
-void detect_encoder(float speed);
-void setCenter();
-float getY();
-float getX();
-float getTetha();
// Inisialisasi Pin TX-RX Joystik dan PC
joysticknucleo joystick(PA_0,PA_1);
Serial pc(USBTX,USBRX);
-// Posisi target
-float XT, YT, Tetha;
-
//encoder variable
float errX, errY, errT, Vt, Vx, Vy;
float V1, V2, V3, V4;
@@ -271,249 +214,497 @@
motorlb.speed(0);
}
- // MOTOR Bawah
+ double koef;
+ double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
+
+ // MOTOR BAWAH
switch (case_ger)
{
case (1):
{
- Tetha = Tetha - 0.05;
+ 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=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\n");
-
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
break;
}
case (2):
{
- Tetha = Tetha + 0.05;
+ 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=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\n");
-
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
break;
}
case (3):
{
- YT = YT + 0.01;
+ 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;
+ }
+
+ //Case s1 untuk mode L2 lebih lambat
+ s1 = (float)(-1*koef*(vcurr+0.005));
+
+ 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=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\n");
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
break;
}
case (4):
{
- YT = YT - 0.01;
+ 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;
+ }
+ //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));
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\n");
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
break;
}
case (5) :
{
- XT = XT + 0.01;
- YT = YT + 0.01;
+ 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=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\n");
+ motor1.speed(s1);
+ motor2.brake(1);
+ //motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.brake(1);
+ //motor4.speed(s4);
break;
}
case (6) :
{
- XT = XT + 0.01;
- YT = YT - 0.01;
-
+ 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=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\n");
+ //motor1.speed(s1);
+ motor1.brake(1);
+ motor2.speed(s2);
+ //motor3.speed(s3);
+ motor3.brake(1);
+ motor4.speed(s4);
break;
}
case (7) :
{
- XT = XT - 0.01;
- YT = YT + 0.01;
-
+ 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=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\n");
+ //motor1.speed(s1);
+ motor1.brake(1);
+ motor2.speed(s2);
+ //motor3.speed(s3);
+ motor3.brake(1);
+ motor4.speed(s4);
break;
}
case (8) :
{
- XT = XT - 0.01;
- YT = YT - 0.01;
-
- //pc.printf("sbki Xt =%.2f x=%.2f YT=%.2f y=%.2f \n",XT,x,YT,y);
+ 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=analog=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) :
{
- XT = XT + 0.01;
+ 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=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\n");
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
break;
}
case (10) :
{
- XT = XT - 0.01;
+ 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=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\n");
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
break;
}
case (11):
{
- XT = XT + 0.01*x;
- YT = YT + 0.01*y;
+ 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);
+
+
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 X =%.2f Y =%.2f \n ",x,y);
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
break;
}
default :
{
-
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+
maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
stop = true;
- //s1 = 0;s2 =0; s3 =0; s4 =0;
- // pc.printf("Stop V1=%.2f V2=%.2f V3=%.2f V4=%.2f\n",V1,V2,V3,V4);
- }
+ pc.printf("Stop\n");
+ }
}
}
-//Begin Encoder
-void setCenter()
-{
- encoderDepan.reset();
- encoderBelakang.reset();
- encoderKanan.reset();
- encoderKiri.reset();
-}
-
-float getX()
-{
- float jarakEncDpn, jarakEncBlk;
- jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
- jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
- return (jarakEncDpn-jarakEncBlk)/2;
-}
-
-float getY()
-{
- float jarakEncKir, jarakEncKan;
- jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
- jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
- return (jarakEncKir-jarakEncKan)/2;
-}
-
-float getTetha()
-{
- float busurDpn, busurBlk, busurKir, busurKan;
- busurDpn = ((encoderDepan.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
- busurBlk = ((encoderBelakang.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
- busurKir = ((encoderKiri.getPulses())/(float)(2000.0)*K_enc)/K_robot*360.0;
- busurKan = ((encoderKanan.getPulses())/(float)(720.0)*K_enc)/K_robot*360.0;
-
- return -(busurDpn+busurBlk+busurKir+busurKan)/4;
-}
-
-void gotoXYT(float xa, float ya, float Ta)
-{
-
- errX = xa-getX();
- Vx = KpX*errX;
-
- errY = ya-getY();
- Vy = KpY*errY;
-
- errT = Ta-getTetha();
- Vt = Kp_tetha*errT;
-
- V1 = Vx+Vy-Vt;
- V2 = Vx-Vy-Vt;
- V3 = -Vx-Vy-Vt;
- V4 = -Vx+Vy-Vt;
-
- if (V1>speed1)
- { V1 = speed1; }
- else if (V1<-speed1)
- { V1 = -speed1; }
-
- if (V2>speed2)
- { V2 = speed2; }
- else if (V2<-speed2)
- { V2 = -speed2; }
-
- if (V3>speed3)
- { V3 = speed3; }
- else if (V3<-speed3)
- { V3 = -speed3; }
-
- if (V4>speed4)
- { V4 = speed4; }
- else if (V4<-speed4)
- { V4 = -speed4; }
-
- if (((errX > 0.05) || (errX<-0.05)) || ((errY > 0.05) || (errY<-0.05)) || ((errT > 0.05) || (errT<-0.05)))
- {
- motor1.speed(V1);
- motor2.speed(V2);
- motor3.speed(V3);
- motor4.speed(V4);
- // pc.printf("V1=%.2f \ ", V1);
- }
- else
- {
- motor1.brake(1);
- motor2.brake(1);
- motor3.brake(1);
- motor4.brake(1);
- //_ms(1000);
- }
- //pc.printf("%f\t%f\t%f ", errX*100,errY*100,errT);
- // wait_ms(10);
-
-}
-//End Encoder
void speedLauncher()
{
@@ -535,17 +726,10 @@
// Set baud rate - 115200
joystick.setup();
pc.baud(115200);
- wait_ms(1000);
- setCenter();
wait_ms(500);
- //Posisi Awal
- XT = 0;
- YT = 0;
- Tetha = 0;
pc.printf("Ready...\n");
kalibrasi();
- waktu.start();
while(1)
{
// Interrupt Serial
@@ -570,12 +754,9 @@
if (joystick.segitiga_click) {Launcher = !Launcher;}
if (joystick.lingkaran_click){
ServoGo = true;
- waktu.reset();
}
if (joystick.silang) {
- XT = 0;
- YT = 0;
- Tetha = 0;
+
//pc.printf("x..\n");
}
speedLauncher();
@@ -584,7 +765,7 @@
joystick.idle();
}
- gotoXYT(XT,YT,Tetha);
+// gotoXYT(XT,YT,Tetha);
}
}