![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
tugas akhir
Dependencies: Motor mbed millis
Fork of DagonFly__RoadToJapan_15Mei_Ultimate by
Revision 53:11515147caf9, committed 2018-05-08
- Comitter:
- Najib_irvani
- Date:
- Tue May 08 14:59:08 2018 +0000
- Parent:
- 52:a39e26b935a9
- Commit message:
- tugas akhir
Changed in this revision
--- a/DigitDisplay.lib Sun Jul 02 01:37:31 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/seeed/code/DigitDisplay/#d3173c8bfd48
--- a/JoystickPS3.h Sun Jul 02 01:37:31 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,231 +0,0 @@ -#ifndef MBED_H -#include "mbed.h" -#endif - -#ifndef JoystickPS3__serialDEFAULT_BAUD -#define JoystickPS3__serialDEFAULT_BAUD 115200 -#endif - -Serial debug(USBTX,USBRX); - -namespace JoystickPS3 { - -class joysticknucleo { -public: - joysticknucleo(PinName tx, PinName rx) : _serial(tx, rx) - { - - } - -// Deklarasi variabel tombol analog - unsigned char LX, LY, RX, RY, R2, L2; - - unsigned char button; - unsigned char RL; - unsigned char button_click; - unsigned char RL_click; - - void setup(){ - _serial.baud(JoystickPS3__serialDEFAULT_BAUD); - debug.baud(9600); - } - - /*********************************************************************************************/ - /** **/ - /** FUNGSI PEMBACAAN DATA **/ - /** - Data yang diterima dari Serial Arduino berbentuk 8-bit **/ - /** - Data yang diterima diolah menjadi boolean / 1-bit untuk data tombol button dan RL **/ - /** karena data yang digunakan adalah 1-bit (true/false) **/ - /** - Untuk analog data yang diterima tidak diolah karena rentang data yang dikirimkan **/ - /** memiliki rentang 0-255 / 8-bit, dan data yang akan digunakan adalah data 8-bit **/ - /** **/ - /** |------|-------|-------|------|-------|--------|-----------|----------| **/ - /** Bit Ke | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ - /** |------|-------|-------|------|-------|--------|-----------|----------| **/ - /** Data | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga | **/ - /** |------|-------|-------|------|-------|--------|-----------|----------| **/ - /** **/ - /** - Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam **/ - /** variabel "RL" **/ - /** - Urutan data pada variabel "RL" dan "RL_click" adalah **/ - /** sebagai berikut **/ - /** **/ - /** |----|--------|-------|----|----|----|----| **/ - /** Bit Ke | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ - /** |----|--------|-------|----|----|----|----| **/ - /** Data | PS | SELECT | START | L3 | L1 | R3 | R1 | **/ - /** |----|--------|-------|----|----|----|----| **/ - /** **/ - /*********************************************************************************************/ - - void olah_data() - { - // Pengolahan data dari data "button" - segitiga = (bool)((button >> 0) & 0x1); - lingkaran = (bool)((button >> 1) & 0x1); - silang = (bool)((button >> 2) & 0x1); - kotak = (bool)((button >> 3) & 0x1); - atas = (bool)((button >> 4) & 0x1); - kanan = (bool)((button >> 5) & 0x1); - bawah = (bool)((button >> 6) & 0x1); - kiri = (bool)((button >> 7) & 0x1); - - // Pengolahan data dari data "RL" - R1 = (bool)((RL >> 0) & 0x1); - R3 = (bool)((RL >> 1) & 0x1); - L1 = (bool)((RL >> 2) & 0x1); - L3 = (bool)((RL >> 3) & 0x1); - START = (bool)((RL >> 4) & 0x1); - SELECT = (bool)((RL >> 5) & 0x1); - PS = (bool)((RL >> 6) & 0x1); - - // R2 click dan L2 click - if (R2 > 100) { - if ( R2sebelum) { R2_click = false; - } else { R2_click = true;} - R2sebelum = true; - }else { - R2sebelum = false; - R2_click = false; - } - if (L2 > 100) { - if ( L2sebelum) { L2_click = false; - } else { L2_click = true;} - L2sebelum = true; - }else { L2sebelum = false; - L2_click = false; - } - - if (LY < 100 && LY > 50) { - if ( LYpSebelum) { LYp_click = false; - } else { LYp_click = true;} - LYpSebelum = true; - }else { LYpSebelum = false; - LYp_click = false; - } - if (LY > 150 && LY < 200) { - if ( LYnSebelum) { LYn_click = false; - } else { LYn_click = true;} - LYnSebelum = true; - }else { LYnSebelum = false; - LYn_click = false; - } - segitiga_click = (bool)((button_click >> 0) & 0x1); - lingkaran_click = (bool)((button_click >> 1) & 0x1); - silang_click = (bool)((button_click >> 2) & 0x1); - kotak_click = (bool)((button_click >> 3) & 0x1); - atas_click = (bool)((button_click >> 4) & 0x1); - kanan_click = (bool)((button_click >> 5) & 0x1); - bawah_click = (bool)((button_click >> 6) & 0x1); - kiri_click = (bool)((button_click >> 7) & 0x1); - - // Pengolahan data dari data "RL" - R1_click = (bool)((RL_click >> 0) & 0x1); - R3_click = (bool)((RL_click >> 1) & 0x1); - L1_click = (bool)((RL_click >> 2) & 0x1); - L3_click = (bool)((RL_click >> 3) & 0x1); - START_click = (bool)((RL_click >> 4) & 0x1); - SELECT_click = (bool)((RL_click >> 5) & 0x1); - PS_click = (bool)((RL_click >> 6) & 0x1); - } - - /*********************************************************************************************/ - /** **/ - /** FUNGSI IDLE **/ - /** - Fungsi dijalankan saat Arduino mengirimkan data yang merupakan **/ - /** kondisi PS3 Disconnected **/ - /** - Fungsi membuat semua data joystik bernilai 0 **/ - /** **/ - /*********************************************************************************************/ - - void idle(){ - // Set 0 - button = 0; - RL = 0; - button_click = 0; - RL_click = 0; - R2_click = 0; - L2_click = 0; - L3_click = 0; - LYp_click = 0; - LYn_click = 0; - R2 = 0; - L2 = 0; - RX = 0; - RY = 0; - LX = 0; - LY = 0; - - } - - /*********************************************************************************************/ - /** **/ - /** FUNGSI PEMBACAAN DATA **/ - /** - Fungsi pembacaan data yang dikirim dari arduino **/ - /** - Data yang dikirim dari arduino merupakan paket data dengan format pengiriman **/ - /** **/ - /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ - /** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY | **/ - /** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ - /** **/ - /** |------|------| **/ - /** | 0x88 | 0x09 | **/ - /** |------|------| **/ - /** **/ - /** - Jika urutan data yang diterima seperti tabel diatas, maka data tersebut akan **/ - /** diolah untuk input ke aktuator **/ - /** **/ - /*********************************************************************************************/ - - void baca_data() - { - // Interrupt Serial - if(_serial.readable()&&(_serial.getc()==0x88)) { - // Pembacaan data dilakukan jika data awal yang diterima adalah 0x88 kemudian 0x08 - if(_serial.getc()==0x08){ - // Proses Pembacaan Data - button = _serial.getc(); - RL = _serial.getc(); - button_click = _serial.getc(); - RL_click = _serial.getc(); - R2 = _serial.getc(); - L2 = _serial.getc(); - RX = _serial.getc(); - RY = _serial.getc(); - LX = _serial.getc(); - LY = _serial.getc(); - } else if(_serial.getc()==0x09) { - // PS3 Disconnected - idle(); - } else { - idle(); } - // Indikator - Print data pada monitor PC - //debug.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",button, RL, button_click, RL_click, R2, L2, RX, RY, LX, LY); - } - } - - - int readable(){ - return _serial.readable(); - } - -public: - // Deklarasi variabel tombol joystik - bool kiri, kanan, atas, bawah; - bool segitiga, lingkaran, kotak, silang; - bool L1, R1, L3, R3, START, SELECT, PS; - - bool kiri_click, kanan_click, atas_click, bawah_click; - bool segitiga_click, lingkaran_click, kotak_click, silang_click; - bool L1_click, R1_click, L3_click, R3_click, R2_click, L2_click, LYp_click, LYn_click; - bool R2sebelum, L2sebelum, L3sebelum, LYpSebelum, LYnSebelum; - bool START_click, SELECT_click, PS_click; - -protected: - virtual int _getc(){return _serial.getc();} - Serial _serial; -}; - -}; - -using namespace JoystickPS3; \ No newline at end of file
--- a/PID.lib Sun Jul 02 01:37:31 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- a/PS3Arduino.txt Sun Jul 02 01:37:31 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,299 +0,0 @@ -/*********************************************************************************************/ -/** **/ -/** PROGRAM KOMUNIKASI DATA JOYSTIK PS3 **/ -/** **/ -/** Joystik PS3 -> Arduino -> STM32Nucleo **/ -/** **/ -/** Fanny Achmad Hindrarta **/ -/** EL'12 - 13212076 **/ -/** **/ -/** Last Update : 01 Februar1 2015, 20.30 **/ -/*********************************************************************************************/ - -#include <PS3BT.h> -#include <usbhub.h> -//#include <PS3USB.h> -#include "Arduino.h" -// Satisfy IDE, which only needs to see the include statment in the ino. -#ifdef dobogusinclude -#include <spi4teensy3.h> -#endif - - -USB Usb; - -// Bluetooth -// You can create the instance of the class in two ways -BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so -//You can create the instance of the class in two ways -PS3BT PS3(&Btd); // This will just create the instance -//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch -/* -// USB -// You can create the instance of the class in two ways -PS3USB PS3(&Usb); // This will just create the instance -//PS3USB PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch -*/ - -boolean printAngle; -uint8_t state = 0; - -// Deklarasi tombol -boolean kiri_click=0, kanan_click=0, atas_click=0, bawah_click=0; -boolean segitiga_click=0, lingkaran_click=0, kotak_click=0, silang_click=0; -boolean L1_click=0, R1_click=0, L3_click=0, R3_click=0; -boolean START_click=0, SELECT_click=0, PS_click=0; - -// Deklarasi variabel tombol analog -unsigned char LX, LY, RX, RY, aL2, aR2, aL3; - -// Deklarasi varibel data yang dikirim -unsigned char button; -unsigned char RL; -unsigned int button_click; -unsigned int RL_click; - -void setup_joystik() -{ - while (!Serial) // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection - { - Serial.write(0x88); - Serial.write(0x09); - } - if (Usb.Init() == -1) { - //Serial.print(F("\r\nOSC did not start")); - while (1) //halt - { - Serial.write(0x88); - Serial.write(0x09); - } - } - //Serial.print(F("\r\nPS3 USB Library Started")); -} - -/*********************************************************************************************/ -/** **/ -/** FUNGSI PENGIRIMAN DATA **/ -/** - Data yang akan dikirim adalah paket data 8-bit dengan urutan sebagai berikut **/ -/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ -/** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY | **/ -/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----| **/ -/** **/ -/*********************************************************************************************/ - -void kirimdatajoystik() -{ - - Serial.write(0x88); - Serial.write(0x08); - Serial.write(button); - Serial.write(RL); - Serial.write(button_click); - Serial.write(RL_click); - Serial.write(aR2); - Serial.write(aL2); - Serial.write(RX); - Serial.write(RY); - Serial.write(LX); - Serial.write(LY); - -// Debug - -/* - Serial.print(millis()); - Serial.print("\t"); - Serial.print(button); - Serial.print("\t"); - Serial.print(RL); - Serial.print("\t"); - Serial.print(button_click); - Serial.print("\t"); - Serial.print(RL_click); - Serial.print("\t"); - Serial.print(aR2); - Serial.print("\t"); - Serial.print(aL2); - Serial.print("\t"); - Serial.print(RX); - Serial.print("\t"); - Serial.print(RY); - Serial.print("\t"); - Serial.print(LX); - Serial.print("\t"); - Serial.println(LY); -*/ -} - -/*****************************************************************************************/ -/** SETUP REGISTER dan INISIALISASI **/ -/** - Setup Joystik **/ -/** - Baud Rate Serial 115200, 8-bit, 1 stop, 0 parity **/ -/*****************************************************************************************/ - -void setup() { - Serial.begin(115200); - - setup_joystik(); -} - - -/*****************************************************************************************/ -/** **/ -/** FUNGSI PEMBACAAN DATA JOYSTIK **/ -/** - Data dari Joystik dikirim melalui Bluetooth. **/ -/** Lihat Library "PS3BT.h" **/ -/** - Data tombol dari joystik adalah data 1-bit. Pengiriman data melalui USART **/ -/** adalah 8-bit. Agar pengiriman data efisien, maka data 1-bit digabungkan **/ -/** agar terbentuk data 8-bit **/ -/** - Penggabungan data segitiga, longkaran, silang, kotak, arah atas, arah kanan, **/ -/** arah bawah, arah kiri disimpan dalam variabel "button" **/ -/** - Urutan data pada variabel "button" dan "button_click" **/ -/** adalah sebagai berikut **/ -/** **/ -/** |------|-------|-------|------|-------|--------|-----------|----------| **/ -/** Bit Ke | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ -/** |------|-------|-------|------|-------|--------|-----------|----------| **/ -/** Data | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga | **/ -/** |------|-------|-------|------|-------|--------|-----------|----------| **/ -/** **/ -/** - Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam **/ -/** variabel "RL" **/ -/** - Urutan data pada variabel "RL" dan "RL_click" adalah **/ -/** sebagai berikut **/ -/** **/ -/** |----|--------|-------|----|----|----|----| **/ -/** Bit Ke | 6 | 5 | 4 | 3 | 2 | 1 | 0 | **/ -/** |----|--------|-------|----|----|----|----| **/ -/** Data | PS | SELECT | START | L3 | L1 | R3 | R1 | **/ -/** |----|--------|-------|----|----|----|----| **/ -/** **/ -/*****************************************************************************************/ - -void loop() { - button = 0; - RL = 0; - button_click=0; - RL_click=0; - Usb.Task(); - - // Pembacaan data joystik dilakukan jika PS3 tersambung ke Arduino - if (PS3.PS3Connected || PS3.PS3NavigationConnected) { - // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri - // Data bernilai '1' jika tombol ditekan - if(PS3.getButtonPress(TRIANGLE)){ - button = button + (0x1 << 0); - } - if(PS3.getButtonPress(CIRCLE)){ - button = button + (0x1 << 1); - } - if(PS3.getButtonPress(CROSS)){ - button = button + (0x1 << 2); - } - if(PS3.getButtonPress(SQUARE)){ - button = button + (0x1 << 3); - } - if(PS3.getButtonPress(UP)){ - button = button + (0x1 << 4); - } - if(PS3.getButtonPress(RIGHT)){ - button = button + (0x1 << 5); - } - if(PS3.getButtonPress(DOWN)){ - button = button + (0x1 << 6); - } - if(PS3.getButtonPress(LEFT)){ - button = button + (0x1 << 7); - } - - // Pembacaan dan penggabungan data R1, R3, L1, L3, START, SELECT, dan PS - // Data bernilai '1' jika tombol ditekan - if(PS3.getButtonPress(R1)){ - RL = RL + (0x1 << 0); - } - if(PS3.getButtonPress(R3)){ - RL = RL + (0x1 << 1); - } - if(PS3.getButtonPress(L1)){ - RL = RL + (0x1 << 2); - } - if(PS3.getButtonPress(L3)){ - RL = RL + (0x1 << 3); - } - if(PS3.getButtonPress(START)){ - RL = RL + (0x1 << 4); - } - if(PS3.getButtonPress(SELECT)){ - RL = RL + (0x1 << 5); - } - if(PS3.getButtonPress(PS)){ - RL = RL + (0x1 << 6); - } - - // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri - // Data bernilai '1' hanya saat tombol pertama kali ditekan - if(PS3.getButtonClick(TRIANGLE)){ - button_click = button_click + (0x1 << 0); - } - if(PS3.getButtonClick(CIRCLE)){ - button_click = button_click + (0x1 << 1); - } - if(PS3.getButtonClick(CROSS)){ - button_click = button_click + (0x1 << 2); - } - if(PS3.getButtonClick(SQUARE)){ - button_click = button_click + (0x1 << 3); - } - if(PS3.getButtonClick(UP)){ - button_click = button_click + (0x1 << 4); - } - if(PS3.getButtonClick(RIGHT)){ - button_click = button_click + (0x1 << 5); - } - if(PS3.getButtonClick(DOWN)){ - button_click = button_click + (0x1 << 6); - } - if(PS3.getButtonClick(LEFT)){ - button_click = button_click + (0x1 << 7); - } - - if(PS3.getButtonClick(R1)){ - RL_click = RL_click + (0x1 << 0); - } - if(PS3.getButtonClick(R3)){ - RL_click = RL_click + (0x1 << 1); - } - if(PS3.getButtonClick(L1)){ - RL_click = RL_click + (0x1 << 2); - } - if(PS3.getButtonClick(L3)){ - RL_click = RL_click + (0x1 << 3); - } - if(PS3.getButtonClick(START)){ - RL_click = RL_click + (0x1 << 4); - } - if(PS3.getButtonClick(SELECT)){ - RL_click = RL_click + (0x1 << 5); - } - if(PS3.getButtonClick(PS)){ - RL_click = RL_click + (0x1 << 6); - } - - // Tombol Analog - LX = PS3.getAnalogHat(LeftHatX); - LY = PS3.getAnalogHat(LeftHatY); - RX = PS3.getAnalogHat(RightHatX); - RY = PS3.getAnalogHat(RightHatY); - - aL2 = PS3.getAnalogButton(L2); - aR2 = PS3.getAnalogButton(R2); - - kirimdatajoystik(); - } else { - // PS3 Disconnected - Serial.write(0x88); - Serial.write(0x09); - } - delay(3); -} - - \ No newline at end of file
--- 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