KRAI 2017
Fix pisan inimah plis jangan revisi ultimate mantep
Dependencies: DigitDisplay Motor PID mbed millis
Fork of
DagonFly__RoadToJapan_15Mei
by 
KRAI 2017
main.cpp
- Committer:
- be_bryan
- Date:
- 2017-07-02
- Revision:
- 52:a39e26b935a9
- Parent:
- 51:df6391c3fa68
File content as of revision 52:a39e26b935a9:
/*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 "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;
/**********************************************************/
/**********************************************************/
/* 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;}
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);
}
}
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();
}
}
}