KRAI 2017
/
MainProgram_BaseBaru_13FebV2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Motor PID Joystick_OrdoV5 mbed millis
Fork of
Joystick_ManualBaseBaru_12FEB
by 
KRAI 2017
Revision 0:9072e932503c, committed 2015-05-28
- Comitter:
- fanny868
- Date:
- Thu May 28 14:50:05 2015 +0000
- Child:
- 1:56bd3e8f38c5
- Commit message:
- .
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/JoystickPS3.h Thu May 28 14:50:05 2015 +0000
@@ -0,0 +1,200 @@
+
+
+#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);
+
+ 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 = 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;
+ 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Thu May 28 14:50:05 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/aberk/code/Motor/#c75b234558af
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3Arduino.txt Thu May 28 14:50:05 2015 +0000
@@ -0,0 +1,283 @@
+/*********************************************************************************************/
+/** **/
+/** PROGRAM KOMUNIKASI DATA JOYSTIK PS3 **/
+/** **/
+/** Joystik PS3 -> Arduino -> STM32Nucleo **/
+/** **/
+/** Fanny Achmad Hindrarta **/
+/** EL'12 - 13212076 **/
+/** **/
+/** Last Update : 15 Januari 2015, 20.30 **/
+/*********************************************************************************************/
+
+#include <PS3BT.h>
+#include <usbhub.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;
+/* 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
+
+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;
+
+// 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
+ if (Usb.Init() == -1) {
+ Serial.print(F("\r\nOSC did not start"));
+ while (1); //halt
+ }
+ 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;
+
+ 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)){
+ segitiga_click = !segitiga_click;
+ }
+ if(PS3.getButtonClick(CIRCLE)){
+ lingkaran_click = !lingkaran_click;
+ }
+ if(PS3.getButtonClick(CROSS)){
+ silang_click = !silang_click;
+ }
+ if(PS3.getButtonClick(SQUARE)){
+ kotak_click = !kotak_click;
+ }
+ if(PS3.getButtonClick(UP)){
+ atas_click = !atas_click;
+ }
+ if(PS3.getButtonClick(RIGHT)){
+ kanan_click = !kanan_click;
+ }
+ if(PS3.getButtonClick(DOWN)){
+ bawah_click = !bawah_click;
+ }
+ if(PS3.getButtonClick(LEFT)){
+ kiri_click = !kiri_click;
+ }
+
+ button_click = 0;
+ button_click = (segitiga_click << 0) + (lingkaran_click << 1) + (silang_click << 2) + (kotak_click << 3) + (atas_click << 4) + (kanan_click << 5) + (bawah_click << 6) + (kiri_click << 7);
+
+ if(PS3.getButtonClick(R1)){
+ R1_click = !R1_click;
+ }
+ if(PS3.getButtonClick(R3)){
+ R3_click = !R3_click;
+ }
+ if(PS3.getButtonClick(L1)){
+ L1_click = !L1_click;
+ }
+ if(PS3.getButtonClick(L3)){
+ L3_click = !L3_click;
+ }
+ if(PS3.getButtonClick(START)){
+ START_click = !START_click;
+ }
+ if(PS3.getButtonClick(SELECT)){
+ SELECT_click = !SELECT_click;
+ }
+ if(PS3.getButtonClick(PS)){
+ PS_click = !PS_click;
+ }
+
+ RL_click = 0;
+ RL_click = (R1_click << 0) + (R3_click << 1) + (L1_click << 2) + (L3_click << 3) + (START_click << 4) + (SELECT_click << 5) + (PS_click << 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);
+ } else {
+ // PS3 Disconnected
+ Serial.write(0x88);
+ Serial.write(0x09);
+ }
+
+ kirimdatajoystik();
+ delay(5);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu May 28 14:50:05 2015 +0000
@@ -0,0 +1,659 @@
+/**
+Base 4 Nasional
+
+Case Gerak
+1. Pivot Kanan
+2. Pivot Kiri
+3. Maju
+4. Mundur
+5. Serong Atas Kanan
+6. Serong Bawah Kanan
+7. Serong Atas Kiri
+8. Serong Bawah Kiri
+9. Kanan
+10. Kiri
+11. Stop
+
+
+Bima Sahbani EL'12
+Fanny Achmad Hindrarta EL'12
+**/
+#include "mbed.h"
+#include "JoystickPS3.h"
+#include "Motor.h"
+
+#define vmax 1
+#define vmaxserong 0.9
+#define vmaxpivot 0.7
+#define ax 0.005
+//#define koefperlambatan 0.8
+
+// Deklarasi variabel motor
+Motor motor1(PA_15, PA_13, PA_14); // pwm, fwd, rev
+Motor motor2(PA_0, PC_14, PC_15); // pwm, fwd, rev
+Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
+Motor motor4(PC_6, PC_9, PC_8); // pwm, fwd, rev
+
+// Deklarasi Register Pneumatik
+DigitalOut pneumatik1(PC_11);
+DigitalOut pneumatik2(PD_2);
+
+// Deklarasi Timer Pneumatik
+Timer timer_pneu;
+
+// Inisialisasi Pin TX-RX Joystik dan PC
+joysticknucleo joystick(PA_11,PA_12);
+//Serial pc(USBTX,USBRX);
+
+//bool perlambatan=0;
+char case_ger;
+bool maju=false,mundur=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,pivki=false,pivka=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false;
+bool stop = true;
+bool t1, t2, pneu1, pneu2;
+int delay_pneumatik;
+double vcurr;
+
+
+int case_gerak()
+{
+ int casegerak;
+ if (!joystick.L1 && joystick.R1) {
+ // Pivot Kanan
+ casegerak = 1;
+ } else if (!joystick.R1 && joystick.L1) {
+ // Pivot Kiri
+ casegerak = 2;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>110 && joystick.LX<190) && (joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) ) {
+ // Maju
+ casegerak = 3;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>90 && joystick.LX<190) && (joystick.LY>=200) )|| ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))) {
+ // Mundur
+ casegerak = 4;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)))) {
+ // Serong Atas Kanan
+ casegerak = 5;
+ } else if(((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))) {
+ // Serong Bawah Kanan
+ casegerak = 6;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ // Serong Atas Kiri
+ casegerak = 7;
+ } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {
+ // Serong Bawah Kiri
+ casegerak = 8;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>=210) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) ) {
+ // Kanan
+ casegerak = 9;
+ } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX<=50) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))) {
+ // Kiri
+ casegerak = 10;
+ } else {
+ // Stop
+ casegerak = 11;
+ }
+
+ if(joystick.silang_click && t1==0 && t2==0) {
+ pneu1 = 1;
+ } else {
+ pneu1 = 0;
+ }
+
+ if(joystick.kotak_click && t1==0 && t2==0) {
+ pneu2 = 1;
+ } else {
+ pneu2 = 0;
+ }
+ return(casegerak);
+}
+
+
+
+/**
+
+** Case 1 : Pivot Kanan
+** Case 2 : Pivot Kiri
+** Case 3 : Maju
+** Case 4 : Mundur
+** Case 5 : Serong Atas Kanan
+** Case 6 : Serong Bawah Kanan
+** Case 7 : Serong Atas Kiri
+** Case 8 : Serong Bawah Kiri
+** Case 9 : Kanan
+** Case 10 : Kiri
+** Case 11 : Break
+
+**/
+void aktuator()
+{
+ double koef;
+ double s1=0,s2=0,s3=0,s4=0;
+
+ // PNEUMATIK
+ if(t1==1) {
+ if(timer_pneu.read_ms() - delay_pneumatik > 800) {
+ pneumatik1 = 1;
+ t1=0;
+ }
+ }
+ if(t2==1) {
+ if(timer_pneu.read_ms() - delay_pneumatik > 800) {
+ pneumatik2 = 1;
+ t2=0;
+ }
+ }
+
+ if (pneu1 == 1 || pneu2==1) {
+ timer_pneu.reset();
+ delay_pneumatik = timer_pneu.read_ms();
+ if(pneu1 == 1) {
+ pneumatik1 = 0;
+ t1 = 1;
+ pneu1 = 0;
+ } else if(pneu2 == 1) {
+ pneumatik2 = 0;
+ t2 = 1;
+ pneu2 = 0;
+ }
+
+ }
+
+ // MOTOR
+ switch (case_ger)
+ {
+ case (1):
+ {
+ if (pivka) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmaxpivot) {
+ vcurr=vmaxpivot;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ }
+ else {
+ koef=1;
+ }
+
+ s1 = (float)(-0.5*koef*vcurr);
+ s2 = (float)(0.5*koef*vcurr);
+ s3 = (float)(-0.5*koef*vcurr);
+ s4 = (float)(0.5*koef*vcurr);
+
+ pivka=true;
+ maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("pivKa\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (2):
+ {
+ if (pivki){
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmaxpivot) {
+ vcurr=vmaxpivot;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(0.5*koef*vcurr);
+ s2 = (float)(-0.5*koef*vcurr);
+ s3 = (float)(0.5*koef*vcurr);
+ s4 = (float)(-0.5*koef*vcurr);
+
+ pivki=true;
+ maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("pivKi\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (3):
+ {
+ if (maju) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) { koef=0.5;}
+ else {
+ koef=1;
+ }
+
+ s1 = (float)(-1*koef*vcurr);
+ s2 = (float)(1.0*koef*vcurr);
+ s3 = (float)(1.0*koef*vcurr);
+ s4 = (float)(-1*koef*vcurr);
+
+ //s1 =-0.8*koef*vcurr;
+ //s2 =koef*vcurr;
+ //s3 =-koef*vcurr;
+ //s4 =koef*vcurr;
+
+ maju=true;
+ mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("maju\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (4):
+ {
+ if (mundur) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(1*koef*vcurr);
+ s2 = (float)(-1*koef*vcurr);
+ s3 = (float)(-1*koef*vcurr);
+ s4 = (float)(1*koef*vcurr);
+
+ mundur=true;
+ maju=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("mundur\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (5) :
+ {
+ if (saka) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ } if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(-koef*vcurr);
+ s2 = (float)(0); //koef*0.1*vcurr;
+ s3 = (float)(koef*vcurr);
+ s4 = (float)(0); //-koef*0.1*vcurr;
+
+ saka=true;
+ maju=mundur=kiri=kanan=sbka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("saka\n");
+
+ motor1.speed(s1);
+ motor2.brake(1);
+ //motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.brake(1);
+ //motor4.speed(s4);
+
+ break;
+ }
+ case (6) :
+ {
+ if (sbka){
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmaxserong) {
+ vcurr=vmaxserong;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(0); //koef*0.1*vcurr;
+ s2 = (float)(-koef*vcurr);
+ s3 = (float)(0); //-koef*0.1*vcurr;
+ s4 = (float)(koef*vcurr);
+
+ sbka=true;
+ maju=mundur=kiri=kanan=saka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("sbka\n");
+
+ //motor1.speed(s1);
+ motor1.brake(1);
+ motor2.speed(s2);
+ //motor3.speed(s3);
+ motor3.brake(1);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (7) :
+ {
+ if (saki) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmaxserong) {
+ vcurr=vmaxserong;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(0); //-koef*0.1*vcurr;
+ s2 = (float)(koef*vcurr);
+ s3 = (float)(0); //koef*0.1*vcurr;
+ s4 = (float)(-koef*vcurr);
+
+ saki=true;
+ maju=kiri=kanan=saka=mundur=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("saki\n");
+
+ //motor1.speed(s1);
+ motor1.brake(1);
+ motor2.speed(s2);
+ //motor3.speed(s3);
+ motor3.brake(1);
+ motor4.speed(s4);
+
+ break;
+ }
+ case (8) :
+ {
+ if (sbki) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmaxserong) {
+ vcurr=vmaxserong;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 = (float)(koef*vcurr);
+ s2 = (float)(0); //-koef*0.1*vcurr;
+ s3 = (float)(-koef*vcurr);
+ s4 = (float)(0); //koef*0.1*vcurr;
+
+ sbki=true;
+ maju=kiri=kanan=saka=saki=sbka=mundur=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("sbki\n");
+
+ motor1.speed(s1);
+ //motor2.speed(s2);
+ motor2.brake(1);
+ motor3.speed(s3);
+ //motor4.speed(s4);
+ motor4.brake(1);
+
+ break;
+ }
+ case (9) :
+ {
+ if (kanan) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=0;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 =(float)(-1*koef*vcurr);
+ s2 =(float)(-1.0*koef*vcurr);
+ s3 =(float)(1*koef*vcurr);
+ s4 =(float)(1.0*koef*vcurr);
+
+ kanan=true;
+ maju=kiri=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("Kanan\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+ break;
+ }
+ case (10) :
+ {
+ if (kiri) {
+ if(vcurr<0.1) {
+ vcurr=0.1;
+ } else {
+ vcurr+=ax;
+ }
+ //perlambatan=1;
+ } else {
+ //perlambatan=1;
+ }
+
+ if (vcurr>=vmax) {
+ vcurr=vmax;
+ }
+
+ if(joystick.R2==255 && joystick.L2==0) {
+ koef=2;
+ } else if (joystick.L2==255 && joystick.R2==0) {
+ koef=0.5;
+ } else {
+ koef=1;
+ }
+
+ s1 =(float)(1*koef*vcurr);
+ s2 =(float)(1*koef*vcurr);
+ s3 =(float)(-1*koef*vcurr);
+ s4 =(float)(-1.0*koef*vcurr);
+
+ kiri=true;
+ maju=kanan=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+
+ //pc.printf("Kiri\n");
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+ break;
+ }
+ default :
+ {
+ //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
+ //if (maju && (vcurr>=0.5)) wait_ms(100);
+ //else if (maju && (vcurr<0.5)) wait_ms(50);
+ /*
+ if(s1>0.2 || s1<-0.2 || s2>0.2 || s2<-0.2) {
+ s1 = koefperlambatan * s1;
+ s2 = koefperlambatan * s2;
+ s3 = koefperlambatan * s3;
+ s4 = koefperlambatan * s4;
+
+ motor1.speed(s1);
+ motor2.speed(s2);
+ motor3.speed(s3);
+ motor4.speed(s4);
+
+
+ } else {
+ */
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ //}
+
+ maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+ stop = true;
+
+ //s1 = 0;s2 =0; s3 =0; s4 =0;
+
+ //pc.printf("Stop\n");
+ }
+ }
+}
+
+
+int main (void)
+{
+ // Set baud rate - 115200
+ joystick.setup();
+ //pc.baud(115200);
+ //pc.printf("Ready...\n");
+ timer_pneu.start();
+ pneumatik1=1;
+ pneumatik2=1;
+ t1=0;
+ t2=0;
+ while(1)
+ {
+ // Interrupt Serial
+ joystick.idle();
+ if(joystick.readable() ) {
+ // Panggil fungsi pembacaan joystik
+ joystick.baca_data();
+ // Panggil fungsi pengolahan data joystik
+ joystick.olah_data();
+ //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
+ case_ger = case_gerak();
+ aktuator();
+
+ } else {
+ joystick.idle();
+
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu May 28 14:50:05 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5 \ No newline at end of file