Bryan Purba
/
DagonFly_Nasional_Mbed
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: DigitDisplay Motor PID mbed millis
Fork of
DagonFly__RoadToJapan_15Mei_Ultimate
by 
KRAI 2017
Revision 52:a39e26b935a9, committed 2017-07-02
- Comitter:
- be_bryan
- Date:
- Sun Jul 02 01:37:31 2017 +0000
- Parent:
- 51:df6391c3fa68
- Commit message:
- Convert_KeilToMbed
Changed in this revision
| Ping.lib | Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Ping.lib Mon May 15 07:49:13 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/rosienej/code/Ping/#6996f66161d7
--- a/main.cpp Mon May 15 07:49:13 2017 +0000
+++ b/main.cpp Sun Jul 02 01:37:31 2017 +0000
@@ -43,7 +43,6 @@
#include "Motor.h"
#include "encoderKRAI.h"
#include "millis.h"
-#include "Ping.h"
#include "DigitDisplay.h"
/***********************************************/
@@ -51,242 +50,338 @@
/***********************************************/
#define PI 3.14159265
#define D_ENCODER 10 // Diameter Roda Encoder
-#define D_ROBOT 80 // Diameter Roda Robot
+#define D_ROBOT1 54 // Diameter Roda Robot dari kiri ke kanan
+#define D_ROBOT2 79 // Diameter Roda Robot dari depan ke belakang
-// Variable Atas
-// indek 2 untuk motor depan, 1 untuk motor belakang
-double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
-const double kpA1=0.19982, kdA1=0.91824, kiA1=0.00072609;
-const double kpA2=0.20481, kdA2=0.92191, kiA2=0.00076326;
-double a1,b1,c1;
-double a2,b2,c2;
-double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
-double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
-double previous_speed1 = 0;
-double previous_speed2 = 0;
+/**********************************************************/
+/* LAUNCHER */
+bool isLauncher = false;
+double speed;
+const double maxSpeed = 1.0, minSpeed = -1.0, Ts = 20;
+const double kpA=0.0004667, kdA=0, kiA=6.2645e-07;
+double a,b,c;
+double current_error, previous_error_1 = 0, previous_error_2 = 0;
+double previous_speed = 0;
-float rpm, rpm2;
-double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
-const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
-
-const float pwmPowerUp = 1.0;
-const float pwmPowerDown = -1.0;
-
-double jarak_ping=0;
-double ping_target = 15;
-double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
-double ping_Kp = -0.2747, ping_Kd = -0.535, ping_Ts=10;
-double ping_pwm, ping_previous_pwm = 0;
+float rpm;
+double target_rpm = 2600;
+const float maxRPM = 4000, minRPM = 0.0;
+/**********************************************************/
-// Variable Bawah
-float PIVOT = 0.17; // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn = 1.0; // Tunning PWM motor Depan
-float tuneBlk = 1.0; // Tunning PWM motor belakang
+/**********************************************************/
+/* BASE */
+float PIVOT = 0.3; // PWM Pivot Kanan, Pivot Kiri
float tuneAksel = 0.6;
-int aksel = 0;
-float tuneAkselBlk = 0.4;
-float tuneR = 1.0;
-float tuneL = 1.0;
-float serong = 0.4;
-float rasio = 1.4545;
-float t_new = 0.1;
+float serong = 0.45;
+float rasio= (D_ROBOT2/D_ROBOT1);
+//float jariR = 15, jariL = 64, jariDpn = 27, jariBlk = 27;
+float K_enc = PI*D_ENCODER;
+float K_robot1 = PI*D_ROBOT1;
+float K_robot2 = PI*D_ROBOT2;
+float speedDpn, speedBlk, speedR, speedL;
+//unsigned long int previousMillis=0;
+float jarakXnow, jarakXbefore, jarakYnow, jarakYbefore; //Untuk aksel
+/* PID BASE */
+float setpointX=0.0, setpointY=0.0, setpointT=0.0;
+const float maxPIDSpeedX = 0.6, minPIDSpeedX = -0.6;
+const float maxPIDSpeedY = 0.6, minPIDSpeedY = -0.6;
+const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
+const float TsBase = 50;
+double current_efrror, previous_error1 = 0, previous_error2 = 0;
+float PIDSpeedX, PIDSpeedY, PIDSpeedT;
+float errorX, previous_errorX=0, derivativeX, integralX=0;
+float errorY, previous_errorY=0, derivativeY, integralY=0;
+float errorT, previous_errorT=0, derivativeT, integralT=0;
+float KpX=0.06, KiX=0, KdX=0.4;
+float KpY=0.06, KiY=0, KdY=0.4;
+float KpT=0.08, KiT=0, KdT=3.33;
+//float KpT=0.08, KiT=0, KdT=0.0;
+// Otomatis sb X
+bool autoX = false, auto_rotate = false;
+bool reset_encoder, inAutoRotate;
+int mode=0;
+/**********************************************************/
-/* variable tunning */
-float tunning_L;
-float tunning_R;
-float tunning_Dpn;
-float tunning_Blk;
-
-/* Deklarasi Variable Millis */
-unsigned long int previousMillis = 0; // PID launcher
-unsigned long int currentMillis;
-unsigned long int previousMillis2 = 0; // PID launcher
-unsigned long int currentMillis2;
-unsigned long int previousMillis3 = 0; // Pneumatik
-unsigned long int previousMillis4 = 0; // Ping
-unsigned long int previousMillis5 = 0; // Display
-unsigned long int previousMillis6 = 0; // Display
-
-/* Variabel Stick */
-//Logic untuk masuk aktuator
-int case_joy;
-bool isLauncher = false;
+/**********************************************************/
+/* RELOADER */
bool isReload = false;
bool ReloadOn = false;
bool flag_Pneu = false;
-bool flag_paku = false;
-bool ready = false;
-
-/*****************************************************/
-/* Definisi Prosedur, Fungsi dan Setting Pinout */
-/*****************************************************/
+bool readySlideFromLeft = false;
+bool readySlideFromMiddle = false;
+bool getBack = false;
+bool isDown = false, sliderOn = false;
+bool ready = true;
+bool init_slider = true;
+bool init_lifter = true;
+bool sliderReady = false;
+bool flag_tengah = true;
+bool delay = false;
+float lempar1 = -0.8, lempar2 = -0.85, balik = 0.7;
+float lempar_naik1 = -0.85, lempar2_naik = -0.8;
+float swipper_dorong1, swipper_dorong2;
+bool slideNowLeft = false, slideNowMid = false;
+bool naik = false;
+/**********************************************************/
-/* Fungsi dan Procedur Encoder */
-void init_speed(); //
-void aktuator(); // Pergerakan aktuator berdasarkan case joystick
-int case_joystick(); // Mendapatkan case dari joystick
-//void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
+/**********************************************************/
+/* MILLIS */
+static volatile uint32_t previousMillis = 0;
+static volatile uint32_t previousMillis3 = 0; // Pneumatik
+static volatile uint32_t previousMillis5 = 0; // Display
+static volatile uint32_t previousMillis6 = 0; // pneu
+static volatile uint32_t previousMillis7 = 0; // delay
+static volatile uint32_t currentMillis;
+static volatile uint32_t previousMillisAutoX=0; // Otomatis X
+/**********************************************************/
-/* Inisialisasi Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_0,PA_1);
-Serial pc(USBTX,USBRX);
-
-/* Deklarasi Encoder Launcher */
-encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
-encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
+/**********************************************************/
+/* JOYSTICK */
+bool flag_LXmax = true, flag_LXmin = true, flag_LYmax = true, flag_LYmin = true;
+int case_joy;
+//int debug = 0;
+bool awal = true;
+/**********************************************************/
-/* Deklarasi Motor Base */
-Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
-//Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
-Motor motorBlk(PB_2, PB_15, PB_1);
-Motor motorL (PB_9, PA_12, PA_6);
-Motor motorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5);
+/**********************************************************/
+/* Motor */
+Motor motorDpn(PB_6, PA_10, PB_13);
+Motor motorBlk(PB_7, PA_8, PB_1);
+Motor motorL(PB_9, PA_11, PA_12);
+Motor motorR(PB_8, PA_9, PA_5);
+Motor Launcher(PA_6, PB_2, PB_12);
+Motor powerScrew(PA_7, PB_14, PB_15);
+Motor swipper(PB_0, PA_4, PA_13);
+/**********************************************************/
-/* Deklarasi Motor Launcher */
-Motor launcherDpn(PA_5,PA_11,PB_12); // pwm ,fwd, rev
-Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev
-Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev
+/**********************************************************/
+/* Encoder */
+encoderKRAI Benc_R(PC_5, PC_4, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_L(PC_7, PC_6, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_Dpn(PC_1, PC_0, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Benc_Blk(PC_3, PC_2, 540, encoderKRAI::X4_ENCODING);
+encoderKRAI Lenc(PC_9, PC_8, 1000, encoderKRAI::X4_ENCODING);
+
+/**********************************************************/
-/* Deklarasi Penumatik Launcher */
-DigitalOut pneumatik(PA_4, PullUp);
-DigitalOut pneu_paku(PC_2, PullUp);
+/**********************************************************/
+/* Display 7 Segmen */
+DigitDisplay display (PC_12, PB_10);
+/**********************************************************/
-/*Dekalrasi Limit Switch */
-//DigitalIn infraAtas(PC_9, PullUp);
-DigitalIn limitTengah(PA_9, PullUp);
-DigitalIn limitBawah(PC_7, PullUp);
-DigitalIn limitBawah1(PA_7, PullUp);
+/**********************************************************/
+/* Limit Switch */
+DigitalIn limitKanan(PD_2, PullUp);
+DigitalIn limitTengah(PB_4, PullUp);
+DigitalIn limitKiri(PB_5, PullUp);
+DigitalIn limitBawah(PC_13, PullUp);
+DigitalIn limitAtas(PC_14, PullUp);
+DigitalIn limitFB(PC_15, PullUp);
+/**********************************************************/
-/*deklarasi PING ultrasonic*/
-Ping pingAtas(PC_15);
+/**********************************************************/
+/* Pneumatik */
+DigitalOut Pneumatik(PA_14, PullUp);
+DigitalOut Elevator(PA_15, PullUp);
+/****************************************************/
-/*Deklarasi Display*/
-DigitDisplay display (PA_8, PC_8);
+/**********************************************************/
+/* Serial */
+Serial pc(USBTX,USBRX);
+joysticknucleo joystick(PA_0,PA_1);
+/**********************************************************/
-/****************************************************/
/* Deklarasi Fungsi dan Procedure */
/****************************************************/
+
+void setCenter()
+ {
+ Benc_R.reset();
+ Benc_L.reset();
+ Benc_Dpn.reset();
+ Benc_Blk.reset();
+ Lenc.reset();
+ }
+
+float getX()
+{
+ float jarakEncDpn, jarakEncBlk;
+ jarakEncDpn = (Benc_Dpn.getPulses())/(float)(540.0)*K_enc;
+ jarakEncBlk = (Benc_Blk.getPulses())/(float)(540.0)*K_enc;
+ return (jarakEncDpn-jarakEncBlk)/2;
+}
+
+float getY()
+{
+ float jarakEncKir, jarakEncKan;
+ jarakEncKir = (Benc_L.getPulses())/(float)(540.0)*K_enc;
+ jarakEncKan = (Benc_R.getPulses())/(float)(540.0)*K_enc;
+ return (jarakEncKir-jarakEncKan)/2;
+}
+
+float getTetha()
+{
+ float busurDpn, busurBlk, busurKir, busurKan, sudut;
+ busurKir = ((Benc_L.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+ busurKan = ((Benc_R.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+ busurDpn = ((Benc_Dpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
+ busurBlk = ((Benc_Blk.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
+ sudut = -(busurDpn+busurBlk+busurKir+busurKan)/4;
+ if (sudut>=360.0 || sudut<=-360.0)
+ { sudut = 0.0; }
+ return sudut;
+}
+
+void PIDcompute_X()
+{
+ errorX = setpointX - getX();
+ integralX = integralX + errorX*TsBase;
+ derivativeX = (errorX - previous_errorX)/TsBase;
+ PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
+ if(PIDSpeedX > maxPIDSpeedX)
+ { PIDSpeedX = maxPIDSpeedX; }
+ else if (PIDSpeedX < minPIDSpeedX)
+ { PIDSpeedX = minPIDSpeedX; }
+ previous_errorX = errorX;
+}
+
+void PIDcompute_Y()
+{
+ errorY = setpointY - getY();
+ integralY = integralY + errorY*TsBase;
+ derivativeY = (errorY - previous_errorY)/TsBase;
+ PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
+ if(PIDSpeedY > maxPIDSpeedY)
+ { PIDSpeedY = maxPIDSpeedY; }
+ else if (PIDSpeedY < minPIDSpeedY)
+ { PIDSpeedY = minPIDSpeedY; }
+ previous_errorY = errorY;
+}
+
+void PIDcompute_T()
+{
+ errorT = setpointT - getTetha();
+ integralT = integralT + errorT*TsBase;
+ derivativeT = (errorT - previous_errorT)/TsBase;
+ PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
+ if(PIDSpeedT > maxPIDSpeedT)
+ { PIDSpeedT = maxPIDSpeedT; }
+ else if (PIDSpeedT < minPIDSpeedT)
+ { PIDSpeedT = minPIDSpeedT; }
+ previous_errorT = errorT;
+}
+
int case_joystick()
{
-//---------------------------------------------------//
-// Gerak Motor Base //
-// Case 1 : Pivot kanan //
-// Case 2 : Pivot Kiri //
-// Case 3 : Kanan //
-// Case 4 : Kiri //
-// Case 5 : Break //
-//---------------------------------------------------//
-
int caseJoystick;
- if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
// Pivot Kanan
caseJoystick = 1;
- }
- 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)) {
// Pivot Kiri
caseJoystick = 2;
}
- else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
+ else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
// tambah rpm dengan nilai tertentu
- caseJoystick = 31;
+ caseJoystick = 31;
}
- else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
+ else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
// kurangi rpm dengan nilai tertentu
- caseJoystick = 32;
+ caseJoystick = 32;
}
- else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
+ else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
// kurangi rpm dengan nilai tertentu
- caseJoystick = 33;
+ caseJoystick = 33;
}
- 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 + Rotasi kanan
caseJoystick = 17;
- }
- 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 + Rotasi kiri
caseJoystick = 18;
}
- 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 + Rotasi kanan
caseJoystick = 19;
- }
- 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 + Rotasi kiri
caseJoystick = 20;
}
- 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 + Rotasi kanan
caseJoystick = 21;
- }
- 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 + Rotasi kiri
caseJoystick = 22;
}
- 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 + Rotasi kanan
caseJoystick = 23;
- }
- 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 + Rotasi kiri
caseJoystick = 24;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) {
// Kanan + segitiga
caseJoystick = 25;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) {
// Kiri + segitiga
caseJoystick = 26;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) {
// Kanan + lingkaran
caseJoystick = 27;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) {
// Kiri + lingkaran
caseJoystick = 28;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) {
// Kanan + kotak
caseJoystick = 29;
}
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) {
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) {
// Kiri + kotak
caseJoystick = 30;
}
- 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)) {
// Serong kanan maju
- caseJoystick = 13;
+ caseJoystick = 13;
}
- 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)) {
// Serong kiri maju
- caseJoystick = 14;
+ caseJoystick = 14;
}
- 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)) {
// Serong kanan mundur
- caseJoystick = 15;
+ caseJoystick = 15;
}
- 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)) {
// Serong kiri mundur
- caseJoystick = 16;
- }
- else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ caseJoystick = 16;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
// Kanan
caseJoystick = 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)) {
// Kiri
caseJoystick = 4;
}
- 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)) {
// Atas -- Maju
- caseJoystick = 8;
+ caseJoystick = 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)) {
// Bawah -- Mundur
- caseJoystick = 9;
- }
+ caseJoystick = 9;
+ }
else if (joystick.segitiga_click){
// Motor Launcher
caseJoystick = 5;
@@ -294,495 +389,631 @@
else if (joystick.R2_click){
// Target Pulse PID ++ Motor Depan
caseJoystick = 6;
- }
+ }
else if (joystick.L2_click){
- // Target Pulse PID -- Motor
+ // Target Pulse PID -- Motor
caseJoystick = 7;
}
else if (joystick.silang_click){
// Pnemuatik ON
caseJoystick = 10;
}
- else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
+ else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
// Power Screw Up
caseJoystick = 11;
- }
- else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
+ }
+ else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
// Power Screw Down
- caseJoystick = 12;
- }
- else if (joystick.LYp_click) {
- // Paku Bumi Naik
+ caseJoystick = 12;
+ }
+ else if (joystick.L3_click){
+ // elevasi ON/OFF
caseJoystick = 34;
}
- else if (joystick.LYn_click) {
- // Paku Bumi Turun
- caseJoystick = 35;
- }
else
{
- tuneAksel = 0.6;
- aksel = 0;
+ //tuneAksel = 0.6;
}
-
return(caseJoystick);
}
+void aturSetpoint()
+{
+ switch (mode)
+ {
+ case (-2):
+ setpointT = -45.0;
+ break;
+ case (-1):
+ setpointT = -20.0;
+ break;
+ case (0):
+ setpointT = 0.0;
+ break;
+ case (1):
+ setpointT = 20.0;
+ break;
+ case (2):
+ setpointT = 45.0;
+ break;
+ }
+}
+void cekMode()
+{
+ if (mode<-2){mode = -2;}
+ else if (mode>2){mode = 2;}
+}
-void init_rpm (){
- if (target_rpm>maxRPM-2){
- target_rpm = maxRPM-2;
+void case_autoRotate()
+{
+ if ((joystick.LY>175)&&flag_LYmax) // Bawah, Keluar
+ {
+ flag_LYmax = false;
+ auto_rotate = false;
+ setpointT = 0.0;
+ inAutoRotate = false;
+ mode = 0;
+ }
+ else if ((15<joystick.LY&&joystick.LY<175)&&!flag_LYmax){ flag_LYmax = true; }
+
+ if ((joystick.LY<15)&&flag_LYmin) // Atas, Masuk (aktif)
+ {
+ flag_LYmin = false;
+ auto_rotate = true;
+ reset_encoder = true;
+ inAutoRotate =true;
+ setCenter();
+ mode = 0;
+ }
+ else if ((15<joystick.LY && joystick.LY<175)&&!flag_LYmin){ flag_LYmin = true;}
+
+ if ((joystick.LX>200)&&flag_LXmax)
+ {
+ flag_LXmax = false;
+ mode--;
+ cekMode();
+ }
+ else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmax){ flag_LXmax = true; }
+
+ if ((joystick.LX<55)&&flag_LXmin)
+ {
+ flag_LXmin = false;
+ mode++;
+ cekMode();
+ }
+ else if ((55<joystick.LX&&joystick.LX<200)&&!flag_LXmin){ flag_LXmin = true; }
+}
+
+void init_rpm ()
+{
+ if (target_rpm>maxRPM){
+ target_rpm = maxRPM;
}
if (target_rpm<minRPM){
target_rpm = minRPM;
}
- if (target_rpm2>maxRPM){
- target_rpm2 = maxRPM;
+}
+
+void sliderMove()
+{
+ if (readySlideFromLeft && slideNowLeft && !delay)
+ {
+ swipper.speed(swipper_dorong2);
+ if(!(limitTengah) && flag_tengah)
+ {
+ swipper.brake(1);
+ slideNowLeft = false;
+ slideNowMid = false;
+ readySlideFromLeft = false;
+ readySlideFromMiddle = true;
+ getBack = false;
+ flag_tengah = false;
+ }
+ else {flag_tengah = true;}
+ }
+ else if (readySlideFromMiddle && slideNowMid && !delay)
+ {
+ swipper.speed(swipper_dorong1);
+ if(!(limitKanan))
+ {
+ readySlideFromMiddle = false;
+ slideNowLeft = false;
+ slideNowMid = false;
+ readySlideFromLeft = false;
+ getBack = true;
+ }
}
- if (target_rpm2<minRPM+2){
- target_rpm2 = minRPM+2;
+ else if (getBack)
+ {
+ swipper.speed(balik);
+ if(!(limitKiri))
+ {
+ swipper.brake(1);
+ slideNowLeft = false;
+ slideNowMid = false;
+ readySlideFromLeft = false;
+ readySlideFromMiddle = false;
+ getBack = false;
+ sliderReady = false;
+ ReloadOn = true;
+ isDown = false;
+ }
+ }
+ else
+ {
+ swipper.brake(1);
+ }
+}
+
+void lifterMove()
+{
+ if(ReloadOn)
+ {
+ if(isDown)
+ {
+ powerScrew.speed(-1.0);
+ if(!(limitBawah))
+ {
+ ReloadOn = false;
+ isDown = false;
+ }
+ }
+ else if (!(limitAtas))
+ {
+ isDown = true;
+ readySlideFromMiddle = false;
+ getBack = true;
+ }
+ else if (sliderReady)
+ {
+ powerScrew.brake(1);
+ sliderMove();
+ }
+ else if(!(limitFB))
+ {
+ sliderReady = true;
+ readySlideFromLeft = true;
+ }
+ else
+ {
+ powerScrew.speed(1.0);
+ }
+ }
+ else
+ {
+ powerScrew.brake(1);
}
}
void aktuator()
{
switch (case_joy) {
- case (1):
- {
+ case (1):
+ {
// Pivot Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT-t_new);
- motorL.speed(-rasio*PIVOT-t_new);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
- case (2):
+ case (2):
{
// Pivot Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT+t_new);
- motorL.speed(rasio*PIVOT+t_new);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
break;
}
- case (17):
- {
+ case (17):
+ {
// Kanan + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT-t_new);
- motorL.speed(-rasio*PIVOT-t_new);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
- case (18):
+ case (18):
{
// Kanan + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT+t_new);
- motorL.speed(rasio*PIVOT+t_new);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
break;
}
- case (19):
- {
+ case (19):
+ {
// Kiri + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT-t_new);
- motorL.speed(-rasio*PIVOT-t_new);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
- case (20):
+ case (20):
{
// Kiri + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT+t_new);
- motorL.speed(rasio*PIVOT+t_new);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
break;
}
- case (21):
- {
+ case (21):
+ {
// Maju + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT-t_new);
- motorL.speed(-rasio*PIVOT-t_new);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
- case (22):
+ case (22):
{
// Maju + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT+t_new);
- motorL.speed(rasio*PIVOT+t_new);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
break;
}
- case (23):
- {
+ case (23):
+ {
// Mundur + Rotasi Kanan
motorDpn.speed(-PIVOT);
motorBlk.speed(-PIVOT);
- motorR.speed(-rasio*PIVOT-t_new);
- motorL.speed(-rasio*PIVOT-t_new);
+ motorR.speed(-rasio*PIVOT);
+ motorL.speed(-rasio*PIVOT);
break;
}
- case (24):
+ case (24):
{
// Mundur + Rotasi Kiri
motorDpn.speed(PIVOT);
motorBlk.speed(PIVOT);
- motorR.speed(rasio*PIVOT+t_new);
- motorL.speed(rasio*PIVOT+t_new);
+ motorR.speed(rasio*PIVOT);
+ motorL.speed(rasio*PIVOT);
break;
}
- case (25):
+ case (25):
{
// Kanan + segitiga
isLauncher = !isLauncher;
break;
}
- case (26):
+ case (26):
{
// Kiri + segitiga
isLauncher = !isLauncher;
break;
}
- case (27):
+ case (27):
{
- // Kanan + lingkaran
- ReloadOn = !ReloadOn;
- isReload = false;
+ // Kanan + lingkaran ----- Lifter Up
+ ReloadOn = !ReloadOn;
+ isDown = false;
break;
}
- case (28):
+ case (28):
{
- // Kiri + lingkaran
- ReloadOn = !ReloadOn;
- isReload = false;
+ // Kiri + lingkaran ----- Lifter Up
+ ReloadOn = !ReloadOn;
+ isDown = false;
break;
}
- case (29):
+ case (29):
{
- // Kanan + kotak
- ReloadOn = !ReloadOn;
- isReload = true;
+ // Kanan + kotak ----- Lifter Down
+ ReloadOn = !ReloadOn;
+ isDown = true;
break;
}
- case (30):
+ case (30):
{
- // Kiri + kotak
- ReloadOn = !ReloadOn;
- isReload = true;
+ // Kiri + kotak ----- Lifter Down
+ ReloadOn = !ReloadOn;
+ isDown = true;
break;
}
- case (13) :
+ case (13) :
{
// Serong kanan maju
+ awal = true;
motorDpn.speed(-serong);
- motorL.speed(-serong-t_new);
motorBlk.speed(serong);
- motorR.speed(serong+t_new);
+ motorR.speed(serong);
+ motorL.speed(-serong);
break;
}
- case (14) :
+ case (14) :
{
// Serong kiri maju
+ awal = true;
motorDpn.speed(serong);
- motorR.speed(serong+t_new);
motorBlk.speed(-serong);
- motorL.speed(-serong-t_new);
+ motorR.speed(serong);
+ motorL.speed(-serong);
break;
}
- case (15) :
+ case (15) :
{
- // Serong kanan mundur
+ // Serong kanan mundur
+ awal = true;
motorDpn.speed(-serong);
- motorR.speed(-serong-t_new);
motorBlk.speed(serong);
- motorL.speed(serong+t_new);
+ motorR.speed(-serong);
+ motorL.speed(serong);
break;
}
- case (16) :
+ case (16) :
{
// Serong kiri mundur
+ awal = true;
motorDpn.speed(serong);
- motorL.speed(serong+t_new);
motorBlk.speed(-serong);
- motorR.speed(-serong-t_new);
+ motorR.speed(-serong);
+ motorL.speed(serong);
break;
}
- case (3) :
+ case (3) :
{
// Kanan
- aksel++;
- if (aksel>300)
- tuneAksel = 0.9;
+ if (awal)
+ {
+ tuneAksel = 0.6;
+ awal = false;
+ }
+ jarakXnow = getX();
+ if(jarakXnow-jarakXbefore>5.0)
+ {
+ tuneAksel = tuneAksel+0.05;
+ jarakXbefore = jarakXnow;
+ }
+ if (tuneAksel>1.0) tuneAksel = 1.0;
motorDpn.speed(-tuneAksel);
motorBlk.speed(tuneAksel);
motorR.brake(1);
motorL.brake(1);
break;
- }
- case (4) :
+ }
+ case (4) :
{
// Kiri
- aksel++;
- if (aksel>300)
- tuneAksel = 0.9;
-
+ if (awal)
+ {
+ tuneAksel = 0.6;
+ awal = false;
+ }
+ jarakXnow = getX();
+ if(jarakXnow-jarakXbefore<-5.0)
+ {
+ tuneAksel = tuneAksel+0.05;
+ jarakXbefore = jarakXnow;
+ }
+ if (tuneAksel>1.0) tuneAksel = 1.0;
+
motorDpn.speed(tuneAksel);
motorBlk.speed(-tuneAksel);
motorR.brake(1);
motorL.brake(1);
break;
- }
- case (8) :
+ }
+ case (8) :
{
// Maju
- aksel++;
- if (aksel>300)
- tuneAksel = 0.9;
-
+ if (awal)
+ {
+ tuneAksel = 0.525;
+ awal = false;
+ }
+ jarakYnow = getY();
+ if(jarakYnow-jarakYbefore>5.0)
+ {
+ tuneAksel = tuneAksel+0.025;
+ jarakYbefore = jarakYnow;
+ }
+ if (tuneAksel>0.675) tuneAksel = 0.675;
+
+ motorDpn.brake(1);
+ motorBlk.brake(1);
motorR.speed(tuneAksel);
motorL.speed(-tuneAksel);
- motorDpn.brake(1);
- motorBlk.brake(1);
break;
}
- case (9) :
+ case (9) :
{
// Mundur
- aksel++;
- if (aksel>300)
- tuneAksel = 0.9;
-
+ if (awal)
+ {
+ tuneAksel = 0.525;
+ awal = false;
+ }
+ jarakYnow = getY();
+ if(jarakYnow-jarakYbefore<-5.0)
+ {
+ tuneAksel = tuneAksel+0.025;
+ jarakYbefore = jarakYnow;
+ }
+ if (tuneAksel>0.675) tuneAksel = 0.675;
+
+ motorDpn.brake(1);
+ motorBlk.brake(1);
motorR.speed(-tuneAksel);
motorL.speed(tuneAksel);
- motorDpn.brake(1);
- motorBlk.brake(1);
break;
}
- case (5) :
+ case (5) :
{
// Aktifkan motor atas
isLauncher = !isLauncher;
break;
}
- case (6) :
+ case (6) :
{
- // Target Pulse PID ++ Motor Depan
- target_rpm2 = target_rpm2+1.0;
- target_rpm = target_rpm+1.0;
+ // Target Pulse PID ++
+ target_rpm = target_rpm+100.0;
init_rpm();
break;
}
- case (7) :
+ case (7) :
{
- // Target Pulse PID -- Motor Depan
- target_rpm2 = target_rpm2-1.0;
- target_rpm = target_rpm-1.0;
+ // Target Pulse PID --
+ target_rpm = target_rpm-100.0;
init_rpm();
break;
}
- case (10) :
+ case (10) :
{
- // Pneumatik
+ // Pneumatik pendorong
if (ready)
{
- pneumatik = 0;
+ Pneumatik = 0;
previousMillis3 = millis();
flag_Pneu = true;
ready = false;
- //ReloadOn = !ReloadOn;
- //previousMillis6 = millis();
-
+ previousMillis6 = millis();
}
break;
}
- case (11) :
+ case (11) :
{
- // Power Screw Up
+ // Lifter Up
ReloadOn = !ReloadOn;
- isReload = false;
+ isDown = false;
break;
}
- case (31) :
+ case (31) :
{
// start
- target_rpm2 = 24;
- target_rpm = 24;
+ Elevator = 1;
+ target_rpm = 3200;
init_rpm();
break;
}
- case (32) :
+ case (32) :
{
// select
- target_rpm2 = 11;
- target_rpm = 11;
+ target_rpm = 1600;
init_rpm();
break;
}
- case (33) :
+ case (33) :
{
// R3
- target_rpm2 = 17;
- target_rpm = 17;
+ target_rpm = 2600;
init_rpm();
break;
}
- case (12) :
+ case (12) :
{
- // Power Screw Down
+ // Lifter Down
ReloadOn = !ReloadOn;
- isReload = true;
+ if (readySlideFromLeft)
+ {
+ sliderReady = false;
+ readySlideFromLeft = false;
+ }
+ isDown = true;
break;
}
case (34) :
{
- pneu_paku = 1;
- // Paku Bumi Naik
- wait_ms(50);
- PIVOT = 0.17;
+ // Pneumatik naikkan sudut
+ naik = !naik;
+ Elevator = 0;
+ break;
}
- case (35) :
+ default :
{
- // Paku Bumi Turun
- pneu_paku = 0;
- wait_ms(50);
- PIVOT = 0.8;
+ if (tuneAksel>0.4)
+ {
+ if (getX()>0.0)
+ {
+ jarakXnow = getX();
+ if(jarakXnow-jarakXbefore>4.5)
+ {
+ tuneAksel = tuneAksel-0.05;
+ jarakXbefore = jarakXnow;
+ }
+ }
+ if (getX()<0.0)
+ {
+ jarakXnow = getX();
+ if(jarakXnow-jarakXbefore<-4.5)
+ {
+ tuneAksel = tuneAksel-0.05;
+ jarakXbefore = jarakXnow;
+ }
+ }
+ }
+ else
+ {
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.brake(1);
+ motorL.brake(1);
+ setCenter();
+ jarakXbefore = 0;
+ jarakYbefore = 0;
+ }
+ if(PIVOT>0.2 || serong>0.2)
+ {
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.brake(1);
+ motorL.brake(1);
+ }
+ awal = true;
}
- default :
- {
- tuneAksel = 0.6;
- aksel = 0;
- motorDpn.brake(1);
- motorBlk.brake(1);
- motorR.brake(1);
- motorL.brake(1);
- }
} // End Switch
}
-void reloader()
-{
- if(ReloadOn){
- if(isReload){
- powerScrew.speed(pwmPowerDown);
- pc.printf("%.2f\n", jarak_ping);
- if((!limitBawah)||(!limitBawah1)){
- isReload = false;
- ReloadOn = false;
- }
- }
- else if(!limitTengah){
- isReload = true;
- }
- else if(!flag_Pneu){
- //pc.printf("%.2f\n", ping_pwm);
- ping_current_error = (double) (ping_target-jarak_ping);
-
- ping_sum_error += ping_current_error*ping_Ts;
- ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;
- //ping_sum_error = ping_sum_error+ping_current_error;
-
- pc.printf("%.2f\n", jarak_ping);
- powerScrew.speed(ping_pwm);
-
- ping_previous_error1 = ping_current_error;
- }
- if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
- ready = true;
- }else{
- ready = false;
- }
- }
- else{
- powerScrew.brake(1);
- }
-}
-
-
-void launcher()
+ void launcher()
{
if (isLauncher)
{
currentMillis = millis();
- currentMillis2 = millis();
-
+
// PID Launcher Belakang
if (currentMillis-previousMillis>=Ts)
{
- rpm = (float)encLauncherBlk.getPulses();
- current_error1 = target_rpm - rpm;
- a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
- b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
- c1 = kdA1/Ts;
- speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
- //init_speed();
+ rpm = (float)Lenc.getPulses();;
+ current_error = target_rpm - rpm;
+ a = kpA + kiA*Ts/2 + kdA/Ts;
+ b = -kpA + kiA*Ts/2 - 2*kdA/Ts;
+ c = kdA/Ts;
+ speed = previous_speed + a*current_error + b*previous_error_1 + c*previous_error_2;
if(speed > maxSpeed){
- launcherBlk.speed(maxSpeed);
+ Launcher.speed(maxSpeed);
}
else if ( speed < minSpeed){
- launcherBlk.speed(minSpeed);
+ Launcher.speed(minSpeed);
}
else {
- launcherBlk.speed(speed);
+ Launcher.speed(speed);
}
- previous_speed1 = speed;
- previous_error1_2 = previous_error1_1;
- previous_error1_1 = current_error1;
- encLauncherBlk.reset();
+ previous_speed = speed;
+ previous_error_2 = previous_error_1;
+ previous_error_1 = current_error;
previousMillis = currentMillis;
-
+ Lenc.reset();
}
- // PID Launcher Depan
- if (currentMillis2-previousMillis2>=Ts)
- {
- rpm2 = (float)encLauncherDpn.getPulses();
- current_error2 = target_rpm2 - rpm2;
- a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
- b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
- c2 = kdA2/Ts;
- speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
- //init_speed();
- if (speed2 > maxSpeed){
- launcherDpn.speed(maxSpeed);
- }
- else if ( speed2 < minSpeed){
- launcherDpn.speed(minSpeed);
- }
- else{
- launcherDpn.speed(speed2);
- }
- previous_speed2 = speed2;
- previous_error2_2 = previous_error2_1;
- previous_error2_1 = current_error2;
- encLauncherDpn.reset();
- previousMillis2 = currentMillis2;
- }
- //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
}
else
- {
- launcherDpn.brake(1);
- launcherBlk.brake(1);
-
- previous_error1_1 = 0;
- previous_error1_2 = 0;
- previous_error2_1 = 0;
- previous_error2_2 = 0;
- previous_speed1 = 0;
- previous_speed2 = 0;
- }
+ {
+ Launcher.brake(1);
+ current_error = 0;
+ previous_error_1 = 0;
+ previous_error_2 = 0;
+ previous_speed = 0;
+ }
}
-
-/*********************************************************/
-/* Main Function */
-/*********************************************************/
+// Main Program
int main (void)
{
// Set baud rate - 115200
@@ -790,61 +1021,86 @@
pc.baud(115200);
wait_ms(1000);
- // initializing encoder
- pneumatik =1;
-
+ // initializing pneumatik
+ Pneumatik = 1;
+ Elevator = 1;
wait_ms(500);
- //initializing PING
- pingAtas.Send();
-
- pc.printf("ready....");
+ while(init_slider)
+ {
+ swipper.speed(balik);
+ if (!(limitKiri))
+ {
+ init_slider = false;
+ swipper.brake(1);
+ }
+ }
+ while(init_lifter)
+ {
+ powerScrew.speed(-1.0);
+ if(!(limitBawah))
+ {
+ init_lifter = false;
+ powerScrew.brake(1);
+ }
+ }
+
startMillis();
- while(1)
- {
- // interupsi pembacaan PING setiap 30 ms
- if(millis() - previousMillis4 >= 10){ //30
- jarak_ping = (float)pingAtas.Read_cm();
-
- pingAtas.Send();
- previousMillis4 = millis();
- }
-
- // Interrupt Serial
- joystick.idle();
+ /* USER CODE END 2 */
+
+ /* Infinite loop */
+ /* USER CODE BEGIN WHILE */
+ //idle();
+ setCenter();
+ joystick.idle();
+ while (1)
+ {
if(joystick.readable())
{
- // Panggil fungsi pembacaan joystik
joystick.baca_data();
- // Panggil fungsi pengolahan data joystik
- joystick.olah_data();
- // Masuk ke case joystick
+ joystick.olah_data();
case_joy = case_joystick();
- //pc.printf("%d\n",case_joy);
+ if (naik)
+ {
+ swipper_dorong1 = lempar_naik1;
+ swipper_dorong2 = lempar2_naik;
+ }
+ else
+ {
+ swipper_dorong1 = lempar1;
+ swipper_dorong2 = lempar2;
+ }
aktuator();
launcher();
- reloader();
- if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
- pneumatik = 1;
+ lifterMove();
+
+ if ((millis()-previousMillis3 >= 500)&&(flag_Pneu))
+ {
+ Pneumatik = 1;
flag_Pneu = false;
- //if (millis()-previousMillis6 >= 100){
- // ReloadOn = !ReloadOn;
- //}
+ slideNowLeft = true;
+ slideNowMid = true;
+ ready = true;
+ delay = true;
+ previousMillis7 = millis(); //delay
+ }
+ if((millis()-previousMillis7>=200) && delay)
+ {
+ delay = false;
}
}
else
{
joystick.idle();
}
-
if(millis() - previousMillis5 >= 400)
- {
- display.write(0,((int) rpm2) / 10);
- display.write(1,((int)rpm2) % 10);
- display.write(2, (int)target_rpm2 / 10);
- display.write(3, (int)target_rpm2 % 10);
+ {
+ display.write(0,((int)rpm) / 1000);
+ display.write(1,((((int)rpm)%1000)/ 100));
+ display.write(2,((int)target_rpm) / 1000);
+ display.write(3,((((int)target_rpm)%1000)/ 100));
display.setColon(true);
-
+
previousMillis5 = millis();
}
}