KRAI 2017
tugas akhir
Dependencies: Motor mbed millis
Fork of
DagonFly__RoadToJapan_15Mei_Ultimate
by 
KRAI 2017
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