From 82f95eb54e1533adbfa753b7c9b307a285db636f Mon Sep 17 00:00:00 2001 From: Giga Hidjrika Aura Adkhy <52191436+gigahidjrikaaa@users.noreply.github.com> Date: Thu, 15 Jun 2023 20:46:51 +0700 Subject: [PATCH] Initial commit --- .gitattributes | 2 + LICENSE | 21 +++ README.md | 2 + Sinauro-PROMISE-Dualsense.ino | 296 ++++++++++++++++++++++++++++++++++ 4 files changed, 321 insertions(+) create mode 100644 .gitattributes create mode 100644 LICENSE create mode 100644 README.md create mode 100644 Sinauro-PROMISE-Dualsense.ino diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..dfe0770 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +# Auto detect text files and perform LF normalization +* text=auto diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..ad9cf52 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Giga Hidjrika Aura Adkhy + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..b8553cc --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# Sinauro-PROMISE-Dualsense + Control Sinauro PROMISE Robot using Dualsense. diff --git a/Sinauro-PROMISE-Dualsense.ino b/Sinauro-PROMISE-Dualsense.ino new file mode 100644 index 0000000..f4bf2ec --- /dev/null +++ b/Sinauro-PROMISE-Dualsense.ino @@ -0,0 +1,296 @@ +#include +#include + +#include "esp_bt_main.h" +#include "esp_bt_device.h" +#include"esp_gap_bt_api.h" +#include "esp_err.h" + +#if !defined(CONFIG_BT_SPP_ENABLED) +#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip. +#endif + +#define REMOVE_BONDED_DEVICES 1 // <- Set to 0 to view all bonded devices addresses, set to 1 to remove + +#define PAIR_MAX_DEVICES 20 +uint8_t pairedDeviceBtAddr[PAIR_MAX_DEVICES][6]; +char bda_str[18]; + +const int motorPinFwd[2] = {5, 13}; +const int reversePin[2] = {12, 23}; +const int resetFlashPin = 18; +const int resetESPin = 14; +const int buzzerPin = 19; +const int greenLight = 2; +const int yellowLight = 2; +const int redLight = 2; +bool revState[2] = {0, 0}; +int speed[2] = {0, 0}; +bool reset = 0; + +ezButton resetButton(resetFlashPin); + +// Interrupt service routine to reset the ESP32 +void IRAM_ATTR resetModule() { + Serial.println("Resetting module..."); + for (int i = 0; i < 5; i++) + { + digitalWrite(buzzerPin, HIGH); + delay(200); + digitalWrite(buzzerPin, LOW); + delay(200); + } + esp_restart(); +} + +void setup() { + + Serial.begin(115200); + + resetButton.setDebounceTime(50); + + // attach the interrupt to the reset button when it is pressed + attachInterrupt(digitalPinToInterrupt(resetESPin), resetModule, FALLING); + + // Replace the "1a:2b:3c:01:01:01" with the MAC address of the laptop + PS4.begin("50:84:92:81:b7:a7"); + Serial.println("Ready."); + pinMode(buzzerPin, OUTPUT); + for(int i = 0; i < 2; i++) + { + pinMode(motorPinFwd[i], OUTPUT); + pinMode(reversePin[i], OUTPUT); + } + pinMode(redLight, OUTPUT); + pinMode(greenLight, OUTPUT); + pinMode(yellowLight, OUTPUT); +} + +void loop() { + resetButton.loop(); + if(resetButton.isPressed() && !reset) + { + Serial.println("=========== FLASH MEMORY RESET ==========="); + digitalWrite(buzzerPin, HIGH); + removeFlashMemory(); + delay(2000); + digitalWrite(buzzerPin, LOW); + reset = true; + } else if(!resetButton.isPressed() && reset) + { + reset = false; + } + + if (PS4.isConnected()) { + if (PS4.Right()) Serial.println("Right Button"); + if (PS4.Down()) Serial.println("Down Button"); + if (PS4.Up()) Serial.println("Up Button"); + if (PS4.Left()) Serial.println("Left Button"); + + if (PS4.Square()) Serial.println("Square Button"); + if (PS4.Cross()){ + digitalWrite(buzzerPin, HIGH); + } else{ + digitalWrite(buzzerPin, LOW); + } + if (PS4.Circle()) Serial.println("Circle Button"); + if (PS4.Triangle()) Serial.println("Triangle Button"); + + if (PS4.UpRight()) Serial.println("Up Right"); + if (PS4.DownRight()) Serial.println("Down Right"); + if (PS4.UpLeft()) Serial.println("Up Left"); + if (PS4.DownLeft()) Serial.println("Down Left"); + + if (PS4.L1()) + { + } + if (PS4.R1()) + { + } + + if (PS4.Share()) Serial.println("Share Button"); + if (PS4.Options()) Serial.println("Options Button"); + if (PS4.L3()) Serial.println("L3 Button"); + if (PS4.R3()) Serial.println("R3 Button"); + + if (PS4.PSButton()) ; + if (PS4.Touchpad()) ; + + if (PS4.L2()) { + + } + if (PS4.R2()) { + + } + + updateSpeed(); + printSpeed(); + } + else { + for(int i = 0; i < 2; i++) + { + speed[i] = 0; + } + } + motorWrite(); +} + +void updateSpeed() +{ + for(int i = 0; i < 2; i++) + { + speed[i] = PS4.LStickY() * 1.5 + 0.25 * PS4.R2Value() - 0,25 * PS4.L2Value(); + if(i % 2 == 0) + speed[i] -= PS4.RStickX() * 0.5 - PS4.Right() * 255 + PS4.Left() * 255 - PS4.Up() * 255 + PS4.Down() * 255; + else if(i % 2 == 1) + speed[i] += PS4.RStickX() * 0.5 - PS4.Right() * 255 + PS4.Left() * 255 + PS4.Up() * 255 - PS4.Down() * 255; + + if(speed[i] > 255) + speed[i] = 255; + else if(speed[i] < -255) + speed[i] = -255; + } +} +void removeFlashMemory() +{ + initBluetooth(); + Serial.print("ESP32 bluetooth address: "); Serial.println(bda2str(esp_bt_dev_get_address(), bda_str, 18)); + // Get the numbers of bonded/paired devices in the BT module + int count = esp_bt_gap_get_bond_device_num(); + if(!count) { + Serial.println("No bonded device found."); + } else { + Serial.print("Bonded device count: "); Serial.println(count); + if(PAIR_MAX_DEVICES < count) { + count = PAIR_MAX_DEVICES; + Serial.print("Reset bonded device count: "); Serial.println(count); + } + esp_err_t tError = esp_bt_gap_get_bond_device_list(&count, pairedDeviceBtAddr); + if(ESP_OK == tError) { + for(int i = 0; i < count; i++) { + Serial.print("Found bonded device # "); Serial.print(i); Serial.print(" -> "); + Serial.println(bda2str(pairedDeviceBtAddr[i], bda_str, 18)); + if(REMOVE_BONDED_DEVICES) { + esp_err_t tError = esp_bt_gap_remove_bond_device(pairedDeviceBtAddr[i]); + if(ESP_OK == tError) { + Serial.print("Removed bonded device # "); + } else { + Serial.print("Failed to remove bonded device # "); + } + Serial.println(i); + } + } + } + } +} + +unsigned long int printTime; +void printInfo() +{ + if(millis() - printTime >= 1000) + { + if (PS4.isConnected()) { + if (PS4.Right()) Serial.println("Right Button"); + if (PS4.Down()) Serial.println("Down Button"); + if (PS4.Up()) Serial.println("Up Button"); + if (PS4.Left()) Serial.println("Left Button"); + + if (PS4.Square()) Serial.println("Square Button"); + if (PS4.Cross()) Serial.println("Cross Button"); + if (PS4.Circle()) Serial.println("Circle Button"); + if (PS4.Triangle()) Serial.println("Triangle Button"); + + if (PS4.UpRight()) Serial.println("Up Right"); + if (PS4.DownRight()) Serial.println("Down Right"); + if (PS4.UpLeft()) Serial.println("Up Left"); + if (PS4.DownLeft()) Serial.println("Down Left"); + + if (PS4.L1()) Serial.println("L1 Button"); + if (PS4.R1()) Serial.println("R1 Button"); + + if (PS4.Share()) Serial.println("Share Button"); + if (PS4.Options()) Serial.println("Options Button"); + if (PS4.L3()) Serial.println("L3 Button"); + if (PS4.R3()) Serial.println("R3 Button"); + + if (PS4.PSButton()) Serial.println("PS Button"); + if (PS4.Touchpad()) Serial.println("Touch Pad Button"); + + if (PS4.L2()) { + Serial.printf("L2 button at %d\n", PS4.L2Value()); + } + if (PS4.R2()) { + Serial.printf("R2 button at %d\n", PS4.R2Value()); + } + + if (PS4.LStickX()) { + Serial.printf("Left Stick x at %d\n", PS4.LStickX()); + } + if (PS4.LStickY()) { + Serial.printf("Left Stick y at %d\n", PS4.LStickY()); + } + if (PS4.RStickX()) { + Serial.printf("Right Stick x at %d\n", PS4.RStickX()); + } + if (PS4.RStickY()) { + Serial.printf("Right Stick y at %d\n", PS4.RStickY()); + } + Serial.println(); + } + printTime = millis(); + } +} + +unsigned long int printSpeedTime; +void printSpeed() +{ + if(millis() - printSpeedTime >= 200) + { + Serial.printf("%d\tSpeed1: %d\tSpeed2: %d\tRev1: %d\tRev2: %d\n", millis(), speed[0], speed[1], revState[0], revState[1]); + printSpeedTime = millis(); + } +} + +void motorWrite() +{ + for(int i = 0; i < 2; i++) + { + if(speed[i] < 0){ + digitalWrite(reversePin[i], HIGH); + } + else{ + digitalWrite(reversePin[i], LOW); + } + analogWrite(motorPinFwd[i], abs(speed[i])); + } +} + +bool initBluetooth() +{ + if(!btStart()) { + Serial.println("Failed to initialize controller"); + return false; + } + + if(esp_bluedroid_init() != ESP_OK) { + Serial.println("Failed to initialize bluedroid"); + return false; + } + + if(esp_bluedroid_enable() != ESP_OK) { + Serial.println("Failed to enable bluedroid"); + return false; + } + return true; +} + +char *bda2str(const uint8_t* bda, char *str, size_t size) +{ + if (bda == NULL || str == NULL || size < 18) { + return NULL; + } + sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x", + bda[0], bda[1], bda[2], bda[3], bda[4], bda[5]); + return str; +} +