![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
Diff: main.cpp
- Revision:
- 52:a39e26b935a9
- Parent:
- 51:df6391c3fa68
- Child:
- 53:11515147caf9
--- 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(); } }