KRAI 2017
tugas akhir
Dependencies: Motor mbed millis
Fork of
DagonFly__RoadToJapan_15Mei_Ultimate
by 
KRAI 2017
Diff: main.cpp
- Revision:
- 53:11515147caf9
- Parent:
- 52:a39e26b935a9
--- 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