convert_KeilToMbed
Dependencies: DigitDisplay Motor PID mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
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);