KRAI 2017
BASE ROTASI OTOMATIS SEKUENSIAL
Dependencies: Motor PID mbed millis
Revision 0:312d1d0781ac, committed 2017-06-11
- Comitter:
- gustavaditya
- Date:
- Sun Jun 11 19:22:01 2017 +0000
- Commit message:
- BASE ROTASI OTOMATIS SEKUENSIAL
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/JoystickPS3.h Sun Jun 11 19:22:01 2017 +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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.lib Sun Jun 11 19:22:01 2017 +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/PID.lib Sun Jun 11 19:22:01 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3Arduino.txt Sun Jun 11 19:22:01 2017 +0000
@@ -0,0 +1,299 @@
+/*********************************************************************************************/
+/** **/
+/** 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;
+
+// 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.cpp Sun Jun 11 19:22:01 2017 +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_;
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoderKRAI.h Sun Jun 11 19:22:01 2017 +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 */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Jun 11 19:22:01 2017 +0000
@@ -0,0 +1,425 @@
+#include "mbed.h"
+#include "JoystickPS3.h"
+#include "Motor.h"
+#include "encoderKRAI.h"
+#include "millis.h"
+
+#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
+
+bool auto_rotate=false, flag_start=true, flag_buttonL1, flag_buttonR1, flag_select;
+bool reset_encoder = true;
+
+int mode=0;
+
+float V_x = 0.4, V_y = 0.3, V_pivot = 0.2;
+float rasio= (D_ROBOT2/D_ROBOT1);
+
+float K_enc = PI*D_ENCODER;
+float K_robot1 = PI*D_ROBOT1;
+float K_robot2 = PI*D_ROBOT2;
+
+float PIDSpeedX, PIDSpeedY, PIDSpeedT;
+float speedDpn, speedBlk, speedR, speedL;
+const float maxPIDSpeed = 0.3, minPIDSpeed = -0.3, Ts = 50;
+const float maxPIDSpeedT = 0.4, minPIDSpeedT = -0.4;
+double current_error, previous_error1 = 0, previous_error2 = 0;
+unsigned long int previousMillis=0;
+float setpointX=0.0, setpointY=0.0, setpointT=0.0;
+
+/* Untuk PID */
+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.11, KiX=0, KdX=0;
+float KpY=0.11, KiY=0, KdY=0;
+float KpT=0.08, KiT=0, KdT=3.33;
+
+/* Untuk joystick */
+int case_joy;
+
+/* Inisialisasi Pin TX-RX Joystik dan PC */
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
+
+/* Deklarasi Encoder Base */
+encoderKRAI encKanan( PA_14, PA_15, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encBlk( PB_4, PB_5, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encDpn( PC_11, PC_10, 28, encoderKRAI::X4_ENCODING);
+encoderKRAI encKiri( PC_12, PD_2, 28, encoderKRAI::X4_ENCODING);
+
+/* Deklarasi Motor Base */
+Motor motorDpn(PB_7, PC_3, PC_0);
+Motor motorBlk(PB_2, PB_15, PB_1);
+Motor motorL (PB_9, PA_12, PA_6);
+Motor motorR (PB_8, PC_6, PC_5);
+
+void setCenter()
+{
+/* Fungsi untuk menentukan center dari robot */
+ encDpn.reset();
+ encBlk.reset();
+ encKiri.reset();
+ encKanan.reset();
+}
+
+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.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kanan
+ caseJoystick = 3;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan + Rotasi kiri
+ caseJoystick = 4;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri + Rotasi kanan
+ caseJoystick = 5;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri + Rotasi kiri
+ caseJoystick = 6;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kanan
+ caseJoystick = 7;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju + Rotasi kiri
+ caseJoystick = 8;
+ }
+ else if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kanan
+ caseJoystick = 9;
+ }
+ else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur + Rotasi kiri
+ caseJoystick = 10;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
+ // Kanan
+ caseJoystick = 11;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
+ // Kiri
+ caseJoystick = 12;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Maju
+ caseJoystick = 13;
+ }
+ else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
+ // Mundur
+ caseJoystick = 14;
+ }
+
+ return(caseJoystick);
+}
+
+void aktuator()
+{
+ switch (case_joy) {
+ case (1):
+ // Pivot Kanan
+ motorDpn.speed(-V_pivot/rasio);
+ motorBlk.speed(-V_pivot/rasio);
+ motorR.speed(-V_pivot);
+ motorL.speed(-V_pivot);
+ break;
+ case (2):
+ // Pivot Kiri
+ motorDpn.speed(V_pivot/rasio);
+ motorBlk.speed(V_pivot/rasio);
+ motorR.speed(V_pivot);
+ motorL.speed(V_pivot);
+ break;
+ case (11):
+ // Kanan
+ /*speedDpn = -(V_x + PIDSpeedT);
+ speedBlk = (V_x + PIDSpeedT);
+ speedR = rasio*PIDSpeedT + PIDSpeedY;
+ speedL = rasio*PIDSpeedT - PIDSpeedY;*/
+ speedDpn = -(V_x);
+ speedBlk = (V_x);
+ //speedR = PIDSpeedY;
+ //speedL = - PIDSpeedY;
+ motorDpn.speed(speedDpn);
+ motorBlk.speed(speedBlk);
+ motorR.brake(1);
+ motorL.brake(1);
+ break;
+ case (12):
+ // Kiri
+ /*speedDpn = (V_x + PIDSpeedT);
+ speedBlk = -(V_x + PIDSpeedT);
+ speedR = rasio*PIDSpeedT + PIDSpeedY;
+ speedL = rasio*PIDSpeedT - PIDSpeedY;*/
+ speedDpn = (V_x);
+ speedBlk = -(V_x);
+ //speedR = PIDSpeedY;
+ //speedL = -PIDSpeedY;
+ motorDpn.speed(speedDpn);
+ motorBlk.speed(speedBlk);
+ motorR.brake(1);
+ motorL.brake(1);
+ break;
+ case (13):
+ // Maju
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.speed(0.3);
+ motorL.speed(-0.3);
+ break;
+ case (14):
+ // Mundur
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.speed(-0.3);
+ motorL.speed(0.3);
+ break;
+ default :
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.brake(1);
+ motorL.brake(1);
+ break;
+ } // End Switch
+}
+
+float getX()
+{
+ float jarakEncDpn, jarakEncBlk;
+ jarakEncDpn = (encDpn.getPulses())/(float)(540.0)*K_enc;
+ jarakEncBlk = (encBlk.getPulses())/(float)(540.0)*K_enc;
+ return (jarakEncBlk-jarakEncDpn)/2;
+}
+
+float getY()
+{
+ float jarakEncKir, jarakEncKan;
+ jarakEncKir = (encKiri.getPulses())/(float)(540.0)*K_enc;
+ jarakEncKan = (encKanan.getPulses())/(float)(540.0)*K_enc;
+ return (jarakEncKan-jarakEncKir)/2;
+}
+
+float getTetha()
+{
+ float busurDpn, busurBlk, busurKir, busurKan, sudut;
+ busurKir = ((encKiri.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+ busurKan = ((encKanan.getPulses())/(float)(540.0)*K_enc)/K_robot2*360.0;
+ busurDpn = ((encDpn.getPulses())/(float)(540.0)*K_enc)/K_robot1*360.0;
+ busurBlk = ((encBlk.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()
+{
+ errorX = setpointX - getX();
+ errorY = setpointY - getY();
+ errorT = setpointT - getTetha();
+
+ integralX = integralX + errorX*Ts;
+ integralY = integralY + errorY*Ts;
+ integralT = integralT + errorT*Ts;
+
+ derivativeX = (errorX - previous_errorX)/Ts;
+ derivativeY = (errorY - previous_errorY)/Ts;
+ derivativeT = (errorT - previous_errorT)/Ts;
+
+ PIDSpeedX = KpX*errorY + KiX*integralY + KdX*derivativeX;
+ PIDSpeedY = KpY*errorY + KiY*integralY + KdY*derivativeY;
+ PIDSpeedT = KpT*errorT + KiT*integralT + KdT*derivativeT;
+
+ previous_errorX = errorX;
+ previous_errorY = errorY;
+ previous_errorT = errorT;
+
+ if(PIDSpeedX > maxPIDSpeed){
+ PIDSpeedX = maxPIDSpeed;
+ }
+ else if (PIDSpeedX < minPIDSpeed){
+ PIDSpeedX = minPIDSpeed;
+ }
+
+ if(PIDSpeedY > maxPIDSpeed){
+ PIDSpeedY = maxPIDSpeed;
+ }
+ else if (PIDSpeedY < minPIDSpeed){
+ PIDSpeedY = minPIDSpeed;
+ }
+
+ if(PIDSpeedT > maxPIDSpeedT){
+ PIDSpeedT = maxPIDSpeedT;
+ }
+ else if (PIDSpeedT < minPIDSpeedT){
+ PIDSpeedT = minPIDSpeedT;
+ }
+}
+
+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.R1_click)&&(!joystick.L1_click)&&flag_buttonR1)
+ {
+ flag_buttonR1 = false;
+ mode--;
+ cekMode();
+ pc.printf("masuk R1, mode = %d\n",mode);
+ }
+ else { flag_buttonR1 = true; }
+
+ if ((!joystick.R1_click)&&(joystick.L1_click)&&flag_buttonL1)
+ {
+ flag_buttonL1 = false;
+ mode++;
+ cekMode();
+ pc.printf("masuk L1, mode = %d\n",mode);
+ }
+ else { flag_buttonL1 = true; }
+
+ if ((joystick.START_click)&&(!joystick.SELECT_click)&&flag_start)
+ {
+ // Masuk auto rotate
+ flag_start = false;
+ auto_rotate = true;
+ flag_buttonR1 = true;
+ flag_buttonL1 = true;
+ flag_select = true;
+ pc.printf("MASUK AUTO ROTATE");
+ }
+ else { flag_start = true; }
+
+ if ((!joystick.START_click)&&(joystick.SELECT_click)&&flag_select)
+ {
+ // Keluar auto rotate
+ flag_select = false;
+ auto_rotate = false;
+ reset_encoder = true;
+ setpointT = 0.0;
+ mode = 0;
+ pc.printf("KELUAR AUTO ROTATE");
+ }
+ else { flag_select = true; }
+}
+
+int main(void)
+{
+ joystick.setup();
+ pc.baud(115200);
+ wait_ms(1000);
+ startMillis();
+ while(1)
+ {
+
+ //COBA ROTASI
+ 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();
+ aktuator();
+
+ case_autoRotate();
+
+ while(auto_rotate)
+ {
+ joystick.idle();
+ if(joystick.readable())
+ {
+ joystick.baca_data();
+ joystick.olah_data();
+
+ if (reset_encoder){ setCenter(); reset_encoder = false; }
+
+ case_autoRotate();
+ aturSetpoint();
+
+ while (millis()-previousMillis>=Ts)
+ { PIDcompute(); previousMillis = millis(); }
+
+ //speedDpn = PIDSpeedT/rasio - PIDSpeedX;
+ //speedBlk = PIDSpeedT/rasio + PIDSpeedX;
+ //speedR = PIDSpeedT + PIDSpeedY;
+ //speedL = PIDSpeedT - PIDSpeedY;
+
+ speedDpn = PIDSpeedT;
+ speedBlk = PIDSpeedT;
+ speedR = PIDSpeedT*rasio;
+ speedL = PIDSpeedT*rasio;
+
+ if ((errorT > 0.5) || (errorT<-0.5))
+ {
+ motorDpn.speed(speedDpn);
+ motorBlk.speed(speedBlk);
+ motorR.speed(speedR);
+ motorL.speed(speedL);
+ }
+ else
+ {
+ motorDpn.brake(1);
+ motorBlk.brake(1);
+ motorR.brake(1);
+ motorL.brake(1);
+ }
+ pc.printf("SUDUT = %f\n",getTetha());
+ case_autoRotate();
+ }
+ else
+ {
+ joystick.idle();
+ }
+ }
+ }
+ else
+ {
+ joystick.idle();
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Jun 11 19:22:01 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/e1686b8d5b90 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/millis.lib Sun Jun 11 19:22:01 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/DFRobot/code/millis/#736e6cc31bcd