KRAI 2017
Pakai akselerasi
Dependencies: DigitDisplay Motor PID Ping mbed millis
Fork of
MainProgram_BaseBaru_fix_omni_5April_23-04
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 16:90119f03c5d1
- Parent:
- 15:98f0d56b14f0
- Child:
- 17:e4229d77a5ab
--- a/main.cpp Fri Dec 09 12:03:44 2016 +0000
+++ b/main.cpp Tue Jan 24 12:34:29 2017 +0000
@@ -28,10 +28,10 @@
/* Tombol segitiga => Aktif motor Launcher */
/* Tombol lingkaran=> Aktif servo Launcher */
/* Tombol L3 => PWM Launcher dikurangin */
-/* Tombol R3 => PWM Launcher ditambahin */
+/* Tombol R3 => PWM Launcher ditambahin */
/* */
-/* */
-/* */
+/* Bismillahirahmanirrahim */
+/* Jagalah Kebersihan kodingan */
/****************************************************************************/
@@ -39,209 +39,141 @@
#include "JoystickPS3.h"
#include "Motor.h"
#include "Servo.h"
-
-//#define koefperlambatan 0.8
#include "encoderKRAI.h"
-#define pi 22/7
-#define diaEncoder 0.058
-#define PPR 1000
-#define diaRobot 0.64
-#define pinservo1 PC_8
-//PC 9
-#define pinservo2 PC_9
-
-float K_enc = pi*diaEncoder;
-float K_robot = pi*diaRobot;
+/***********************************************/
+/* Konstanta dan Variabel */
+/***********************************************/
+#define PI 3.14159265
+#define D_ENCODER 0.058
+#define D_ROBOT 0.64
-float speed1=0.6;
-float speed2=0.6;
-float speed3=0.6;
-float speed4=0.6;
-float speedB=0.43 ;
-float speedL=0.4;
+float k_enc = PI*D_ENCODER;
+float k_robot = PI*D_ROBOT;
-float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
-
-Timer waktu;
-//float jarakGlobalY;
+float speed1 =0.6;
+float speed2 =0.6;
+float speed3 =0.6;
+float speed4 =0.6;
+float speedB =0.43;
+float speedL =0.4;
-// 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);
-*/
+float kpX=0.5, kpY=0.5, kp_tetha=0.03;
+
+/* Deklarasi 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); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderKanan( PC_12, PD_2, 720, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderKiri( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING); //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
-// 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 Motor Base */
+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
-// 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
+/* Deklarasi Motor Launcher */
+Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
+Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
-//Motor Atas
-Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
-Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
-
-//Servo Atas
+/* Deklarasi Servo Launcher */
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;
+/**
+ * posX dan posY berdasarkan arah robot
+ * encoder Depan & Belakang sejajar sumbu Y
+ * encoder Kanan & Kiri sejajar sumbu X
+ **/
-void detect_encoder(float speed);
-void setCenter();
-float getY();
-float getX();
-float getTetha();
+/* Variabel Encoder */
+float jarak, posX, posY; //
+float XT, YT, Tetha; // Jarak Target Robot
+float errX, errY, errT, Vt, Vx, Vy; // Variabel yang didapatkan encoder
+float v1, v2, v3, v4; // Variabel kecepatan motor dari encoder
-// Inisialisasi Pin TX-RX Joystik dan PC
+/* Fungsi dan Procedur Encoder */
+void setCenter(); // Fungsi reset agar robot di tengah
+float getY(); // FUngsi mendapatkan jarak Y
+float getX(); // FUngsi mendapatkan jarak X
+float getTetha(); // FUngsi mendapatkan jarak Tetha
+
+/* 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;
-
-//bool perlambatan=0;
+/* Variabel Stick */
char case_ger;
-bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
-bool stop = true;
-bool Launcher = false,ServoGo = false;
-float jLX,jLY;
-double vcurr;
-float x,y; // untuk analoghat kiri
-float errorx=0,errory=0;
-
-// Fungsi mapping 0-255 ke -128 sampai 127
-float mapping(float a,float error)
-{
- float hasil,b;
- b = (float)((a-128)/128);
- if (b>(error - 0.2) && b<(error + 0.2))
- {
- hasil = 0;
- } else {
- hasil = b;
- }
- return (hasil);
-}
+bool launcher = false,servoGo = false;
-// Kalibrasi tombol analog kiri
-void kalibrasi()
-{
- errorx = (jLX - 128)/128;
- errory = (jLY - 128)/128;
-
-}
-
-// simpan data analog
-void baca_analog()
-{
- jLX = joystick.LX;
- jLY = joystick.LY;
-
- // Pembacaan nilai Y terbalik
- x = mapping(jLX,errorx);
- y = -mapping(jLY,errory);
-}
-
-// Gerak dari Motor base
-int case_gerak()
-{
+/***********************************************/
+/* Deklarasi Fungsi dan Procedure */
+/***********************************************/
+int case_gerak(){
+/*****************************************************
+ ** Gerak Motor Base
+ ** Case 1 : Pivot kanan
+ ** Case 2 : Pivot Kiri
+ ** Case 3 : Maju
+ ** Case 4 : Mundur
+ ** Case 5 : Serong Atas Kanan
+ ** Case 6 : Serong Bawah Kanan
+ ** Case 7 : Serong Atas Kiri
+ ** Case 8 : Serong Bawah Kiri
+ ** Case 9 : Kanan
+ ** Case 10 : Kiri
+ ** Case 12 : break
+ ****************************************************/
int casegerak;
- baca_analog();
- if (!joystick.L1 && joystick.R1) {
+ if (!joystick.L1 && joystick.R1) {
// Pivot Kanan
casegerak = 1;
- } else if (!joystick.R1 && joystick.L1) {
+ }
+ else if (!joystick.R1 && joystick.L1) {
// Pivot Kiri
casegerak = 2;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Maju
casegerak = 3;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Mundur
casegerak = 4;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Atas Kanan
casegerak = 5;
- } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
+ }
+ else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {
// Serong Bawah Kanan
casegerak = 6;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Atas Kiri
casegerak = 7;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {
// Serong Bawah Kiri
casegerak = 8;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
casegerak = 9;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
// Kiri
casegerak = 10;
- } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
- // case gerak analog on off
- if (analog){
- casegerak = 11;
- } else {
- casegerak = 12;
- }
-}
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ casegerak = 12;
+ }
return(casegerak);
}
-
-
-/**
-
-** Case 1 : Pivot kanan
-** Case 2 : Pivot Kiri
-** Case 3 : Maju
-** Case 4 : Mundur
-** Case 5 : Serong Atas Kanan
-** Case 6 : Serong Bawah Kanan
-** Case 7 : Serong Atas Kiri
-** Case 8 : Serong Bawah Kiri
-** Case 9 : Kanan
-** Case 10 : Kiri
-** Case 11 : Analog
-** Case 11 : break
-
-**/
-
-
-// Pergerakan dari base
-void aktuator()
-{
- //Servo
- if (ServoGo){
+void aktuator(){
+/* Fungsi untuk menggerakkan servo */
+ // Servo
+ if (servoGo){
servoS.position(20);
wait_ms(500);
servoS.position(-28);
@@ -254,16 +186,15 @@
}
wait_ms(500);
servoB.position(0);
- ServoGo = false;
-
- }else{
+ servoGo = false;
+ }
+ else{
servoS.position(20);
servoB.position(0);
-
}
// Motor Atas
- if (Launcher) {
+ if (launcher) {
motorld.speed(speedL);
motorlb.speed(speedB);
}else{
@@ -272,317 +203,201 @@
}
// MOTOR Bawah
- switch (case_ger)
- {
-case (1):
- {
- 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);
-
-
+ switch (case_ger) {
+ case (1):{
+ Tetha = Tetha - 0.05;
break;
}
- case (2):
- {
- 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);
-
-
- break;
- }
- case (3):
- {
- 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);
-
+ case (2):{
+ Tetha = Tetha + 0.05;
break;
}
- case (4):
- {
- 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);
-
-
+ case (3):{
+ YT = YT + 0.01;
break;
}
- case (5) :
- {
- 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);
-
-
+ case (4):{
+ YT = YT - 0.01;
break;
}
- case (6) :
- {
- 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);
-
-
+ case (5) :{
+ XT = XT + 0.01;
+ YT = YT + 0.01;
break;
}
- case (7) :
- {
- 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);
-
-
+ case (6) :{
+ XT = XT + 0.01;
+ YT = YT - 0.01;
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);
-
-
+ case (7) :{
+ XT = XT - 0.01;
+ YT = YT + 0.01;
break;
}
- case (9) :
- {
- 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);
-
+ case (8) :{
+ XT = XT - 0.01;
+ YT = YT - 0.01;
break;
}
- case (10) :
- {
- 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);
-
-
+ case (9) :{
+ XT = XT + 0.01;
break;
}
- case (11):
- {
-
- 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);
-
+ case (10) :{
+ XT = XT - 0.01;
break;
- }
- default :
- {
-
- 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);
- }
- }
+ }
+ default :{}
+ } //end of switch
}
-//Begin Encoder
-
-void setCenter()
-{
+void setCenter(){
+/* Fungsi untuk menentukan center dari robot */
encoderDepan.reset();
encoderBelakang.reset();
encoderKanan.reset();
encoderKiri.reset();
}
-float getX()
-{
+float getX(){
+/* Fungsi untuk mendapatkan jarak X */
float jarakEncDpn, jarakEncBlk;
- jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*K_enc;
- jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*K_enc;
+ jarakEncDpn = (encoderDepan.getPulses())/(float)(2000.0)*k_enc;
+ jarakEncBlk = (encoderBelakang.getPulses())/(float)(2000.0)*k_enc;
return (jarakEncDpn-jarakEncBlk)/2;
}
-float getY()
-{
+float getY(){
+/* Fungsi untuk mendapatkan jarak Y */
float jarakEncKir, jarakEncKan;
- jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*K_enc;
- jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*K_enc;
+ jarakEncKir = (encoderKiri.getPulses())/(float)(2000.0)*k_enc;
+ jarakEncKan = (encoderKanan.getPulses())/(float)(720.0)*k_enc;
return (jarakEncKir-jarakEncKan)/2;
}
-float getTetha()
-{
+float getTetha(){
+/* Fungsi untuk mendapatkan nilai tetha */
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;
+ 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)
-{
+void gotoXYT(float xa, float ya, float Ta){
+/* Fungsi untuk bergerat ke target */
+ 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);
+ }
+ else{
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ }
+}
- 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()
-{
+void speedLauncher(){
+/* Fungsi untuk speed launcher */
if (joystick.R3_click and speedL < 0.8){
- speedL = speedL + 0.01;}
+ speedL = speedL + 0.01;
+ }
if (joystick.L3_click and speedL > 0.1){
- speedL = speedL - 0.01;}
+ speedL = speedL - 0.01;
+ }
if (joystick.R2_click and speedB < 0.8 ){
- speedB = speedB + 0.01;}
+ 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);
+ speedB = speedB - 0.01;
+ }
}
-
-
-int main (void)
-{
- // Set baud rate - 115200
+/***********************************************/
+/* Main Function */
+/***********************************************/
+
+int main (void){
+ /* Set baud rate - 115200 */
joystick.setup();
- pc.baud(115200);
wait_ms(1000);
setCenter();
wait_ms(500);
- //Posisi Awal
+ /* Posisi Awal */
XT = 0;
YT = 0;
Tetha = 0;
- pc.printf("Ready...\n");
- kalibrasi();
- waktu.start();
+
+ /* Untuk mendapatkan serial dari Arduino */
while(1)
{
// Interrupt Serial
- joystick.idle();
- if(joystick.readable() ) {
+ joystick.idle();
+
+ if(joystick.readable() ) {
// Panggil fungsi pembacaan joystik
joystick.baca_data();
+
// Panggil fungsi pengolahan data joystik
joystick.olah_data();
- //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
- //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
+
+ // Masuk ke case gerak
case_ger = case_gerak();
aktuator();
-
- //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
-
- //kalibrasi
- if (joystick.START){
- kalibrasi();}
- // analog switch mode
- if (joystick.SELECT_click) {analog = !analog;}
- if (joystick.segitiga_click) {Launcher = !Launcher;}
- if (joystick.lingkaran_click){
- ServoGo = true;
- waktu.reset();
- }
+
+ if (joystick.segitiga_click) launcher = !launcher;
+ if (joystick.lingkaran_click) servoGo = true;
if (joystick.silang) {
XT = 0;
YT = 0;
Tetha = 0;
- //pc.printf("x..\n");
- }
+ }
speedLauncher();
- } else {
- joystick.idle();
-
+ }
+ else {
+ joystick.idle();
}
gotoXYT(XT,YT,Tetha);