Kodingan KRAI 2017
Dependencies: mbed DigitDisplay PID Motor Ping millis
Diff: main.cpp
- Revision:
- 0:dd4c20b9a83e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,848 @@ +/*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)//kanan \ Omniwheel */ +/* | | */ +/* | 3 (kiri)//depan 4 (kanan) | Roda Kiri Kanan: */ +/* | | Omniwheel */ +/* \ 1 (Depan)//kiri / */ +/* \______________________/ 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 "Ping.h" +#include "DigitDisplay.h" + +/***********************************************/ +/* Konstanta dan Variabel */ +/***********************************************/ +#define PI 3.14159265 +#define D_ENCODER 10 // Diameter Roda Encoder +#define D_ROBOT 80 // Diameter Roda Robot + +// 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.1478, kdA1=0.9295, kiA1=0.0004226; +const double kpA2=0.1293, kdA2=1.0070, kiA2=0.0002986; +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; + +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; // ping lama +double ping_target = 14; // ping baru +double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0; +double ping_Kp = -0.3747, ping_Kd = -0.049, ping_Ts=10; +double ping_pwm, ping_previous_pwm = 0; + +// Variable Bawah +float PIVOT = 0.5; //0.17 // PWM Pivot Kanan, Pivot Kiri +float tuneDpn = 1.0; //1.0 // Tunning PWM motor Depan +float tuneBlk = 1.0; //1.0 // Tunning PWM motor belakang +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; + +/* variable tunning */ +float tunning_L; +//float tunning_R; +float tunning_Dpn; +float tunning_Blk; + +/* Deklarasi Variable Millis */ +static volatile uint32_t previousMillis = 0; // PID launcher +static volatile uint32_t currentMillis; +static volatile uint32_t previousMillis2 = 0; // PID launcher +static volatile uint32_t currentMillis2; +static volatile uint32_t previousMillis3 = 0; // Pneumatik +static volatile uint32_t previousMillis4 = 0; // Ping +static volatile uint32_t previousMillis5 = 0; // Display +static volatile uint32_t previousMillis6 = 0; // pneu + +/* Variabel Stick */ +//Logic untuk masuk aktuator +int case_joy; +bool isLauncher = false; +bool isReload = false; +bool ReloadOn = false; +bool flag_Pneu = false; +bool flag_paku = false; + +bool ready = false; + +/*****************************************************/ +/* Definisi Prosedur, Fungsi dan Setting Pinout */ +/*****************************************************/ + +/* 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 + +/* Inisialisasi Pin TX-RX Joystik dan PC */ +joysticknucleo joystick(PA_0,PA_1); +Serial pc(USBTX,USBRX,115200); + +/* Deklarasi Encoder Launcher */ +encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING); +encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING); + +/* 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_6, PC_14, PC_13); +Motor motorL (PB_9, PA_12, PA_6); +//Motor //////////MotorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5); + +/* 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 + +/* Deklarasi Penumatik Launcher */ +DigitalOut pneumatik(PA_4, PullUp); +DigitalOut pneu_paku(PC_2, PullUp); + +/*Dekalrasi Limit Switch */ +//DigitalIn infraAtas(PC_9, PullUp); +DigitalIn limitTengah(PA_9, PullUp); +DigitalIn limitBawah(PC_7, PullUp); +DigitalIn limitBawah1(PA_7, PullUp); + +/*deklarasi PING ultrasonic*/ +Ping pingAtas(PC_15); + +/*Deklarasi Display*/ +DigitDisplay display (PA_8, PC_8); + +/****************************************************/ +/* Deklarasi Fungsi dan Procedure */ +/****************************************************/ +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)) { + // 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){ + // Paku Bumi ON/OFF + caseJoystick = 34; + } + else + { + tuneAksel = 0.6; + aksel = 0; + } + + return(caseJoystick); +} + + +void init_rpm (){ + if (target_rpm>maxRPM-2){ + target_rpm = maxRPM-2; + } + if (target_rpm<minRPM){ + target_rpm = minRPM; + } + if (target_rpm2>maxRPM){ + target_rpm2 = maxRPM; + } + if (target_rpm2<minRPM+2){ + target_rpm2 = minRPM+2; + } +} + +void aktuator() +{ + switch (case_joy) { + case (1): + { + // Pivot Kanan + motorDpn.speed(-rasio*PIVOT-t_new); + motorBlk.speed(-rasio*PIVOT-t_new); + ////////////////MotorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-PIVOT); + break; + } + case (2): + { + // Pivot Kiri + motorDpn.speed(rasio*PIVOT+t_new); + motorBlk.speed(rasio*PIVOT+t_new); + ////////////////MotorR.speed(rasio*PIVOT+t_new); + motorL.speed(PIVOT); + break; + } + case (17): + { + // Kanan + Rotasi Kanan + motorDpn.speed(-PIVOT); + motorBlk.speed(-rasio*PIVOT-t_new); + ////////////////MotorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); + break; + } + case (18): + { + // Kanan + Rotasi Kiri + motorDpn.speed(PIVOT); + motorBlk.speed(rasio*PIVOT+t_new); + ////////////////MotorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); + break; + } + case (19): + { + // Kiri + Rotasi Kanan + motorDpn.speed(-PIVOT); + motorBlk.speed(-PIVOT); + ////////////////MotorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); + break; + } + case (20): + { + // Kiri + Rotasi Kiri + motorDpn.speed(PIVOT); + motorBlk.speed(PIVOT); + //MotorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); + break; + } + case (21): + { + // Maju + Rotasi Kanan + motorDpn.speed(-PIVOT); + motorBlk.speed(-PIVOT); + //MotorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); + break; + } + case (22): + { + // Maju + Rotasi Kiri + motorDpn.speed(PIVOT); + motorBlk.speed(PIVOT); + //////////////MotorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); + break; + } + case (23): + { + // Mundur + Rotasi Kanan + motorDpn.speed(-PIVOT); + motorBlk.speed(-PIVOT); + //////////////MotorR.speed(-rasio*PIVOT-t_new); + motorL.speed(-rasio*PIVOT-t_new); + break; + } + case (24): + { + // Mundur + Rotasi Kiri + motorDpn.speed(PIVOT); + motorBlk.speed(PIVOT); + //////////////MotorR.speed(rasio*PIVOT+t_new); + motorL.speed(rasio*PIVOT+t_new); + break; + } + case (25): + { + // Kanan + segitiga + isLauncher = !isLauncher; + break; + } + case (26): + { + // Kiri + segitiga + isLauncher = !isLauncher; + break; + } + case (27): + { + // Kanan + lingkaran + ReloadOn = !ReloadOn; + isReload = false; + break; + } + case (28): + { + // Kiri + lingkaran + ReloadOn = !ReloadOn; + isReload = false; + break; + } + case (29): + { + // Kanan + kotak + ReloadOn = !ReloadOn; + isReload = true; + break; + } + case (30): + { + // Kiri + kotak + ReloadOn = !ReloadOn; + isReload = true; + break; + } + case (13) : + { + // Serong kanan maju + motorDpn.speed(-serong); + motorL.speed(-serong-t_new); + motorBlk.speed(serong); + //////////////MotorR.speed(serong+t_new); + break; + } + case (14) : + { + // Serong kiri maju + motorDpn.speed(serong); + //////////////MotorR.speed(serong+t_new); + motorBlk.speed(-serong); + motorL.speed(-serong-t_new); + break; + } + case (15) : + { + // Serong kanan mundur + motorDpn.speed(-serong); + //////////////MotorR.speed(-serong-t_new); + motorBlk.speed(serong); + motorL.speed(serong+t_new); + break; + } + case (16) : + { + // Serong kiri mundur + motorDpn.speed(serong); + motorL.speed(serong+t_new); + motorBlk.speed(-serong); + //////////////MotorR.speed(-serong-t_new); + break; + } + case (3) : + { + // Kanan + aksel++; + if (aksel>300) + tuneAksel = 0.9; + + motorDpn.speed(-tuneAksel); + motorBlk.speed(tuneAksel); + //////////////MotorR.brake(1); + motorL.brake(1); + break; + } + case (4) : + { + // Kiri + aksel++; + if (aksel>300) + tuneAksel = 0.9; + + motorDpn.speed(tuneAksel); + motorBlk.speed(-tuneAksel); + //////////////MotorR.brake(1); + motorL.brake(1); + break; + } + case (8) : + { + // Maju + aksel++; + if (aksel>300) + tuneAksel = 0.9; + + ////////////////MotorR.speed(tuneAksel); + motorL.speed(1); + motorDpn.brake(-tuneAksel); + motorBlk.speed(tuneAksel); + break; + } + case (9) : + { + // Mundur + aksel++; + if (aksel>300) + tuneAksel = 0.9; + + ////////////////MotorR.speed(-tuneAksel); + motorL.speed(1); + motorDpn.brake(tuneAksel); + motorBlk.brake(-tuneAksel); + break; + } + case (5) : + { + // Aktifkan motor atas + isLauncher = !isLauncher; + break; + } + case (6) : + { + // Target Pulse PID ++ Motor Depan + target_rpm2 = target_rpm2+1.0; + target_rpm = target_rpm+1.0; + init_rpm(); + break; + } + case (7) : + { + // Target Pulse PID -- Motor Depan + target_rpm2 = target_rpm2-1.0; + target_rpm = target_rpm-1.0; + init_rpm(); + break; + } + case (10) : + { + // Pneumatik + if (ready) + { + pneumatik = 0; + previousMillis3 = millis(); + flag_Pneu = true; + ready = false; + previousMillis6 = millis(); + + } + break; + } + case (11) : + { + // Power Screw Up + ReloadOn = !ReloadOn; + isReload = false; + break; + } + case (31) : + { + // start + target_rpm2 = 24; + target_rpm = 24; + init_rpm(); + break; + } + case (32) : + { + // select + target_rpm2 = 11; + target_rpm = 11; + init_rpm(); + break; + } + case (33) : + { + // R3 + target_rpm2 = 17; + target_rpm = 17; + init_rpm(); + break; + } + case (12) : + { + // Power Screw Down + ReloadOn = !ReloadOn; + isReload = true; + break; + } + case (34) :{ + pneu_paku = !pneu_paku; + wait_ms(50); + if (pneu_paku == 1){ + PIVOT = 0.17; + }else{ + PIVOT = 0.8; + } + } + 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); + if (millis()-previousMillis6>700) + { + 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; + + if (ping_pwm>1) ping_pwm=1; + if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5; + if (ping_pwm<-0.049 && ping_pwm>-0.3) ping_pwm = -0.3; + if (ping_pwm<-1) ping_pwm=-1; + + 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() +{ + 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(); + if(speed > maxSpeed){ + launcherBlk.speed(maxSpeed); + } + else if ( speed < minSpeed){ + launcherBlk.speed(minSpeed); + } + else { + launcherBlk.speed(speed); + } + previous_speed1 = speed; + previous_error1_2 = previous_error1_1; + previous_error1_1 = current_error1; + encLauncherBlk.reset(); + previousMillis = currentMillis; + + } + // 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; + } +} +*/ +/*********************************************************/ +/* Main Function */ +/*********************************************************/ + +int main (void) +{ + // Set baud rate - 115200 + joystick.setup(); + pc.baud(115200); + wait_ms(1000); + + // initializing encoder + pneumatik =1; + + wait_ms(500); + + //initializing PING + pingAtas.Send(); + + pc.printf("ready...."); + 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(); + if(joystick.readable()) + { + // Panggil fungsi pembacaan joystik + joystick.baca_data(); + // Panggil fungsi pengolahan data joystik + joystick.olah_data(); + // Masuk ke case joystick + case_joy = case_joystick(); + //pc.printf("%d\n",case_joy); + aktuator(); + //launcher(); + //reloader(); + if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){ + pneumatik = 1; + flag_Pneu = false; + //wait_ms(1000); + } + } + 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.setColon(true); + + previousMillis5 = millis(); + } + } +} \ No newline at end of file