KRTMI 2019
Kodingan KRAI 2017
Dependencies: mbed DigitDisplay PID Motor Ping millis
Revision 0:dd4c20b9a83e, committed 2019-02-24
- Comitter:
- SalbiFaza
- Date:
- Sun Feb 24 10:39:24 2019 +0000
- Commit message:
- bismillah
Changed in this revision
diff -r 000000000000 -r dd4c20b9a83e DigitDisplay.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DigitDisplay.lib Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Seeed/code/DigitDisplay/#d3173c8bfd48
diff -r 000000000000 -r dd4c20b9a83e JoystickPS3.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/JoystickPS3.h Sun Feb 24 10:39:24 2019 +0000
@@ -0,0 +1,214 @@
+#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;
+ }
+
+ 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;
+ 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;
+ bool R2sebelum,L2sebelum;
+ 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
diff -r 000000000000 -r dd4c20b9a83e Motor.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/Motor/#c75b234558af
diff -r 000000000000 -r dd4c20b9a83e PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r dd4c20b9a83e Ping.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Ping.lib Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/rosienej/code/Ping/#6996f66161d7
diff -r 000000000000 -r dd4c20b9a83e encoderKRAI.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp Sun Feb 24 10:39:24 2019 +0000
@@ -0,0 +1,126 @@
+/********************************************************/
+/* Library untuk pembacaan Encoder */
+/* Adapsi dari QEI */
+/* */
+/* Encoder yang sudah dicoba : */
+/* 1. Autonics */
+/* 2. Encoder bawaan Motor */
+/* */
+/* ______________________ */
+/* |______Autonics______| */
+/* | Out A = Input 1 | */
+/* | Out B = Input 2 | */
+/* | 5V | */
+/* |_Gnd________________| */
+/* */
+/********************************************************/
+
+#include "mbed.h"
+
+#include "encoderKRAI.h"
+
+encoderKRAI::encoderKRAI(PinName channelA,
+ PinName channelB,
+ int pulsesPerRev,
+ Encoding encoding) : channelA_(channelA), channelB_(channelB)
+{
+ pulses_ = 0;
+ revolutions_ = 0;
+ pulsesPerRev_ = pulsesPerRev;
+ encoding_ = encoding;
+
+ //Workout what the current state is.
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+ prevState_ = currState_;
+
+ //X2 encoding uses interrupts on only channel A.
+ //X4 encoding uses interrupts on channel A,
+ //and on channel B.
+ channelA_.rise(this, &encoderKRAI::encode);
+ channelA_.fall(this, &encoderKRAI::encode);
+
+ //If we're using X4 encoding, then attach interrupts to channel B too.
+ if (encoding == X4_ENCODING) {
+ channelB_.rise(this, &encoderKRAI::encode);
+ channelB_.fall(this, &encoderKRAI::encode);
+ }
+}
+
+void encoderKRAI::reset(void) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+
+}
+
+/*int encoderKRAI::getCurrentState(void) {
+
+ return currState_;
+
+}*/
+
+int encoderKRAI::getPulses(void) {
+
+ return pulses_;
+
+}
+
+int encoderKRAI::getRevolutions(void) {
+
+ revolutions_ = pulses_ / pulsesPerRev_;
+ return revolutions_;
+
+}
+
+////////////////////////////////////////////////////////
+
+void encoderKRAI::encode(void) {
+
+ int change = 0;
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+
+ if (encoding_ == X2_ENCODING) {
+
+ //11->00->11->00 is counter clockwise rotation or "forward".
+ if ((prevState_ == 0x3 && currState_ == 0x0) ||
+ (prevState_ == 0x0 && currState_ == 0x3)) {
+
+ pulses_++;
+
+ }
+ //10->01->10->01 is clockwise rotation or "backward".
+ else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+ (prevState_ == 0x1 && currState_ == 0x2)) {
+
+ pulses_--;
+
+ }
+
+ } else if (encoding_ == X4_ENCODING) {
+
+ //Entered a new valid state.
+ if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+ //2 bit state. Right hand bit of prev XOR left hand bit of current
+ //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+ change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+ if (change == 0) {
+ change = -1;
+ }
+
+ pulses_ -= change;
+ }
+
+ }
+
+ prevState_ = currState_;
+
+}
diff -r 000000000000 -r dd4c20b9a83e encoderKRAI.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h Sun Feb 24 10:39:24 2019 +0000
@@ -0,0 +1,106 @@
+#ifndef ENCODERKRAI_H
+#define ENCODERKRAI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID 0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class encoderKRAI {
+
+public:
+
+ typedef enum Encoding {
+
+ X2_ENCODING,
+ X4_ENCODING
+
+ } Encoding;
+
+ /** Membuat interface dari encoder
+ *
+ * @param inA DigitalIn, out A dari encoder
+ * @param inB DigitalIn, out B dari encoder
+ */
+ encoderKRAI(PinName channelA, PinName channelB, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+ /**
+ * Reset encoder.
+ *
+ * Menset pulse dan revolusi/putaran menjadi 0
+ */
+ void reset(void);
+
+ /**
+ * Membaca pulse yang didapat oleh encoder
+ *
+ * @return Nilai pulse yang telah dilalui.
+ */
+ int getPulses(void);
+
+ /**
+ * Membaca putaran yang didapat oleh encoder
+ *
+ * @return Nilai revolusi/putaran yang telah dilalui.
+ */
+ int getRevolutions(void);
+
+ /**
+ * Membaca pulse yang didapat oleh encoder
+ *
+ * @return Nilai pulse yang telah dilalui.
+ */
+ //int readDeltaPulses(void);
+
+ /**
+ * Membaca putaran yang didapat oleh encoder
+ *
+ * @return Nilai revolusi/putaran yang telah dilalui.
+ */
+ //int readDeltaRevolutions(void);
+
+private:
+
+ /**
+ * Menghitung pulse
+ *
+ * Digunakan setiap rising/falling edge baik channel A atau B
+ * Membaca putaran CW atau CCW => mengakibatkan pertambahan/pengurangan pulse
+ */
+ void encode(void);
+
+ /**
+ * Indeks setiap rising edge untuk menghitung putaran
+ * Nilai bertambah 1
+ */
+ //void index(void);
+
+ Encoding encoding_;
+
+ InterruptIn channelA_;
+ InterruptIn channelB_;
+ //InterruptIn index_;
+
+ int pulsesPerRev_;
+ int prevState_;
+ int currState_;
+
+ volatile int pulses_;
+ volatile int revolutions_;
+
+
+};
+
+#endif /* ENCODERKRAI_H */
diff -r 000000000000 -r dd4c20b9a83e main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Feb 24 10:39:24 2019 +0000
@@ -0,0 +1,848 @@
+/*tuning motor baru untuk konstanta pid baru */
+/****************************************************************************/
+/* PROGRAM UNTUK PID CLOSED LOOP */
+/* */
+/* Last Update : 16 April 2017 */
+/* */
+/* - Digunakan encoder autonics */
+/* - Konfigurasi Motor dan Encoder sbb : */
+/* ______________________ */
+/* / \ Rode Depan Belakang: */
+/* / 2 (Belakang)//kanan \ Omniwheel */
+/* | | */
+/* | 3 (kiri)//depan 4 (kanan) | Roda Kiri Kanan: */
+/* | | Omniwheel */
+/* \ 1 (Depan)//kiri / */
+/* \______________________/ Putaran berlawanan arah */
+/* jarum jam positif */
+/* SETTINGS (WAJIB!) : */
+/* 1. Settings Pin Encoder, Resolusi, dan Tipe encoding di omniPos.h */
+/* 2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder */
+/* */
+/****************************************************************************/
+/* */
+/* Joystick */
+/* Kanan => */
+/* Kiri => */
+/* */
+/* Tombol silang => Pneumatik aktif */
+/* Tombol segitiga => Aktif motor Launcher */
+/* Tombol lingkaran => Reloader naik */
+/* Tombol kotak => Reloader turun */
+/* Tombol L1 => Pivot Kiri */
+/* Tombol R1 => Pivot Kanan */
+/* Tombol L2 => Kurang PWM Motor Launcher */
+/* Tombol R2 => Tambah PWM Motor Launcher */
+/* */
+/* Bismillahirahmanirrahim */
+/* Jagalah Kebersihan Kodingan */
+/****************************************************************************/
+
+#include "mbed.h"
+#include "JoystickPS3.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+#include "Ping.h"
+#include "DigitDisplay.h"
+
+/***********************************************/
+/* Konstanta dan Variabel */
+/***********************************************/
+#define PI 3.14159265
+#define D_ENCODER 10 // Diameter Roda Encoder
+#define D_ROBOT 80 // Diameter Roda Robot
+
+// Variable Atas
+// indek 2 untuk motor depan, 1 untuk motor belakang
+double speed, speed2;
+const double maxSpeed = 0.95, minSpeed = -0.95, Ts = 12.5;
+const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
+const double kpA2=0.1293, kdA2=1.0070, kiA2=0.0002986;
+double a1,b1,c1;
+double a2,b2,c2;
+double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
+double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
+double previous_speed1 = 0;
+double previous_speed2 = 0;
+
+float rpm, rpm2;
+double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
+const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
+
+const float pwmPowerUp = 1.0;
+const float pwmPowerDown = -1.0;
+
+double jarak_ping=0;
+//double ping_target = 15; // ping lama
+double ping_target = 14; // ping baru
+double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
+double ping_Kp = -0.3747, ping_Kd = -0.049, ping_Ts=10;
+double ping_pwm, ping_previous_pwm = 0;
+
+// Variable Bawah
+float PIVOT = 0.5; //0.17 // PWM Pivot Kanan, Pivot Kiri
+float tuneDpn = 1.0; //1.0 // Tunning PWM motor Depan
+float tuneBlk = 1.0; //1.0 // Tunning PWM motor belakang
+float tuneAksel = 0.6;
+int aksel = 0;
+float tuneAkselBlk = 0.4;
+//float tuneR = 1.0;
+float tuneL = 1.0;
+float serong = 0.4;
+float rasio = 1.4545;
+float t_new = 0.1;
+
+/* variable tunning */
+float tunning_L;
+//float tunning_R;
+float tunning_Dpn;
+float tunning_Blk;
+
+/* Deklarasi Variable Millis */
+static volatile uint32_t previousMillis = 0; // PID launcher
+static volatile uint32_t currentMillis;
+static volatile uint32_t previousMillis2 = 0; // PID launcher
+static volatile uint32_t currentMillis2;
+static volatile uint32_t previousMillis3 = 0; // Pneumatik
+static volatile uint32_t previousMillis4 = 0; // Ping
+static volatile uint32_t previousMillis5 = 0; // Display
+static volatile uint32_t previousMillis6 = 0; // pneu
+
+/* Variabel Stick */
+//Logic untuk masuk aktuator
+int case_joy;
+bool isLauncher = false;
+bool isReload = false;
+bool ReloadOn = false;
+bool flag_Pneu = false;
+bool flag_paku = false;
+
+bool ready = false;
+
+/*****************************************************/
+/* Definisi Prosedur, Fungsi dan Setting Pinout */
+/*****************************************************/
+
+/* Fungsi dan Procedur Encoder */
+void init_speed(); //
+void aktuator(); // Pergerakan aktuator berdasarkan case joystick
+int case_joystick(); // Mendapatkan case dari joystick
+//void speedKalibrasiMotor(); // Pertambahan target RPM motor atas melalui joystick
+
+/* Inisialisasi Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX,115200);
+
+/* Deklarasi Encoder Launcher */
+encoderKRAI encLauncherDpn( PC_10, PC_11, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encLauncherBlk( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
+
+/* Deklarasi Motor Base */
+Motor motorDpn(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
+//Motor motorBlk(PB_6, PC_14, PC_13); //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13);
+Motor motorBlk(PB_6, PC_14, PC_13);
+Motor motorL (PB_9, PA_12, PA_6);
+//Motor //////////MotorR (PB_8, PC_6, PC_5); //(PC_6, PB_4, PB_5);
+
+/* Deklarasi Motor Launcher */
+//Motor launcherDpn(PA_5,PA_11,PB_12); // pwm ,fwd, rev
+//Motor launcherBlk(PB_3, PC_4, PA_10); // pwm, fwd, rev
+//Motor powerScrew(PB_10, PB_14, PB_13); // pwm, fwd, rev
+
+/* Deklarasi Penumatik Launcher */
+DigitalOut pneumatik(PA_4, PullUp);
+DigitalOut pneu_paku(PC_2, PullUp);
+
+/*Dekalrasi Limit Switch */
+//DigitalIn infraAtas(PC_9, PullUp);
+DigitalIn limitTengah(PA_9, PullUp);
+DigitalIn limitBawah(PC_7, PullUp);
+DigitalIn limitBawah1(PA_7, PullUp);
+
+/*deklarasi PING ultrasonic*/
+Ping pingAtas(PC_15);
+
+/*Deklarasi Display*/
+DigitDisplay display (PA_8, PC_8);
+
+/****************************************************/
+/* Deklarasi Fungsi dan Procedure */
+/****************************************************/
+int case_joystick()
+{
+//---------------------------------------------------//
+// Gerak Motor Base //
+// Case 1 : Pivot kanan //
+// Case 2 : Pivot Kiri //
+// Case 3 : Kanan //
+// Case 4 : Kiri //
+// Case 5 : Break //
+//---------------------------------------------------//
+
+ int caseJoystick;
+ if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Pivot Kanan
+ caseJoystick = 1;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Pivot Kiri
+ caseJoystick = 2;
+ }
+ else if ((joystick.START_click)&&(!joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // tambah rpm dengan nilai tertentu
+ caseJoystick = 31;
+ }
+ else if ((!joystick.START_click)&&(joystick.SELECT_click)&&(!joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 32;
+ }
+ else if ((!joystick.START_click)&&(!joystick.SELECT_click)&&(joystick.R3_click)) {
+ // kurangi rpm dengan nilai tertentu
+ caseJoystick = 33;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kanan
+ caseJoystick = 17;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kiri
+ caseJoystick = 18;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri + Rotasi kanan
+ caseJoystick = 19;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kanan + Rotasi kiri
+ caseJoystick = 20;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kanan
+ caseJoystick = 21;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kiri
+ caseJoystick = 22;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kanan
+ caseJoystick = 23;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kiri
+ caseJoystick = 24;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.segitiga_click)) {
+ // Kanan + segitiga
+ caseJoystick = 25;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.segitiga_click)) {
+ // Kiri + segitiga
+ caseJoystick = 26;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.lingkaran_click)) {
+ // Kanan + lingkaran
+ caseJoystick = 27;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.lingkaran_click)) {
+ // Kiri + lingkaran
+ caseJoystick = 28;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)&&(joystick.kotak_click)) {
+ // Kanan + kotak
+ caseJoystick = 29;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)&&(joystick.kotak_click)) {
+ // Kiri + kotak
+ caseJoystick = 30;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Serong kanan maju
+ caseJoystick = 13;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Serong kiri maju
+ caseJoystick = 14;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Serong kanan mundur
+ caseJoystick = 15;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Serong kiri mundur
+ caseJoystick = 16;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan
+ caseJoystick = 3;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri
+ caseJoystick = 4;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Atas -- Maju
+ caseJoystick = 8;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Bawah -- Mundur
+ caseJoystick = 9;
+ }
+ else if (joystick.segitiga_click){
+ // Motor Launcher
+ caseJoystick = 5;
+ }
+ else if (joystick.R2_click){
+ // Target Pulse PID ++ Motor Depan
+ caseJoystick = 6;
+ }
+ else if (joystick.L2_click){
+ // Target Pulse PID -- Motor
+ caseJoystick = 7;
+ }
+ else if (joystick.silang_click){
+ // Pnemuatik ON
+ caseJoystick = 10;
+ }
+ else if ((joystick.lingkaran_click)&&(!joystick.kotak_click)) {
+ // Power Screw Up
+ caseJoystick = 11;
+ }
+ else if ((joystick.kotak_click)&&(!joystick.lingkaran_click)) {
+ // Power Screw Down
+ caseJoystick = 12;
+ }
+ else if (joystick.L3){
+ // Paku Bumi ON/OFF
+ caseJoystick = 34;
+ }
+ else
+ {
+ tuneAksel = 0.6;
+ aksel = 0;
+ }
+
+ return(caseJoystick);
+}
+
+
+void init_rpm (){
+ if (target_rpm>maxRPM-2){
+ target_rpm = maxRPM-2;
+ }
+ if (target_rpm<minRPM){
+ target_rpm = minRPM;
+ }
+ if (target_rpm2>maxRPM){
+ target_rpm2 = maxRPM;
+ }
+ if (target_rpm2<minRPM+2){
+ target_rpm2 = minRPM+2;
+ }
+}
+
+void aktuator()
+{
+ switch (case_joy) {
+ case (1):
+ {
+ // Pivot Kanan
+ motorDpn.speed(-rasio*PIVOT-t_new);
+ motorBlk.speed(-rasio*PIVOT-t_new);
+ ////////////////MotorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-PIVOT);
+ break;
+ }
+ case (2):
+ {
+ // Pivot Kiri
+ motorDpn.speed(rasio*PIVOT+t_new);
+ motorBlk.speed(rasio*PIVOT+t_new);
+ ////////////////MotorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(PIVOT);
+ break;
+ }
+ case (17):
+ {
+ // Kanan + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-rasio*PIVOT-t_new);
+ ////////////////MotorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
+ break;
+ }
+ case (18):
+ {
+ // Kanan + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(rasio*PIVOT+t_new);
+ ////////////////MotorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
+ break;
+ }
+ case (19):
+ {
+ // Kiri + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ ////////////////MotorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
+ break;
+ }
+ case (20):
+ {
+ // Kiri + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ //MotorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
+ break;
+ }
+ case (21):
+ {
+ // Maju + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ //MotorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
+ break;
+ }
+ case (22):
+ {
+ // Maju + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ //////////////MotorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
+ break;
+ }
+ case (23):
+ {
+ // Mundur + Rotasi Kanan
+ motorDpn.speed(-PIVOT);
+ motorBlk.speed(-PIVOT);
+ //////////////MotorR.speed(-rasio*PIVOT-t_new);
+ motorL.speed(-rasio*PIVOT-t_new);
+ break;
+ }
+ case (24):
+ {
+ // Mundur + Rotasi Kiri
+ motorDpn.speed(PIVOT);
+ motorBlk.speed(PIVOT);
+ //////////////MotorR.speed(rasio*PIVOT+t_new);
+ motorL.speed(rasio*PIVOT+t_new);
+ break;
+ }
+ case (25):
+ {
+ // Kanan + segitiga
+ isLauncher = !isLauncher;
+ break;
+ }
+ case (26):
+ {
+ // Kiri + segitiga
+ isLauncher = !isLauncher;
+ break;
+ }
+ case (27):
+ {
+ // Kanan + lingkaran
+ ReloadOn = !ReloadOn;
+ isReload = false;
+ break;
+ }
+ case (28):
+ {
+ // Kiri + lingkaran
+ ReloadOn = !ReloadOn;
+ isReload = false;
+ break;
+ }
+ case (29):
+ {
+ // Kanan + kotak
+ ReloadOn = !ReloadOn;
+ isReload = true;
+ break;
+ }
+ case (30):
+ {
+ // Kiri + kotak
+ ReloadOn = !ReloadOn;
+ isReload = true;
+ break;
+ }
+ case (13) :
+ {
+ // Serong kanan maju
+ motorDpn.speed(-serong);
+ motorL.speed(-serong-t_new);
+ motorBlk.speed(serong);
+ //////////////MotorR.speed(serong+t_new);
+ break;
+ }
+ case (14) :
+ {
+ // Serong kiri maju
+ motorDpn.speed(serong);
+ //////////////MotorR.speed(serong+t_new);
+ motorBlk.speed(-serong);
+ motorL.speed(-serong-t_new);
+ break;
+ }
+ case (15) :
+ {
+ // Serong kanan mundur
+ motorDpn.speed(-serong);
+ //////////////MotorR.speed(-serong-t_new);
+ motorBlk.speed(serong);
+ motorL.speed(serong+t_new);
+ break;
+ }
+ case (16) :
+ {
+ // Serong kiri mundur
+ motorDpn.speed(serong);
+ motorL.speed(serong+t_new);
+ motorBlk.speed(-serong);
+ //////////////MotorR.speed(-serong-t_new);
+ break;
+ }
+ case (3) :
+ {
+ // Kanan
+ aksel++;
+ if (aksel>300)
+ tuneAksel = 0.9;
+
+ motorDpn.speed(-tuneAksel);
+ motorBlk.speed(tuneAksel);
+ //////////////MotorR.brake(1);
+ motorL.brake(1);
+ break;
+ }
+ case (4) :
+ {
+ // Kiri
+ aksel++;
+ if (aksel>300)
+ tuneAksel = 0.9;
+
+ motorDpn.speed(tuneAksel);
+ motorBlk.speed(-tuneAksel);
+ //////////////MotorR.brake(1);
+ motorL.brake(1);
+ break;
+ }
+ case (8) :
+ {
+ // Maju
+ aksel++;
+ if (aksel>300)
+ tuneAksel = 0.9;
+
+ ////////////////MotorR.speed(tuneAksel);
+ motorL.speed(1);
+ motorDpn.brake(-tuneAksel);
+ motorBlk.speed(tuneAksel);
+ break;
+ }
+ case (9) :
+ {
+ // Mundur
+ aksel++;
+ if (aksel>300)
+ tuneAksel = 0.9;
+
+ ////////////////MotorR.speed(-tuneAksel);
+ motorL.speed(1);
+ motorDpn.brake(tuneAksel);
+ motorBlk.brake(-tuneAksel);
+ break;
+ }
+ case (5) :
+ {
+ // Aktifkan motor atas
+ isLauncher = !isLauncher;
+ break;
+ }
+ case (6) :
+ {
+ // Target Pulse PID ++ Motor Depan
+ target_rpm2 = target_rpm2+1.0;
+ target_rpm = target_rpm+1.0;
+ init_rpm();
+ break;
+ }
+ case (7) :
+ {
+ // Target Pulse PID -- Motor Depan
+ target_rpm2 = target_rpm2-1.0;
+ target_rpm = target_rpm-1.0;
+ init_rpm();
+ break;
+ }
+ case (10) :
+ {
+ // Pneumatik
+ if (ready)
+ {
+ pneumatik = 0;
+ previousMillis3 = millis();
+ flag_Pneu = true;
+ ready = false;
+ previousMillis6 = millis();
+
+ }
+ break;
+ }
+ case (11) :
+ {
+ // Power Screw Up
+ ReloadOn = !ReloadOn;
+ isReload = false;
+ break;
+ }
+ case (31) :
+ {
+ // start
+ target_rpm2 = 24;
+ target_rpm = 24;
+ init_rpm();
+ break;
+ }
+ case (32) :
+ {
+ // select
+ target_rpm2 = 11;
+ target_rpm = 11;
+ init_rpm();
+ break;
+ }
+ case (33) :
+ {
+ // R3
+ target_rpm2 = 17;
+ target_rpm = 17;
+ init_rpm();
+ break;
+ }
+ case (12) :
+ {
+ // Power Screw Down
+ ReloadOn = !ReloadOn;
+ isReload = true;
+ break;
+ }
+ case (34) :{
+ pneu_paku = !pneu_paku;
+ wait_ms(50);
+ if (pneu_paku == 1){
+ PIVOT = 0.17;
+ }else{
+ PIVOT = 0.8;
+ }
+ }
+ default :
+ {
+ tuneAksel = 0.6;
+ aksel = 0;
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ //////////////MotorR.brake(1);
+ motorL.brake(1);
+ }
+ } // End Switch
+ }
+
+/*void reloader()
+{
+ if(ReloadOn){
+ if(isReload){
+ powerScrew.speed(pwmPowerDown);
+ pc.printf("%.2f\n", jarak_ping);
+ if((!limitBawah)||(!limitBawah1)){
+ isReload = false;
+ ReloadOn = false;
+ }
+ }
+ else if(!limitTengah){
+ isReload = true;
+ }
+ else if(!flag_Pneu){
+ //pc.printf("%.2f\n", ping_pwm);
+ if (millis()-previousMillis6>700)
+ {
+ ping_current_error = (double) (ping_target-jarak_ping);
+
+ ping_sum_error += ping_current_error*ping_Ts;
+ ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;
+
+ if (ping_pwm>1) ping_pwm=1;
+ if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5;
+ if (ping_pwm<-0.049 && ping_pwm>-0.3) ping_pwm = -0.3;
+ if (ping_pwm<-1) ping_pwm=-1;
+
+ powerScrew.speed(ping_pwm);
+
+ ping_previous_error1 = ping_current_error;
+ }
+
+ }
+ if ((jarak_ping>(ping_target-2))&&(jarak_ping<(ping_target+2))){
+ ready = true;
+ }else{
+ ready = false;
+ }
+ }
+ else{
+ powerScrew.brake(1);
+ }
+}
+
+
+void launcher()
+{
+ if (isLauncher)
+ {
+ currentMillis = millis();
+ currentMillis2 = millis();
+
+ // PID Launcher Belakang
+ if (currentMillis-previousMillis>=Ts)
+ {
+ rpm = (float)encLauncherBlk.getPulses();
+ current_error1 = target_rpm - rpm;
+ a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
+ b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
+ c1 = kdA1/Ts;
+ speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
+ //init_speed();
+ if(speed > maxSpeed){
+ launcherBlk.speed(maxSpeed);
+ }
+ else if ( speed < minSpeed){
+ launcherBlk.speed(minSpeed);
+ }
+ else {
+ launcherBlk.speed(speed);
+ }
+ previous_speed1 = speed;
+ previous_error1_2 = previous_error1_1;
+ previous_error1_1 = current_error1;
+ encLauncherBlk.reset();
+ previousMillis = currentMillis;
+
+ }
+ // PID Launcher Depan
+ if (currentMillis2-previousMillis2>=Ts)
+ {
+ rpm2 = (float)encLauncherDpn.getPulses();
+ current_error2 = target_rpm2 - rpm2;
+ a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
+ b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
+ c2 = kdA2/Ts;
+ speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
+ //init_speed();
+ if (speed2 > maxSpeed){
+ launcherDpn.speed(maxSpeed);
+ }
+ else if ( speed2 < minSpeed){
+ launcherDpn.speed(minSpeed);
+ }
+ else{
+ launcherDpn.speed(speed2);
+ }
+ previous_speed2 = speed2;
+ previous_error2_2 = previous_error2_1;
+ previous_error2_1 = current_error2;
+ encLauncherDpn.reset();
+ previousMillis2 = currentMillis2;
+ }
+ //pc.printf("%.2f\t%.2f\n",rpm,rpm2);
+ }
+ else
+ {
+ launcherDpn.brake(1);
+ launcherBlk.brake(1);
+
+ previous_error1_1 = 0;
+ previous_error1_2 = 0;
+ previous_error2_1 = 0;
+ previous_error2_2 = 0;
+ previous_speed1 = 0;
+ previous_speed2 = 0;
+ }
+}
+*/
+/*********************************************************/
+/* Main Function */
+/*********************************************************/
+
+int main (void)
+{
+ // Set baud rate - 115200
+ joystick.setup();
+ pc.baud(115200);
+ wait_ms(1000);
+
+ // initializing encoder
+ pneumatik =1;
+
+ wait_ms(500);
+
+ //initializing PING
+ pingAtas.Send();
+
+ pc.printf("ready....");
+ startMillis();
+ while(1)
+ {
+ // interupsi pembacaan PING setiap 30 ms
+ if(millis() - previousMillis4 >= 10){ //30
+ jarak_ping = (float)pingAtas.Read_cm();
+
+ pingAtas.Send();
+ previousMillis4 = millis();
+ }
+
+ // Interrupt Serial
+ joystick.idle();
+ if(joystick.readable())
+ {
+ // Panggil fungsi pembacaan joystik
+ joystick.baca_data();
+ // Panggil fungsi pengolahan data joystik
+ joystick.olah_data();
+ // Masuk ke case joystick
+ case_joy = case_joystick();
+ //pc.printf("%d\n",case_joy);
+ aktuator();
+ //launcher();
+ //reloader();
+ if ((millis()-previousMillis3 >= 230)&&(flag_Pneu)){
+ pneumatik = 1;
+ flag_Pneu = false;
+ //wait_ms(1000);
+ }
+ }
+ else
+ {
+ joystick.idle();
+ }
+
+ if(millis() - previousMillis5 >= 400)
+ {
+ display.write(0,((int) rpm2) / 10);
+ display.write(1,((int)rpm2) % 10);
+ display.write(2, (int)target_rpm2 / 10);
+ display.write(3, (int)target_rpm2 % 10);
+ display.setColon(true);
+
+ previousMillis5 = millis();
+ }
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r dd4c20b9a83e mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/3a7713b1edbc \ No newline at end of file
diff -r 000000000000 -r dd4c20b9a83e millis.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/millis.lib Sun Feb 24 10:39:24 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/DFRobot/code/millis/#736e6cc31bcd