KRTMI 2019
Kodingan KRAI 2017
Dependencies: mbed DigitDisplay PID Motor Ping millis
main.cpp
- Committer:
- SalbiFaza
- Date:
- 2019-02-24
- Revision:
- 0:dd4c20b9a83e
File content as of revision 0:dd4c20b9a83e:
/*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();
}
}
}