![](/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:
- 53:11515147caf9
- Parent:
- 52:a39e26b935a9
diff -r a39e26b935a9 -r 11515147caf9 main.cpp --- a/main.cpp Sun Jul 02 01:37:31 2017 +0000 +++ b/main.cpp Tue May 08 14:59:08 2018 +0000 @@ -1,1107 +1,24 @@ -/*tuning motor baru untuk konstanta pid baru */ -/****************************************************************************/ -/* PROGRAM UNTUK PID CLOSED LOOP */ -/* */ -/* Last Update : 16 April 2017 */ -/* */ -/* - Digunakan encoder autonics */ -/* - Konfigurasi Motor dan Encoder sbb : */ -/* ______________________ */ -/* / \ Rode Depan Belakang: */ -/* / 2 (Belakang) \ Omniwheel */ -/* | | */ -/* | 3 (kiri) 4 (kanan) | Roda Kiri Kanan: */ -/* | | Omniwheel */ -/* \ 1 (Depan) / */ -/* \______________________/ Putaran berlawanan arah */ -/* jarum jam positif */ -/* SETTINGS (WAJIB!) : */ -/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */ -/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */ -/* */ -/****************************************************************************/ -/* */ -/* Joystick */ -/* Kanan => */ -/* Kiri => */ -/* */ -/* Tombol silang => Pneumatik aktif */ -/* Tombol segitiga => Aktif motor Launcher */ -/* Tombol lingkaran => Reloader naik */ -/* Tombol kotak => Reloader turun */ -/* Tombol L1 => Pivot Kiri */ -/* Tombol R1 => Pivot Kanan */ -/* Tombol L2 => Kurang PWM Motor Launcher */ -/* Tombol R2 => Tambah PWM Motor Launcher */ -/* */ -/* Bismillahirahmanirrahim */ -/* Jagalah Kebersihan Kodingan */ -/****************************************************************************/ - #include "mbed.h" -#include "JoystickPS3.h" +#include "encoderKRAI.h" #include "Motor.h" -#include "encoderKRAI.h" #include "millis.h" -#include "DigitDisplay.h" - -/***********************************************/ -/* Konstanta dan Variabel */ -/***********************************************/ -#define PI 3.14159265 -#define D_ENCODER 10 // Diameter Roda Encoder -#define D_ROBOT1 54 // Diameter Roda Robot dari kiri ke kanan -#define D_ROBOT2 79 // Diameter Roda Robot dari depan ke belakang - -/**********************************************************/ -/* 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; -double target_rpm = 2600; -const float maxRPM = 4000, minRPM = 0.0; -/**********************************************************/ - -/**********************************************************/ -/* BASE */ -float PIVOT = 0.3; // PWM Pivot Kanan, Pivot Kiri -float tuneAksel = 0.6; -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; -/**********************************************************/ - -/**********************************************************/ -/* RELOADER */ -bool isReload = false; -bool ReloadOn = false; -bool flag_Pneu = false; -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; -/**********************************************************/ + +encoderKRAI encLauncherDpn( D2, D3, 28, encoderKRAI::X4_ENCODING); +//encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); +Serial pc(USBTX,USBRX); -/**********************************************************/ -/* 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 -/**********************************************************/ -/**********************************************************/ -/* JOYSTICK */ -bool flag_LXmax = true, flag_LXmin = true, flag_LYmax = true, flag_LYmin = true; -int case_joy; -//int debug = 0; -bool awal = true; -/**********************************************************/ - -/**********************************************************/ -/* 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); -/**********************************************************/ - -/**********************************************************/ -/* 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); - -/**********************************************************/ - -/**********************************************************/ -/* Display 7 Segmen */ -DigitDisplay display (PC_12, PB_10); -/**********************************************************/ - -/**********************************************************/ -/* 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); -/**********************************************************/ - -/**********************************************************/ -/* Pneumatik */ -DigitalOut Pneumatik(PA_14, PullUp); -DigitalOut Elevator(PA_15, PullUp); -/****************************************************/ - -/**********************************************************/ -/* 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() -{ - int caseJoystick; - 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)) { - // Pivot Kiri - caseJoystick = 2; - } - else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) { - // tambah rpm dengan nilai tertentu - caseJoystick = 31; - } - else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) { - // kurangi rpm dengan nilai tertentu - caseJoystick = 32; - } - else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) { - // kurangi rpm dengan nilai tertentu - caseJoystick = 33; - } - 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)) { - // Kanan + Rotasi kiri - caseJoystick = 18; - } - 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)) { - // Kanan + Rotasi kiri - caseJoystick = 20; - } - 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)) { - // Maju + Rotasi kiri - caseJoystick = 22; - } - 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)) { - // Mundur + Rotasi kiri - caseJoystick = 24; - } - 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)) { - // Kiri + segitiga - caseJoystick = 26; - } - 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)) { - // Kiri + lingkaran - caseJoystick = 28; - } - 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)) { - // Kiri + kotak - caseJoystick = 30; - } - else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { - // Serong kanan maju - caseJoystick = 13; - } - else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) { - // Serong kiri maju - caseJoystick = 14; - } - else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) { - // Serong kanan mundur - caseJoystick = 15; - } - 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)) { - // Kanan - caseJoystick = 3; - } - 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)) { - // Atas -- Maju - caseJoystick = 8; - } - else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) { - // Bawah -- Mundur - caseJoystick = 9; - } - else if (joystick.segitiga_click){ - // Motor Launcher - caseJoystick = 5; - } - else if (joystick.R2_click){ - // Target Pulse PID ++ Motor Depan - caseJoystick = 6; - } - else if (joystick.L2_click){ - // Target Pulse PID -- Motor - caseJoystick = 7; - } - else if (joystick.silang_click){ - // Pnemuatik ON - caseJoystick = 10; - } - else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) { - // Power Screw Up - caseJoystick = 11; - } - else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) { - // Power Screw Down - caseJoystick = 12; - } - else if (joystick.L3_click){ - // elevasi ON/OFF - caseJoystick = 34; - } - else - { - //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 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;} +int main() { + +pc.baud(115200); +//wait_ms(10000); +startMillis(); +pc.printf("Hello World\n"); + + while(1) { - 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; - } -} - -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; - } - } - 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); + pc.printf("%d\n", encLauncherDpn.getPulses()); + wait_ms(100); } } - -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): - { - // Pivot Kanan - motorDpn.speed(-PIVOT); - motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); - break; - } - case (2): - { - // Pivot Kiri - motorDpn.speed(PIVOT); - motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); - break; - } - case (17): - { - // Kanan + Rotasi Kanan - motorDpn.speed(-PIVOT); - motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); - break; - } - case (18): - { - // Kanan + Rotasi Kiri - motorDpn.speed(PIVOT); - motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); - break; - } - case (19): - { - // Kiri + Rotasi Kanan - motorDpn.speed(-PIVOT); - motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); - break; - } - case (20): - { - // Kiri + Rotasi Kiri - motorDpn.speed(PIVOT); - motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); - break; - } - case (21): - { - // Maju + Rotasi Kanan - motorDpn.speed(-PIVOT); - motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); - break; - } - case (22): - { - // Maju + Rotasi Kiri - motorDpn.speed(PIVOT); - motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); - break; - } - case (23): - { - // Mundur + Rotasi Kanan - motorDpn.speed(-PIVOT); - motorBlk.speed(-PIVOT); - motorR.speed(-rasio*PIVOT); - motorL.speed(-rasio*PIVOT); - break; - } - case (24): - { - // Mundur + Rotasi Kiri - motorDpn.speed(PIVOT); - motorBlk.speed(PIVOT); - motorR.speed(rasio*PIVOT); - motorL.speed(rasio*PIVOT); - break; - } - case (25): - { - // Kanan + segitiga - isLauncher = !isLauncher; - break; - } - case (26): - { - // Kiri + segitiga - isLauncher = !isLauncher; - break; - } - case (27): - { - // Kanan + lingkaran ----- Lifter Up - ReloadOn = !ReloadOn; - isDown = false; - break; - } - case (28): - { - // Kiri + lingkaran ----- Lifter Up - ReloadOn = !ReloadOn; - isDown = false; - break; - } - case (29): - { - // Kanan + kotak ----- Lifter Down - ReloadOn = !ReloadOn; - isDown = true; - break; - } - case (30): - { - // Kiri + kotak ----- Lifter Down - ReloadOn = !ReloadOn; - isDown = true; - break; - } - case (13) : - { - // Serong kanan maju - awal = true; - motorDpn.speed(-serong); - motorBlk.speed(serong); - motorR.speed(serong); - motorL.speed(-serong); - break; - } - case (14) : - { - // Serong kiri maju - awal = true; - motorDpn.speed(serong); - motorBlk.speed(-serong); - motorR.speed(serong); - motorL.speed(-serong); - break; - } - case (15) : - { - // Serong kanan mundur - awal = true; - motorDpn.speed(-serong); - motorBlk.speed(serong); - motorR.speed(-serong); - motorL.speed(serong); - break; - } - case (16) : - { - // Serong kiri mundur - awal = true; - motorDpn.speed(serong); - motorBlk.speed(-serong); - motorR.speed(-serong); - motorL.speed(serong); - break; - } - case (3) : - { - // Kanan - 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) : - { - // Kiri - 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) : - { - // Maju - 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); - break; - } - case (9) : - { - // Mundur - 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); - break; - } - case (5) : - { - // Aktifkan motor atas - isLauncher = !isLauncher; - break; - } - case (6) : - { - // Target Pulse PID ++ - target_rpm = target_rpm+100.0; - init_rpm(); - break; - } - case (7) : - { - // Target Pulse PID -- - target_rpm = target_rpm-100.0; - init_rpm(); - break; - } - case (10) : - { - // Pneumatik pendorong - if (ready) - { - Pneumatik = 0; - previousMillis3 = millis(); - flag_Pneu = true; - ready = false; - previousMillis6 = millis(); - } - break; - } - case (11) : - { - // Lifter Up - ReloadOn = !ReloadOn; - isDown = false; - break; - } - case (31) : - { - // start - Elevator = 1; - target_rpm = 3200; - init_rpm(); - break; - } - case (32) : - { - // select - target_rpm = 1600; - init_rpm(); - break; - } - case (33) : - { - // R3 - target_rpm = 2600; - init_rpm(); - break; - } - case (12) : - { - // Lifter Down - ReloadOn = !ReloadOn; - if (readySlideFromLeft) - { - sliderReady = false; - readySlideFromLeft = false; - } - isDown = true; - break; - } - case (34) : - { - // Pneumatik naikkan sudut - naik = !naik; - Elevator = 0; - break; - } - default : - { - 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; - } - } // End Switch - } - - void launcher() -{ - if (isLauncher) - { - currentMillis = millis(); - - // PID Launcher Belakang - if (currentMillis-previousMillis>=Ts) - { - 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){ - Launcher.speed(maxSpeed); - } - else if ( speed < minSpeed){ - Launcher.speed(minSpeed); - } - else { - Launcher.speed(speed); - } - previous_speed = speed; - previous_error_2 = previous_error_1; - previous_error_1 = current_error; - previousMillis = currentMillis; - Lenc.reset(); - } - } - else - { - Launcher.brake(1); - current_error = 0; - previous_error_1 = 0; - previous_error_2 = 0; - previous_speed = 0; - } -} - -// Main Program -int main (void) -{ - // Set baud rate - 115200 - joystick.setup(); - pc.baud(115200); - wait_ms(1000); - - // initializing pneumatik - Pneumatik = 1; - Elevator = 1; - wait_ms(500); - - 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(); - /* USER CODE END 2 */ - - /* Infinite loop */ - /* USER CODE BEGIN WHILE */ - //idle(); - setCenter(); - joystick.idle(); - while (1) - { - if(joystick.readable()) - { - joystick.baca_data(); - joystick.olah_data(); - case_joy = case_joystick(); - if (naik) - { - swipper_dorong1 = lempar_naik1; - swipper_dorong2 = lempar2_naik; - } - else - { - swipper_dorong1 = lempar1; - swipper_dorong2 = lempar2; - } - aktuator(); - launcher(); - lifterMove(); - - if ((millis()-previousMillis3 >= 500)&&(flag_Pneu)) - { - Pneumatik = 1; - flag_Pneu = false; - 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)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(); - } - } -} \ No newline at end of file