From c55efb671248d01953a9fb8652597af319cfad41 Mon Sep 17 00:00:00 2001 From: Corbin <63320116+Tw1ZZLER@users.noreply.github.com> Date: Wed, 13 Nov 2024 20:19:15 -0500 Subject: [PATCH 01/23] Popped changes from dev/data-aq into dev/experimental+data-aq --- src/Drive/Drive.cpp | 67 ------------------------- src/Utilities/PrintSerial.cpp | 92 +++++++++++++++++++++++++++++++++++ src/Utilities/PrintSerial.h | 34 +++++++++++++ src/main.cpp | 24 ++++++++- 4 files changed, 149 insertions(+), 68 deletions(-) create mode 100644 src/Utilities/PrintSerial.cpp create mode 100644 src/Utilities/PrintSerial.h diff --git a/src/Drive/Drive.cpp b/src/Drive/Drive.cpp index 889c3764..884835a8 100644 --- a/src/Drive/Drive.cpp +++ b/src/Drive/Drive.cpp @@ -328,73 +328,6 @@ void Drive::printSetup() { Serial.print(F("\n")); } -/** - * prints the internal variables to the serial monitor in a clean format, - * this function exists out of pure laziness to not have to comment out all the print statments - * @author - * Updated: -*/ -void Drive::printDebugInfo() { - Serial.print(F("L_Hat_Y: ")); - Serial.print(stickForwardRev); - Serial.print(F(" R_HAT_X: ")); - Serial.print(stickTurn); - - // Serial.print(F(" | Turn: ")); - // Serial.print(lastTurnPwr); - - // Serial.print(F(" | Left ReqPwr: ")); - // Serial.print(requestedMotorPower[0]); - // Serial.print(F(" Right ReqPwr: ")); - // Serial.print(requestedMotorPower[1]); - - Serial.print(F(" | Omega: ")); - Serial.print(omega); - - Serial.print(F(" omega_L: ")); - Serial.print(omega_L); - Serial.print(F(" omega_R: ")); - Serial.print(omega_R); - - // Serial.print(F(" lastRampTime ")); - // Serial.print(lastRampTime[0]); - // Serial.print(F(" requestedPower ")); - // Serial.print(requestedPower); - // Serial.print(F(" current ")); - // Serial.print(currentRampPower[0]); - // Serial.print(F(" requestedPower - currentRampPower ")); - // Serial.println(requestedPower - currentRampPower[mtr], 10); - - Serial.print(F(" Left Motor: ")); - Serial.print(requestedMotorPower[0]); - Serial.print(F(" Right: ")); - Serial.print(requestedMotorPower[1]); - - //Serial.print(F(" scaledSensitiveTurn: ")); - //Serial.print(scaledSensitiveTurn); - - Serial.print(F("\n")); -} -/** - * @brief Prints variables to the serial monitor in a csv format - * This function is important for data acquisition - * The options below are configurable, change them as you need - * Remember to adhere to printing guidelines under PR-Docs - * @author Corbin Hibler - * Updated: 2023-10-30 -*/ -void Drive::printCsvInfo() { - Serial.print(F("header1,")); // name of value to be used as header - Serial.print(1); // variable you want to track - Serial.print(F(",header2,")); - Serial.print(2); - Serial.print(F(",header3,")); - Serial.print(3); - Serial.print(F(",header4,")); - Serial.print(4); - Serial.print(F(",header5,")); - Serial.println(5); // last line is -ALWAYS- println or else the python script will break -} /** * @brief updates the motors after calling all the functions to generate * turning and scaling motor values, the intention of this is so the diff --git a/src/Utilities/PrintSerial.cpp b/src/Utilities/PrintSerial.cpp new file mode 100644 index 00000000..b3b2add4 --- /dev/null +++ b/src/Utilities/PrintSerial.cpp @@ -0,0 +1,92 @@ +#include +#include +#include "PrintSerial.h" + +PrintSerial::PrintSerial() { + serialHeaders = {"forwardPower","turnPower","header3","header4","header5"}; +} + +void PrintSerial::updateValues() { + PrintSerial::serialValues = {drive->getForwardPower(), 2.0, 3.0, 4.0, 5.0}; +} + +void PrintSerial::setDriveObj(Drive* driveObj) { + PrintSerial::drive = driveObj; +} + +/** + * @brief prints the internal variables to the serial monitor in a clean and easy to read format + * @author Everybody + * @date 2024-02-12 +*/ +void PrintSerial::printDebugInfo() { + // Serial.print(F("L_Hat_Y: ")); + // Serial.print(stickForwardRev); + // Serial.print(F(" R_HAT_X: ")); + // Serial.print(stickTurn); + + // Serial.print(F(" | Turn: ")); + // Serial.print(lastTurnPwr); + + // Serial.print(F(" | Left ReqPwr: ")); + // Serial.print(requestedMotorPower[0]); + // Serial.print(F(" Right ReqPwr: ")); + // Serial.print(requestedMotorPower[1]); + + // Serial.print(F(" | Omega: ")); + // Serial.print(omega); + + // Serial.print(F(" omega_L: ")); + // Serial.print(omega_L); + // Serial.print(F(" omega_R: ")); + // Serial.print(omega_R); + + // Serial.print(F(" lastRampTime ")); + // Serial.print(lastRampTime[0]); + // Serial.print(F(" requestedPower ")); + // Serial.print(requestedPower); + // Serial.print(F(" current ")); + // Serial.print(currentRampPower[0]); + // Serial.print(F(" requestedPower - currentRampPower ")); + // Serial.println(requestedPower - currentRampPower[mtr], 10); + + // Serial.print(F(" Left Motor: ")); + // Serial.print(requestedMotorPower[0]); + // Serial.print(F(" Right: ")); + // Serial.print(requestedMotorPower[1]); + + //Serial.print(F(" scaledSensitiveTurn: ")); + //Serial.print(scaledSensitiveTurn); + + // Serial.print(F("\n")); +} + +/** + * @brief Prints variables to the serial monitor in a csv format + * This function is important for data acquisition + * The options below are configurable, change them as you need + * Remember to adhere to printing guidelines under PR-Docs + * @param values A vector of float values that will be sent to serial monitor + * @param headers A vector of header strings that will be sent to serial monitor + * @author Corbin Hibler + * Updated: 2024-02-12 +*/ +void PrintSerial::printCsvInfo() { + for (int i = 0; i < serialValues.size(); i++) { + if (i == 0) { + String header = serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.print(serialValues[i]); + } + else if (i < (serialValues.size() - 1)) { + String header = "," + serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.print(serialValues[i]); + } + else { + String header = "," + serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.println(serialValues[i]); + } + } +} \ No newline at end of file diff --git a/src/Utilities/PrintSerial.h b/src/Utilities/PrintSerial.h new file mode 100644 index 00000000..b830ec48 --- /dev/null +++ b/src/Utilities/PrintSerial.h @@ -0,0 +1,34 @@ +#pragma once + +#ifndef PRINTSERIAL_H +#define PRINTSERIAL_H + +#include +#include +#include + +/** + * @author Corbin Hibler + * @date 2024-02-12 + * @brief Prints information to serial in various formats + */ +class PrintSerial { + private: + PrintSerial(); + Drive* drive; + std::vector serialValues; + std::vector serialHeaders; + public: + static PrintSerial& getInstance() { + static PrintSerial instance; + return instance; + } + PrintSerial(const PrintSerial& obj) = delete; // delete copy constructor + void operator=(PrintSerial const&) = delete; // delete set operator + void setDriveObj(Drive* driveObj); + void updateValues(); + void printDebugInfo(); + void printCsvInfo(); +}; + +#endif // PRINTSERIAL_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 7771848b..141f90e8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,11 +9,13 @@ #include #include // ESP PS5 library, access using global instance `ps5` +#include // Custom Polar Robotics Libraries: #include #include #include +#include // Robot Includes #include @@ -34,6 +36,7 @@ Robot* robot = nullptr; // subclassed if needed Drive* drive = nullptr; // subclassed if needed Lights& lights = Lights::getInstance(); +PrintSerial& printserial = PrintSerial::getInstance(); //* How to use subclasses: ((SubclassName*) robot)->function() //! You must downcast each time you use a special function @@ -140,7 +143,7 @@ void setup() { //! Activate Pairing Process: this code is BLOCKING, not instantaneous activatePairing(); - + // Once paired, set lights to appropriate status lights.setLEDStatus(Lights::PAIRED); @@ -149,6 +152,8 @@ void setup() { ((Kicker*) robot)->enable(); } + printserial.setDriveObj(drive); + ps5.attachOnConnect(onConnection); ps5.attachOnDisconnect(onDisconnect); } @@ -231,6 +236,23 @@ void loop() { lights.updateTime = millis(); } } + + //* Update the motors based on the inputs from the controller + //* Can change functionality depending on subclass, like robot.action() + drive->update(); + + //* Data Acquisition *// + // Include values you want to monitor + //std::vector serialValues = {1.0, 2.0, 3.0, 4.0, 5.0}; + // Include headers of the values + //std::vector serialHeaders = {"header1","header2","header3","header4","header5"}; + + // printserial.printDebugInfo(serialValues); // prints info to serial monitor in a clean format (not usable by scripts) + printserial.updateValues(); + printserial.printCsvInfo(); // prints info to serial monitor in a csv (comma separated value) format + + if (lights.returnStatus() == lights.DISCO) + lights.updateLEDS(); //! Performs all special robot actions depending on the instantiated Robot subclass robot->action(); From a3fd5c94889124606cbc6b46f2e711400215cc14 Mon Sep 17 00:00:00 2001 From: Corbin <63320116+Tw1ZZLER@users.noreply.github.com> Date: Wed, 13 Nov 2024 20:34:39 -0500 Subject: [PATCH 02/23] Popped changes from prod+pin-pairing to dev/experimental+pin-pairing --- platformio.ini | 25 ++++++ src/Pairing/pairing.cpp | 171 +++++++++++++++++++++------------------- src/Pairing/pairing.h | 3 +- src/PolarRobotics.h | 6 ++ src/main.cpp | 1 + 5 files changed, 125 insertions(+), 81 deletions(-) diff --git a/platformio.ini b/platformio.ini index 6551880a..83cad68d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -20,6 +20,31 @@ lib_deps = https://github.com/PolarRobotics/PR-Lib.git adafruit/Adafruit LIS3MDL@^1.2.4 extra_scripts = pre:pio_build_script.py +; default to small motor size +; build_flags = -D MOTOR_TYPE=1 +; build_src_filter = +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; + +; - +; - +; - +; - [env:robot] build_src_filter = diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index fdc2ea85..f0f957e5 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -58,6 +58,7 @@ bool foundController = false; // Bluetooth connection security and role for ESP32 esp_spp_sec_t sec_mask = ESP_SPP_SEC_NONE; // or ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE to request pincode confirmation esp_spp_role_t role = ESP_SPP_ROLE_SLAVE; // ESP_SPP_ROLE_MASTER or ESP_SPP_ROLE_SLAVE +bool doDiscovery = false; // default false, only true if PAIRING_PIN jumped to HIGH // MAC Addresses to match to PS5 Controllers const char* macTest = "bc:c7:46:03"; // length 11 @@ -132,7 +133,7 @@ void getAddress(const char* &addr) { /// @brief Search for PS5 Controllers and pair to the first one found /// @param doRePair whether or not to search for the controller whose MAC address is stored in non-volatile memory, default true /// @param discoverTime the time limit to repair to existing devices, or search for new devices, in milliseconds -void activatePairing(bool doRePair, int discoverTime) { +void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int repairTime = DEFAULT_BT_REPAIR_TIME) { Serial.begin(115200); // pinMode(LED_BUILTIN, OUTPUT); @@ -141,81 +142,68 @@ void activatePairing(bool doRePair, int discoverTime) { const char* addrCharPtr = nullptr; getAddress(addrCharPtr); - if (doRePair) { - // see if we have a stored MAC address and try to pair to it - if (addrCharPtr != nullptr) { - Serial.print(F("Connecting to PS5 Controller @ ")); - Serial.println(addrCharPtr); - ps5.begin(addrCharPtr); + Serial.print(F("PAIRING PIN ")); + Serial.println(digitalRead(PAIRING_PIN)); + + if (digitalRead(PAIRING_PIN) == HIGH) { //! search for new devices + // begin broadcasting as "ESP32" as master role + if (!SerialBT.begin("ESP32", true)) { + Serial.println(F("SerialBT failed!")); // function returns false if failed + abort(); + } + SerialBT.enableSSP(); // according to SRC of this code, doesn't seem to change anything + + Serial.println(F("Searching for devices...")); + BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! + + // Beginning of Asynchronous Discovery Process + if (startDiscovery()) { int timer = 0; - // wait until discovery time passes or we connect to a controller - while (timer < discoverTime && !ps5.isConnected()) { + // recall foundController is set by the callback in `startDiscovery` when a valid PS5 controller is found + while (timer < discoverTime && !foundController) { delay(LOOP_DELAY); timer += LOOP_DELAY; - - // slow blink when searching for previous device - if (timer % (5 * LOOP_DELAY) == 0) { + Lights::getInstance().updateLEDS(); + + // double blink when in pairing mode like PS5 controller + // at: 300/400, 600/700 + if ((timer % 1000) % (7 * LOOP_DELAY) == 0) + toggleBuiltInLED(); + else if ((timer % 1000) % (4 * LOOP_DELAY) == 0) + toggleBuiltInLED(); + else if ((timer % 1000) % (3 * LOOP_DELAY) == 0 && + (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 toggleBuiltInLED(); - } } - // return if we get a connection at this point - if (ps5.isConnected()) { - Serial.println(F("PS5 Controller Connected!")); - yeet; - } // otherwise look for devices to pair with - } - } - - // begin broadcasting as "ESP32" as master role - if (!SerialBT.begin("ESP32", true)) { - Serial.println(F("SerialBT failed!")); // function returns false if failed - abort(); - } - SerialBT.enableSSP(); // according to SRC of this code, doesn't seem to change anything - - Serial.println(F("Searching for devices...")); - BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! - - // Beginning of Asynchronous Discovery Process - if (startDiscovery()) { - int timer = 0; - - // recall foundController is set by the callback in `startDiscovery` when a valid PS5 controller is found - while (timer < discoverTime && !foundController) { - delay(LOOP_DELAY); - timer += LOOP_DELAY; - Lights::getInstance().updateLEDS(); + Serial.println(F("Stopping discoverAsync... ")); + SerialBT.discoverAsyncStop(); + Serial.println(F("discoverAsync stopped")); + delay(5000); //! why is this delay here? does removing it affect anything? this was in the original code, I must never have noticed it. - // double blink when in pairing mode like PS5 controller - // at: 300/400, 600/700 - if ((timer % 1000) % (7 * LOOP_DELAY) == 0) - toggleBuiltInLED(); - else if ((timer % 1000) % (4 * LOOP_DELAY) == 0) - toggleBuiltInLED(); - else if ((timer % 1000) % (3 * LOOP_DELAY) == 0 && - (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 - toggleBuiltInLED(); - } - - Serial.println(F("Stopping discoverAsync... ")); - SerialBT.discoverAsyncStop(); - Serial.println(F("discoverAsync stopped")); - delay(5000); //! why is this delay here? does removing it affect anything? this was in the original code, I must never have noticed it. - - // If we find devices, list them and try to pair if it is a valid controller. - if(btDeviceList->getCount() > 0) { - BTAddress addr; - int channel = 0; - Serial.println(F("Found devices:")); - for (int i = 0; i < btDeviceList->getCount(); i++) { - BTAdvertisedDevice* device = btDeviceList->getDevice(i); - addr = device->getAddress(); - auto name = device->getName().c_str(); // get name to print and check - auto addrStr = addr.toString().c_str(); // std::string doesn't work with Serial.print for some reason - // ps5.begin requires a const char*, so get memory address of string/char array - addrCharPtr = &addr.toString().c_str()[0]; // declared at top of function + // If we find devices, list them and try to pair if it is a valid controller. + if(btDeviceList->getCount() > 0) { + BTAddress addr; + int channel = 0; + Serial.println(F("Found devices:")); + for (int i = 0; i < btDeviceList->getCount(); i++) { + BTAdvertisedDevice* device = btDeviceList->getDevice(i); + addr = device->getAddress(); + auto name = device->getName().c_str(); // get name to print and check + auto addrStr = addr.toString().c_str(); // std::string doesn't work with Serial.print for some reason + // ps5.begin requires a const char*, so get memory address of string/char array + addrCharPtr = &addr.toString().c_str()[0]; // declared at top of function + + // print out relevant controller details + // reminder that we need to use flash strings whenever possible, so don't try to collapse this + Serial.print(i); + Serial.print(F(" | ")); + Serial.print(addr.toString().c_str()); + Serial.print(F(" | ")); + Serial.print(device->getName().c_str()); + Serial.print(F(" | ")); + Serial.println(device->getRSSI()); // print out relevant controller details // reminder that we need to use flash strings whenever possible, so don't try to collapse this @@ -236,21 +224,44 @@ void activatePairing(bool doRePair, int discoverTime) { delay(LOOP_DELAY); Lights::getInstance().updateLEDS(); } - Serial.print(F("PS5 Controller Connected: ")); - Serial.println(ps5.isConnected()); - storeAddress(&addr.toString().c_str()[0], true); - setBuiltInLED(true); // solid blue light when fully paired } - } - // if not connected at this point, no valid controllers have been found - if (!ps5.isConnected()) setBuiltInLED(false); // turn the led off + // if not connected at this point, no valid controllers have been found + if (!ps5.isConnected()) setBuiltInLED(false); // turn the led off + } else { + Serial.println(F("Found no pairable devices.")); + setBuiltInLED(false); + } } else { - Serial.println(F("Found no pairable devices.")); + Serial.println(F("Asynchronous discovery failed.")); setBuiltInLED(false); } - } else { - Serial.println(F("Asynchronous discovery failed.")); - setBuiltInLED(false); + + } else { //! if PAIRING_PIN is LOW, reconnect to previous controller + if (doRePair) { + // see if we have a stored MAC address and try to pair to it + if (addrCharPtr != nullptr) { + Serial.print(F("Connecting to PS5 Controller @ ")); + Serial.println(addrCharPtr); + ps5.begin(addrCharPtr); + int timer = 0; + + // wait until discovery time passes or we connect to a controller + while (timer < discoverTime && !ps5.isConnected()) { + delay(LOOP_DELAY); + timer += LOOP_DELAY; + + // slow blink when searching for previous device + if (timer % (5 * LOOP_DELAY) == 0) { + toggleBuiltInLED(); + } + } + + // return if we get a connection at this point + if (ps5.isConnected()) { + Serial.println(F("PS5 Controller Connected!")); + yeet; + } + } + } } -} \ No newline at end of file diff --git a/src/Pairing/pairing.h b/src/Pairing/pairing.h index a317d3a9..db2e0e90 100644 --- a/src/Pairing/pairing.h +++ b/src/Pairing/pairing.h @@ -2,7 +2,8 @@ #define PAIRING_H #include "PolarRobotics.h" -#define DEFAULT_BT_DISCOVER_TIME 15000 +#define DEFAULT_BT_DISCOVER_TIME 10000 +#define DEFAULT_BT_REPAIR_TIME 1000000 bool addressIsController(const char * addrCharPtr); bool startDiscovery(); void storeAddress(const char *addr, bool clear); diff --git a/src/PolarRobotics.h b/src/PolarRobotics.h index 75f5eb7f..821d19d1 100644 --- a/src/PolarRobotics.h +++ b/src/PolarRobotics.h @@ -46,9 +46,15 @@ // pin for ws2812 LEDs to indicate positions #define LED_PIN 4 + // receiver, tackled, etc... #define TACKLE_PIN 13 +// pairing jumper pin +// jump HIGH to activate pairing discovery +// otherwise robot will only connect to last controller +#define PAIRING_PIN 23 + enum BOT_STATE { PAIRING, CONNECTED, diff --git a/src/main.cpp b/src/main.cpp index 30966701..870e2301 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -65,6 +65,7 @@ void setup() { pinMode(LED_BUILTIN, OUTPUT); pinMode(TACKLE_PIN, INPUT); // Try INPUT_PULLUP + pinMode(PAIRING_PIN, INPUT_PULLDOWN); // PULLDOWN - low until connected to 3v3 // Read robot info from "EEPROM" (ESP32 Preferences) using ConfigManager config.read(); From d3f30f1daa25ff7963be3f54d90859d4f28cf804 Mon Sep 17 00:00:00 2001 From: Corbin <63320116+Tw1ZZLER@users.noreply.github.com> Date: Wed, 13 Nov 2024 21:33:23 -0500 Subject: [PATCH 03/23] Fixed bracket and indentation issues after merge --- src/Pairing/pairing.cpp | 50 +++++++++++++++++++++-------------------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index f0f957e5..66e344b8 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -205,32 +205,33 @@ void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, Serial.print(F(" | ")); Serial.println(device->getRSSI()); - // print out relevant controller details - // reminder that we need to use flash strings whenever possible, so don't try to collapse this - Serial.print(i); - Serial.print(F(" | ")); - Serial.print(addr.toString().c_str()); - Serial.print(F(" | ")); - Serial.print(device->getName().c_str()); - Serial.print(F(" | ")); - Serial.println(device->getRSSI()); - - if (addressIsController(addrCharPtr) || (strcmp(device->getName().c_str(), "Wireless Controller") == 0)) { - Serial.print(F("Connecting to PS5 Controller @ ")); - Serial.println(addrCharPtr); - ps5.begin(addrCharPtr); - while (!ps5.isConnected()) { - toggleBuiltInLED(); // fast blinking when hooked into a device but not yet connected - delay(LOOP_DELAY); - Lights::getInstance().updateLEDS(); + // print out relevant controller details + // reminder that we need to use flash strings whenever possible, so don't try to collapse this + Serial.print(i); + Serial.print(F(" | ")); + Serial.print(addr.toString().c_str()); + Serial.print(F(" | ")); + Serial.print(device->getName().c_str()); + Serial.print(F(" | ")); + Serial.println(device->getRSSI()); + + if (addressIsController(addrCharPtr) || (strcmp(device->getName().c_str(), "Wireless Controller") == 0)) { + Serial.print(F("Connecting to PS5 Controller @ ")); + Serial.println(addrCharPtr); + ps5.begin(addrCharPtr); + while (!ps5.isConnected()) { + toggleBuiltInLED(); // fast blinking when hooked into a device but not yet connected + delay(LOOP_DELAY); + Lights::getInstance().updateLEDS(); + } } - } - // if not connected at this point, no valid controllers have been found - if (!ps5.isConnected()) setBuiltInLED(false); // turn the led off - } else { - Serial.println(F("Found no pairable devices.")); - setBuiltInLED(false); + // if not connected at this point, no valid controllers have been found + if (!ps5.isConnected()) { + Serial.println(F("Found no pairable devices.")); + setBuiltInLED(false); + } + } } } else { Serial.println(F("Asynchronous discovery failed.")); @@ -265,3 +266,4 @@ void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, } } } +} \ No newline at end of file From 3461931acdde41451a6d1214661184882713ef7c Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 2 Dec 2024 20:32:07 -0500 Subject: [PATCH 04/23] functionize pairing actions --- src/Pairing/pairing.cpp | 121 +++++++++++++++++++++++++++++++++++++++- src/Pairing/pairing.h | 2 +- 2 files changed, 119 insertions(+), 4 deletions(-) diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index 66e344b8..ff3ee186 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -130,12 +130,127 @@ void getAddress(const char* &addr) { else addr = &str.c_str()[0]; // get value of char ptr string } +void pairToLastController(int time, const char* &addrCharPtr) { + // see if we have a stored MAC address and try to pair to it + if (addrCharPtr != nullptr) { + Serial.print(F("Connecting to PS5 Controller @ ")); + Serial.println(addrCharPtr); + ps5.begin(addrCharPtr); + int timer = 0; + + // wait until discovery time passes or we connect to a controller + while (timer < time && !ps5.isConnected()) { + delay(LOOP_DELAY); + timer += LOOP_DELAY; + + // slow blink when searching for previous device + if (timer % (5 * LOOP_DELAY) == 0) { + toggleBuiltInLED(); + } + } + + // return if we get a connection at this point + if (ps5.isConnected()) { + Serial.println(F("PS5 Controller Connected!")); + yeet; + } // otherwise look for devices to pair with + } +} + +void searchForNewController(int time, const char* &addrCharPtr) { +// begin broadcasting as "ESP32" as master role + if (!SerialBT.begin("ESP32", true)) { + Serial.println(F("SerialBT failed!")); // function returns false if failed + abort(); + } + SerialBT.enableSSP(); // according to SRC of this code, doesn't seem to change anything + + Serial.println(F("Searching for devices...")); + BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! + + // Beginning of Asynchronous Discovery Process + if (startDiscovery()) { + int timer = 0; + + // recall foundController is set by the callback in `startDiscovery` when a valid PS5 controller is found + while (timer < time && !foundController) { + delay(LOOP_DELAY); + timer += LOOP_DELAY; + Lights::getInstance().updateLEDS(); + + // double blink when in pairing mode like PS5 controller + // at: 300/400, 600/700 + if ((timer % 1000) % (7 * LOOP_DELAY) == 0) + toggleBuiltInLED(); + else if ((timer % 1000) % (4 * LOOP_DELAY) == 0) + toggleBuiltInLED(); + else if ((timer % 1000) % (3 * LOOP_DELAY) == 0 && + (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 + toggleBuiltInLED(); + } + + Serial.println(F("Stopping discoverAsync... ")); + SerialBT.discoverAsyncStop(); + Serial.println(F("discoverAsync stopped")); + delay(5000); //! why is this delay here? does removing it affect anything? this was in the original code, I must never have noticed it. + + // If we find devices, list them and try to pair if it is a valid controller. + if(btDeviceList->getCount() > 0) { + BTAddress addr; + int channel = 0; + Serial.println(F("Found devices:")); + for (int i = 0; i < btDeviceList->getCount(); i++) { + BTAdvertisedDevice* device = btDeviceList->getDevice(i); + addr = device->getAddress(); + auto name = device->getName().c_str(); // get name to print and check + auto addrStr = addr.toString().c_str(); // std::string doesn't work with Serial.print for some reason + // ps5.begin requires a const char*, so get memory address of string/char array + addrCharPtr = &addr.toString().c_str()[0]; // declared at top of function + + // print out relevant controller details + // reminder that we need to use flash strings whenever possible, so don't try to collapse this + Serial.print(i); + Serial.print(F(" | ")); + Serial.print(addr.toString().c_str()); + Serial.print(F(" | ")); + Serial.print(device->getName().c_str()); + Serial.print(F(" | ")); + Serial.println(device->getRSSI()); + + if (addressIsController(addrCharPtr) || (strcmp(device->getName().c_str(), "Wireless Controller") == 0)) { + Serial.print(F("Connecting to PS5 Controller @ ")); + Serial.println(addrCharPtr); + ps5.begin(addrCharPtr); + while (!ps5.isConnected()) { + toggleBuiltInLED(); // fast blinking when hooked into a device but not yet connected + delay(LOOP_DELAY); + Lights::getInstance().updateLEDS(); + } + Serial.print(F("PS5 Controller Connected: ")); + Serial.println(ps5.isConnected()); + storeAddress(&addr.toString().c_str()[0], true); + setBuiltInLED(true); // solid blue light when fully paired + } + } + + // if not connected at this point, no valid controllers have been found + if (!ps5.isConnected()) setBuiltInLED(false); // turn the led off + } else { + Serial.println(F("Found no pairable devices.")); + setBuiltInLED(false); + } + } else { + Serial.println(F("Asynchronous discovery failed.")); + setBuiltInLED(false); + } +} + /// @brief Search for PS5 Controllers and pair to the first one found /// @param doRePair whether or not to search for the controller whose MAC address is stored in non-volatile memory, default true -/// @param discoverTime the time limit to repair to existing devices, or search for new devices, in milliseconds -void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int repairTime = DEFAULT_BT_REPAIR_TIME) { +/// @param discoverTime the time limit to search for new devices, in milliseconds +/// @param rePairTime the time limit to repair to existing devices, in milliseconds +void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int rePairTime = DEFAULT_BT_REPAIR_TIME) { Serial.begin(115200); - // pinMode(LED_BUILTIN, OUTPUT); // if we just returned a char*, it would be deleted and point to nowhere useful // so we have to pass in and mutate a (reference to a) char array. diff --git a/src/Pairing/pairing.h b/src/Pairing/pairing.h index db2e0e90..7c81406a 100644 --- a/src/Pairing/pairing.h +++ b/src/Pairing/pairing.h @@ -8,6 +8,6 @@ bool addressIsController(const char * addrCharPtr); bool startDiscovery(); void storeAddress(const char *addr, bool clear); void getAddress(const char *&addr); -void activatePairing(bool doRePair = true, int discoverTime = DEFAULT_BT_DISCOVER_TIME); +void activatePairing(bool doRePair = true, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int rePairTime = DEFAULT_BT_REPAIR_TIME); #endif // PAIRING_H \ No newline at end of file From 1f6377828539b6c42818f685e827c31b693cbea9 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 2 Dec 2024 20:41:33 -0500 Subject: [PATCH 05/23] spacing --- src/Pairing/pairing.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index ff3ee186..b5a946d2 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -158,7 +158,7 @@ void pairToLastController(int time, const char* &addrCharPtr) { } void searchForNewController(int time, const char* &addrCharPtr) { -// begin broadcasting as "ESP32" as master role + // begin broadcasting as "ESP32" as master role if (!SerialBT.begin("ESP32", true)) { Serial.println(F("SerialBT failed!")); // function returns false if failed abort(); @@ -166,7 +166,7 @@ void searchForNewController(int time, const char* &addrCharPtr) { SerialBT.enableSSP(); // according to SRC of this code, doesn't seem to change anything Serial.println(F("Searching for devices...")); - BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! + BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! // Beginning of Asynchronous Discovery Process if (startDiscovery()) { @@ -185,7 +185,7 @@ void searchForNewController(int time, const char* &addrCharPtr) { else if ((timer % 1000) % (4 * LOOP_DELAY) == 0) toggleBuiltInLED(); else if ((timer % 1000) % (3 * LOOP_DELAY) == 0 && - (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 + (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 toggleBuiltInLED(); } @@ -195,7 +195,7 @@ void searchForNewController(int time, const char* &addrCharPtr) { delay(5000); //! why is this delay here? does removing it affect anything? this was in the original code, I must never have noticed it. // If we find devices, list them and try to pair if it is a valid controller. - if(btDeviceList->getCount() > 0) { + if (btDeviceList->getCount() > 0) { BTAddress addr; int channel = 0; Serial.println(F("Found devices:")); From 8053d96225ae783bd0e34dd8de73db78a90ac6c4 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 2 Dec 2024 21:02:05 -0500 Subject: [PATCH 06/23] update pairing, integrate optional pin pairing with define --- src/Pairing/depairingStation.cpp | 2 +- src/Pairing/pairing.cpp | 138 ++++--------------------------- src/Pairing/pairing.h | 12 +-- src/PolarRobotics.h | 4 + 4 files changed, 27 insertions(+), 129 deletions(-) diff --git a/src/Pairing/depairingStation.cpp b/src/Pairing/depairingStation.cpp index 8cac47f2..7277ded6 100644 --- a/src/Pairing/depairingStation.cpp +++ b/src/Pairing/depairingStation.cpp @@ -49,7 +49,7 @@ void setup() { pinMode(LED_BUILTIN, OUTPUT); setBuiltInLED(false); - activatePairing(false, 1048576); // easy power of two, long enough that it should be fine + activatePairing(1000000, 0); // search for new devices forever, do not re-pair ever // Serial.print(F("\r\nConnected")); diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index b5a946d2..36fb1126 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -63,7 +63,7 @@ bool doDiscovery = false; // default false, only true if PAIRING_PIN jumped to H // MAC Addresses to match to PS5 Controllers const char* macTest = "bc:c7:46:03"; // length 11 const char* macTest2 = "bc:c7:46:04"; // length 11 -const char* RhysController = "10:18:49:57"; // length 17 "10:18:49:57:49:ef" +const char* RhysController = "10:18:49:57"; // length 11, full: "10:18:49:57:49:ef" /// @brief Detects if a given MAC Address is considered a PS5 Controller /// @param addrCharPtr the address to test (C string) @@ -130,6 +130,9 @@ void getAddress(const char* &addr) { else addr = &str.c_str()[0]; // get value of char ptr string } +/// @brief Pairs to controller whose address is stored in ESP32 Preferences +/// @param time time to wait before terminating re-pairing process +/// @param addrCharPtr address to connect to, read but not written to void pairToLastController(int time, const char* &addrCharPtr) { // see if we have a stored MAC address and try to pair to it if (addrCharPtr != nullptr) { @@ -153,10 +156,13 @@ void pairToLastController(int time, const char* &addrCharPtr) { if (ps5.isConnected()) { Serial.println(F("PS5 Controller Connected!")); yeet; - } // otherwise look for devices to pair with + } } } +/// @brief Looks for new controllers to pair to +/// @param time time to wait before terminating discovery process +/// @param addrCharPtr address to connect to, written to and read void searchForNewController(int time, const char* &addrCharPtr) { // begin broadcasting as "ESP32" as master role if (!SerialBT.begin("ESP32", true)) { @@ -246,10 +252,9 @@ void searchForNewController(int time, const char* &addrCharPtr) { } /// @brief Search for PS5 Controllers and pair to the first one found -/// @param doRePair whether or not to search for the controller whose MAC address is stored in non-volatile memory, default true /// @param discoverTime the time limit to search for new devices, in milliseconds /// @param rePairTime the time limit to repair to existing devices, in milliseconds -void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int rePairTime = DEFAULT_BT_REPAIR_TIME) { +void activatePairing(int discoverTime, int rePairTime) { Serial.begin(115200); // if we just returned a char*, it would be deleted and point to nowhere useful @@ -257,128 +262,17 @@ void activatePairing(bool doRePair, int discoverTime = DEFAULT_BT_DISCOVER_TIME, const char* addrCharPtr = nullptr; getAddress(addrCharPtr); + #if USE_PIN_PAIRING // only search for new controller when pairing pin is high Serial.print(F("PAIRING PIN ")); Serial.println(digitalRead(PAIRING_PIN)); if (digitalRead(PAIRING_PIN) == HIGH) { //! search for new devices - // begin broadcasting as "ESP32" as master role - if (!SerialBT.begin("ESP32", true)) { - Serial.println(F("SerialBT failed!")); // function returns false if failed - abort(); - } - SerialBT.enableSSP(); // according to SRC of this code, doesn't seem to change anything - - Serial.println(F("Searching for devices...")); - BTScanResults* btDeviceList = SerialBT.getScanResults(); // may be accessing from different threads! - - // Beginning of Asynchronous Discovery Process - if (startDiscovery()) { - int timer = 0; - - // recall foundController is set by the callback in `startDiscovery` when a valid PS5 controller is found - while (timer < discoverTime && !foundController) { - delay(LOOP_DELAY); - timer += LOOP_DELAY; - Lights::getInstance().updateLEDS(); - - // double blink when in pairing mode like PS5 controller - // at: 300/400, 600/700 - if ((timer % 1000) % (7 * LOOP_DELAY) == 0) - toggleBuiltInLED(); - else if ((timer % 1000) % (4 * LOOP_DELAY) == 0) - toggleBuiltInLED(); - else if ((timer % 1000) % (3 * LOOP_DELAY) == 0 && - (timer % 1000) % (9 * LOOP_DELAY) != 0) // also does 600 - toggleBuiltInLED(); - } - - Serial.println(F("Stopping discoverAsync... ")); - SerialBT.discoverAsyncStop(); - Serial.println(F("discoverAsync stopped")); - delay(5000); //! why is this delay here? does removing it affect anything? this was in the original code, I must never have noticed it. - - // If we find devices, list them and try to pair if it is a valid controller. - if(btDeviceList->getCount() > 0) { - BTAddress addr; - int channel = 0; - Serial.println(F("Found devices:")); - for (int i = 0; i < btDeviceList->getCount(); i++) { - BTAdvertisedDevice* device = btDeviceList->getDevice(i); - addr = device->getAddress(); - auto name = device->getName().c_str(); // get name to print and check - auto addrStr = addr.toString().c_str(); // std::string doesn't work with Serial.print for some reason - // ps5.begin requires a const char*, so get memory address of string/char array - addrCharPtr = &addr.toString().c_str()[0]; // declared at top of function - - // print out relevant controller details - // reminder that we need to use flash strings whenever possible, so don't try to collapse this - Serial.print(i); - Serial.print(F(" | ")); - Serial.print(addr.toString().c_str()); - Serial.print(F(" | ")); - Serial.print(device->getName().c_str()); - Serial.print(F(" | ")); - Serial.println(device->getRSSI()); - - // print out relevant controller details - // reminder that we need to use flash strings whenever possible, so don't try to collapse this - Serial.print(i); - Serial.print(F(" | ")); - Serial.print(addr.toString().c_str()); - Serial.print(F(" | ")); - Serial.print(device->getName().c_str()); - Serial.print(F(" | ")); - Serial.println(device->getRSSI()); - - if (addressIsController(addrCharPtr) || (strcmp(device->getName().c_str(), "Wireless Controller") == 0)) { - Serial.print(F("Connecting to PS5 Controller @ ")); - Serial.println(addrCharPtr); - ps5.begin(addrCharPtr); - while (!ps5.isConnected()) { - toggleBuiltInLED(); // fast blinking when hooked into a device but not yet connected - delay(LOOP_DELAY); - Lights::getInstance().updateLEDS(); - } - } - - // if not connected at this point, no valid controllers have been found - if (!ps5.isConnected()) { - Serial.println(F("Found no pairable devices.")); - setBuiltInLED(false); - } - } - } - } else { - Serial.println(F("Asynchronous discovery failed.")); - setBuiltInLED(false); - } - + searchForNewController(discoverTime, addrCharPtr); } else { //! if PAIRING_PIN is LOW, reconnect to previous controller - if (doRePair) { - // see if we have a stored MAC address and try to pair to it - if (addrCharPtr != nullptr) { - Serial.print(F("Connecting to PS5 Controller @ ")); - Serial.println(addrCharPtr); - ps5.begin(addrCharPtr); - int timer = 0; - - // wait until discovery time passes or we connect to a controller - while (timer < discoverTime && !ps5.isConnected()) { - delay(LOOP_DELAY); - timer += LOOP_DELAY; - - // slow blink when searching for previous device - if (timer % (5 * LOOP_DELAY) == 0) { - toggleBuiltInLED(); - } - } - - // return if we get a connection at this point - if (ps5.isConnected()) { - Serial.println(F("PS5 Controller Connected!")); - yeet; - } - } - } + pairToLastController(rePairTime, addrCharPtr); } + #else // always try to re-pair first, then search for new controller if none is found + pairToLastController(rePairTime, addrCharPtr); + searchForNewController(discoverTime, addrCharPtr); + #endif } \ No newline at end of file diff --git a/src/Pairing/pairing.h b/src/Pairing/pairing.h index 7c81406a..8ec7bd09 100644 --- a/src/Pairing/pairing.h +++ b/src/Pairing/pairing.h @@ -2,12 +2,12 @@ #define PAIRING_H #include "PolarRobotics.h" -#define DEFAULT_BT_DISCOVER_TIME 10000 -#define DEFAULT_BT_REPAIR_TIME 1000000 -bool addressIsController(const char * addrCharPtr); +#define DEFAULT_BT_DISCOVER_TIME 10000 // milliseconds +#define DEFAULT_BT_REPAIR_TIME 1000000 // milliseconds +bool addressIsController(const char* addrCharPtr); bool startDiscovery(); -void storeAddress(const char *addr, bool clear); -void getAddress(const char *&addr); -void activatePairing(bool doRePair = true, int discoverTime = DEFAULT_BT_DISCOVER_TIME, int rePairTime = DEFAULT_BT_REPAIR_TIME); +void storeAddress(const char* addr, bool clear); +void getAddress(const char* &addr); +void activatePairing(int discoverTime = DEFAULT_BT_DISCOVER_TIME, int rePairTime = DEFAULT_BT_REPAIR_TIME); #endif // PAIRING_H \ No newline at end of file diff --git a/src/PolarRobotics.h b/src/PolarRobotics.h index 821d19d1..2e532eff 100644 --- a/src/PolarRobotics.h +++ b/src/PolarRobotics.h @@ -20,6 +20,10 @@ #define PR_CODEBASE_VERSION "PR_CODEBASE_VERSION not defined!" #endif +#ifndef USE_PIN_PAIRING +#define USE_PIN_PAIRING false +#endif + // [PIN DECLARATIONS] // please follow: // https://docs.google.com/spreadsheets/d/17pdff4T_3GTAkoctwm2IMg07Znoo-iJkyDGN5CqXq3w/edit#gid=0 From 82f17c5dfdd834ca44123351baac19f9a06950f8 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Wed, 29 Jan 2025 19:55:51 -0500 Subject: [PATCH 07/23] fix corbin's mistake --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 444ba0e7..678443ad 100644 --- a/README.md +++ b/README.md @@ -59,4 +59,4 @@ src │ ├── ReadBotInfo.cpp │ └── WriteBotInfo.cpp └── main.cpp -``` +``` \ No newline at end of file From 03d73549350b200c41259a05172a52a2ef17079e Mon Sep 17 00:00:00 2001 From: Corbin <63320116+Tw1ZZLER@users.noreply.github.com> Date: Wed, 13 Nov 2024 20:19:15 -0500 Subject: [PATCH 08/23] Popped changes from dev/data-aq into dev/experimental+data-aq --- src/Drive/Drive.cpp | 67 ------------------------- src/Utilities/PrintSerial.cpp | 92 +++++++++++++++++++++++++++++++++++ src/Utilities/PrintSerial.h | 34 +++++++++++++ src/main.cpp | 24 ++++++++- 4 files changed, 149 insertions(+), 68 deletions(-) create mode 100644 src/Utilities/PrintSerial.cpp create mode 100644 src/Utilities/PrintSerial.h diff --git a/src/Drive/Drive.cpp b/src/Drive/Drive.cpp index 889c3764..884835a8 100644 --- a/src/Drive/Drive.cpp +++ b/src/Drive/Drive.cpp @@ -328,73 +328,6 @@ void Drive::printSetup() { Serial.print(F("\n")); } -/** - * prints the internal variables to the serial monitor in a clean format, - * this function exists out of pure laziness to not have to comment out all the print statments - * @author - * Updated: -*/ -void Drive::printDebugInfo() { - Serial.print(F("L_Hat_Y: ")); - Serial.print(stickForwardRev); - Serial.print(F(" R_HAT_X: ")); - Serial.print(stickTurn); - - // Serial.print(F(" | Turn: ")); - // Serial.print(lastTurnPwr); - - // Serial.print(F(" | Left ReqPwr: ")); - // Serial.print(requestedMotorPower[0]); - // Serial.print(F(" Right ReqPwr: ")); - // Serial.print(requestedMotorPower[1]); - - Serial.print(F(" | Omega: ")); - Serial.print(omega); - - Serial.print(F(" omega_L: ")); - Serial.print(omega_L); - Serial.print(F(" omega_R: ")); - Serial.print(omega_R); - - // Serial.print(F(" lastRampTime ")); - // Serial.print(lastRampTime[0]); - // Serial.print(F(" requestedPower ")); - // Serial.print(requestedPower); - // Serial.print(F(" current ")); - // Serial.print(currentRampPower[0]); - // Serial.print(F(" requestedPower - currentRampPower ")); - // Serial.println(requestedPower - currentRampPower[mtr], 10); - - Serial.print(F(" Left Motor: ")); - Serial.print(requestedMotorPower[0]); - Serial.print(F(" Right: ")); - Serial.print(requestedMotorPower[1]); - - //Serial.print(F(" scaledSensitiveTurn: ")); - //Serial.print(scaledSensitiveTurn); - - Serial.print(F("\n")); -} -/** - * @brief Prints variables to the serial monitor in a csv format - * This function is important for data acquisition - * The options below are configurable, change them as you need - * Remember to adhere to printing guidelines under PR-Docs - * @author Corbin Hibler - * Updated: 2023-10-30 -*/ -void Drive::printCsvInfo() { - Serial.print(F("header1,")); // name of value to be used as header - Serial.print(1); // variable you want to track - Serial.print(F(",header2,")); - Serial.print(2); - Serial.print(F(",header3,")); - Serial.print(3); - Serial.print(F(",header4,")); - Serial.print(4); - Serial.print(F(",header5,")); - Serial.println(5); // last line is -ALWAYS- println or else the python script will break -} /** * @brief updates the motors after calling all the functions to generate * turning and scaling motor values, the intention of this is so the diff --git a/src/Utilities/PrintSerial.cpp b/src/Utilities/PrintSerial.cpp new file mode 100644 index 00000000..b3b2add4 --- /dev/null +++ b/src/Utilities/PrintSerial.cpp @@ -0,0 +1,92 @@ +#include +#include +#include "PrintSerial.h" + +PrintSerial::PrintSerial() { + serialHeaders = {"forwardPower","turnPower","header3","header4","header5"}; +} + +void PrintSerial::updateValues() { + PrintSerial::serialValues = {drive->getForwardPower(), 2.0, 3.0, 4.0, 5.0}; +} + +void PrintSerial::setDriveObj(Drive* driveObj) { + PrintSerial::drive = driveObj; +} + +/** + * @brief prints the internal variables to the serial monitor in a clean and easy to read format + * @author Everybody + * @date 2024-02-12 +*/ +void PrintSerial::printDebugInfo() { + // Serial.print(F("L_Hat_Y: ")); + // Serial.print(stickForwardRev); + // Serial.print(F(" R_HAT_X: ")); + // Serial.print(stickTurn); + + // Serial.print(F(" | Turn: ")); + // Serial.print(lastTurnPwr); + + // Serial.print(F(" | Left ReqPwr: ")); + // Serial.print(requestedMotorPower[0]); + // Serial.print(F(" Right ReqPwr: ")); + // Serial.print(requestedMotorPower[1]); + + // Serial.print(F(" | Omega: ")); + // Serial.print(omega); + + // Serial.print(F(" omega_L: ")); + // Serial.print(omega_L); + // Serial.print(F(" omega_R: ")); + // Serial.print(omega_R); + + // Serial.print(F(" lastRampTime ")); + // Serial.print(lastRampTime[0]); + // Serial.print(F(" requestedPower ")); + // Serial.print(requestedPower); + // Serial.print(F(" current ")); + // Serial.print(currentRampPower[0]); + // Serial.print(F(" requestedPower - currentRampPower ")); + // Serial.println(requestedPower - currentRampPower[mtr], 10); + + // Serial.print(F(" Left Motor: ")); + // Serial.print(requestedMotorPower[0]); + // Serial.print(F(" Right: ")); + // Serial.print(requestedMotorPower[1]); + + //Serial.print(F(" scaledSensitiveTurn: ")); + //Serial.print(scaledSensitiveTurn); + + // Serial.print(F("\n")); +} + +/** + * @brief Prints variables to the serial monitor in a csv format + * This function is important for data acquisition + * The options below are configurable, change them as you need + * Remember to adhere to printing guidelines under PR-Docs + * @param values A vector of float values that will be sent to serial monitor + * @param headers A vector of header strings that will be sent to serial monitor + * @author Corbin Hibler + * Updated: 2024-02-12 +*/ +void PrintSerial::printCsvInfo() { + for (int i = 0; i < serialValues.size(); i++) { + if (i == 0) { + String header = serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.print(serialValues[i]); + } + else if (i < (serialValues.size() - 1)) { + String header = "," + serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.print(serialValues[i]); + } + else { + String header = "," + serialHeaders[i] + ","; + Serial.print(header.c_str()); + Serial.println(serialValues[i]); + } + } +} \ No newline at end of file diff --git a/src/Utilities/PrintSerial.h b/src/Utilities/PrintSerial.h new file mode 100644 index 00000000..b830ec48 --- /dev/null +++ b/src/Utilities/PrintSerial.h @@ -0,0 +1,34 @@ +#pragma once + +#ifndef PRINTSERIAL_H +#define PRINTSERIAL_H + +#include +#include +#include + +/** + * @author Corbin Hibler + * @date 2024-02-12 + * @brief Prints information to serial in various formats + */ +class PrintSerial { + private: + PrintSerial(); + Drive* drive; + std::vector serialValues; + std::vector serialHeaders; + public: + static PrintSerial& getInstance() { + static PrintSerial instance; + return instance; + } + PrintSerial(const PrintSerial& obj) = delete; // delete copy constructor + void operator=(PrintSerial const&) = delete; // delete set operator + void setDriveObj(Drive* driveObj); + void updateValues(); + void printDebugInfo(); + void printCsvInfo(); +}; + +#endif // PRINTSERIAL_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 30966701..03198492 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -9,11 +9,13 @@ #include #include // ESP PS5 library, access using global instance `ps5` +#include // Custom Polar Robotics Libraries: #include #include #include +#include // Robot Includes #include @@ -34,6 +36,7 @@ Robot* robot = nullptr; // subclassed if needed Drive* drive = nullptr; // subclassed if needed Lights& lights = Lights::getInstance(); +PrintSerial& printserial = PrintSerial::getInstance(); //* How to use subclasses: ((SubclassName*) robot)->function() //! You must downcast each time you use a special function @@ -139,10 +142,12 @@ void setup() { //! Activate Pairing Process: this code is BLOCKING, not instantaneous activatePairing(); - + // Once paired, set lights to appropriate status lights.setLEDStatus(Lights::PAIRED); + printserial.setDriveObj(drive); + ps5.attachOnConnect(onConnection); ps5.attachOnDisconnect(onDisconnect); } @@ -225,6 +230,23 @@ void loop() { lights.updateTime = millis(); } } + + //* Update the motors based on the inputs from the controller + //* Can change functionality depending on subclass, like robot.action() + drive->update(); + + //* Data Acquisition *// + // Include values you want to monitor + //std::vector serialValues = {1.0, 2.0, 3.0, 4.0, 5.0}; + // Include headers of the values + //std::vector serialHeaders = {"header1","header2","header3","header4","header5"}; + + // printserial.printDebugInfo(serialValues); // prints info to serial monitor in a clean format (not usable by scripts) + printserial.updateValues(); + printserial.printCsvInfo(); // prints info to serial monitor in a csv (comma separated value) format + + if (lights.returnStatus() == lights.DISCO) + lights.updateLEDS(); //! Performs all special robot actions depending on the instantiated Robot subclass robot->action(); From 007e083ad4337470882e90f7b110e55f40296185 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Wed, 29 Jan 2025 20:53:39 -0500 Subject: [PATCH 09/23] fix build error (virtual fn def missing) --- src/Drive/Drive.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Drive/Drive.h b/src/Drive/Drive.h index 67046505..c55048ba 100644 --- a/src/Drive/Drive.h +++ b/src/Drive/Drive.h @@ -114,8 +114,8 @@ class Drive { void generateMotionValues(float tankModePct = TANK_MODE_PCT); virtual void update(); void printSetup(); - virtual void printDebugInfo(); - virtual void printCsvInfo(); + // virtual void printDebugInfo(); + // virtual void printCsvInfo(); int getMotorWifiValue(int motorRequested); //* The following variables are initialized in the constructor From bcd637e7ced26b103278967faab75e86c8db719d Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 3 Feb 2025 19:31:15 -0500 Subject: [PATCH 10/23] change `runningback` to `running_back` --- src/Drive/Drive.cpp | 2 +- src/Utilities/BotTypes.cpp | 2 +- src/Utilities/BotTypes.h | 4 ++-- src/main.cpp | 6 +++--- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Drive/Drive.cpp b/src/Drive/Drive.cpp index 884835a8..e72f4aad 100644 --- a/src/Drive/Drive.cpp +++ b/src/Drive/Drive.cpp @@ -339,7 +339,7 @@ void Drive::printSetup() { */ void Drive::update() { // !TODO Clean up when robots are rewired: - if (this->botType == runningback) { + if (this->botType == running_back) { // Generate turning motion generateMotionValues(); //printDebugInfo(); diff --git a/src/Utilities/BotTypes.cpp b/src/Utilities/BotTypes.cpp index a5929707..debd1073 100644 --- a/src/Utilities/BotTypes.cpp +++ b/src/Utilities/BotTypes.cpp @@ -5,7 +5,7 @@ constexpr Pair botTypeStrings[NUM_POSITIONS] = { { lineman, "lineman" }, { receiver, "receiver" }, - { runningback, "runningback" }, + { running_back, "running_back" }, { center, "center" }, { kicker, "kicker" }, { mecanum_center, "mecanum_center" }, diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 30aed855..8ea21187 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -28,7 +28,7 @@ typedef enum { lineman, receiver, - runningback, + running_back, center, kicker, mecanum_center, @@ -107,7 +107,7 @@ constexpr bot_config_t botConfigArray[NUM_BOTS] = { { 6, ">=", lineman, { small_ampflow, 1.0f, 10.00f, 6.00f, 27.00f }}, //* 6: >= { 7, "32.2", receiver, { small_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 7: 32.2 { 8, "9.8", lineman, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 8: 9.8 - { 9, "c", runningback, { falcon, 0.4f, 8.00f, 6.00f, 36.00f }}, //* 9: c + { 9, "c", running_back, { falcon, 0.4f, 8.00f, 6.00f, 36.00f }}, //* 9: c { 10, "phi", center, { small_ampflow, 0.6f, 11.50f, 9.00f, 36.00f }}, //* 10: Φ { 11, "inf", quarterback_old, { small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f }}, //* 11: ∞ { 12, "theta", kicker, { small_ampflow, 0.5f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ diff --git a/src/main.cpp b/src/main.cpp index 50420a6b..aed0c02b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -103,9 +103,9 @@ void setup() { drive = new Drive(center, driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; - case runningback: + case running_back: robot = new Lineman(); - drive = new Drive(runningback, driveParams); + drive = new Drive(running_back, driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; case quarterback_turret: @@ -203,7 +203,7 @@ void loop() { lights.togglePosition(); // If the robot is able to hold the ball, it is able to be tackled: - if (robotType == receiver || robotType == quarterback_old || robotType == runningback) { + if (robotType == receiver || robotType == quarterback_old || robotType == running_back) { // if the lights are in the home or away state and the tackle pin goes low (tackle sensor is active low), enter the tackled state if ((lights.returnStatus() == Lights::HOME || lights.returnStatus() == Lights::AWAY) && digitalRead(TACKLE_PIN) == LOW) { lights.setLEDStatus(Lights::TACKLED); From 1460b8db30bd44856772a5acd58b071f2a0ebfcd Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 3 Feb 2025 19:31:27 -0500 Subject: [PATCH 11/23] update comment for qb name --- src/Utilities/BotTypes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 8ea21187..213e0849 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -112,8 +112,8 @@ constexpr bot_config_t botConfigArray[NUM_BOTS] = { { 11, "inf", quarterback_old, { small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f }}, //* 11: ∞ { 12, "theta", kicker, { small_ampflow, 0.5f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ { 13, "y=x", mecanum_center, { mecanum, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 13: y=x - { 14, "qb_base", quarterback_base, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 14: unassigned - { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: unassigned + { 14, "qb_base", quarterback_base, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 14: beta (bottom) + { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: beta (top) { 16, "l-man-v1", lineman, { small_12v, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 16: generic lineman V1 { 17, "420", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 17: 420 { 18, "24", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 18: 24 From efb19103815ddfd06d929720662ffbc8c79f2a92 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 3 Feb 2025 20:23:21 -0500 Subject: [PATCH 12/23] updates for old/new kicker --- src/Robot/Kicker.cpp | 14 +++++---- src/Robot/Kicker.h | 3 +- src/Robot/KickerOld.cpp | 51 +++++++++++++++++++++++++++++++ src/Robot/KickerOld.h | 27 +++++++++++++++++ src/Utilities/BotTypes.cpp | 3 +- src/Utilities/BotTypes.h | 61 +++++++++++++++++++++----------------- src/main.cpp | 5 ++++ 7 files changed, 128 insertions(+), 36 deletions(-) create mode 100644 src/Robot/KickerOld.cpp create mode 100644 src/Robot/KickerOld.h diff --git a/src/Robot/Kicker.cpp b/src/Robot/Kicker.cpp index cb36330d..ce7607e1 100644 --- a/src/Robot/Kicker.cpp +++ b/src/Robot/Kicker.cpp @@ -1,3 +1,11 @@ +//! New kicker code isn't really functional (as of 2025-02-03) +//* Need to decide control scheme and adjust action() appropriately. + +// Max's Notes from last time he talked to trent (like 2024-12-04 or something): +// - Startup: home to limit switch, wait half a second, then go back to ~135 degrees from the limit switch +// - Limit switch represents fire position. +// - Rotate towards back of the robot. + #include "Kicker.h" /** @@ -10,7 +18,7 @@ * then fall back a certain number of degrees (~15 degrees for now). * * @param kickerPin The pin of the kicker arm's motor - * @param limitswitchPin the pin of the kicker arm's limit switch + * @param limitSwitchPin the pin of the kicker arm's limit switch * @param kickerEncoderPinA The pin of signal A from the encoder * @param kickerEncoderPinB THe pin of signal B from the encoder */ @@ -55,10 +63,6 @@ void Kicker::kickerEncoderISR() { /** * @brief Kicker Action - * - * Manually control the motor using the Triangle and X (cross) buttons. - * Triangle for winding - * Cross for unwinding */ void Kicker::action() { // Control the motor on the kicker manually diff --git a/src/Robot/Kicker.h b/src/Robot/Kicker.h index da4e6634..61780cd2 100644 --- a/src/Robot/Kicker.h +++ b/src/Robot/Kicker.h @@ -16,7 +16,7 @@ #define KICKER_ENABLE_DB_DELAY 100L /** - * @brief Kicker Class + * @brief Kicker V2 Class * * Contains logic for the operation of the encoder, motorized kicking arm, and adjusting the angle * automatically upon startup. Other features include manual control of kicking arm in case the @@ -47,7 +47,6 @@ class Kicker : public Robot { ); void action() override; //! robot subclass must override action void enable(); - void test(); void turnForward(); void turnReverse(); void stop(); diff --git a/src/Robot/KickerOld.cpp b/src/Robot/KickerOld.cpp new file mode 100644 index 00000000..f45dbff3 --- /dev/null +++ b/src/Robot/KickerOld.cpp @@ -0,0 +1,51 @@ +#include "KickerOld.h" + +KickerOld::KickerOld(uint8_t kickerPin) { + enabled = false; + this->kickerPin = kickerPin; + windupMotor.setup(kickerPin); +} + +void KickerOld::action() { + // Control the motor on the kicker + if (ps5.Triangle()) + turnForward(); + else if (ps5.Cross()) + turnReverse(); + else + stop(); +} + +void KickerOld::enable() { + enabled = true; +} + +void KickerOld::test() { + if (enabled) { + windupMotor.write(-1); //clockwise + delay(3000); + windupMotor.write(0); //stop + delay(1000); + windupMotor.write(1); //counter-clockwise + delay(3000); + windupMotor.write(0); //stop + } +} + +void KickerOld::turnForward() { + if (enabled) { + windupMotor.write(-0.5); + } +} + +void KickerOld::turnReverse() { + if (enabled) { + windupMotor.write(0.5); + } +} + +void KickerOld::stop() { + if (enabled) { + windupMotor.write(0); + } +} \ No newline at end of file diff --git a/src/Robot/KickerOld.h b/src/Robot/KickerOld.h new file mode 100644 index 00000000..ef675d42 --- /dev/null +++ b/src/Robot/KickerOld.h @@ -0,0 +1,27 @@ +#ifndef KICKER_OLD_H +#define KICKER_OLD_H + +#include +#include +#include // ESP PS5 library, access using global instance `ps5` + +/** + * @brief Old Kicker header file + * @authors Andrew Nelson + */ +class KickerOld : public Robot { + private: + bool enabled; // safety feature + uint8_t kickerPin; + MotorControl windupMotor; + public: + KickerOld(uint8_t kickerPin); + void action() override; //! robot subclass must override action + void enable(); + void test(); + void turnForward(); + void turnReverse(); + void stop(); +}; + +#endif // KICKER_OLD_H \ No newline at end of file diff --git a/src/Utilities/BotTypes.cpp b/src/Utilities/BotTypes.cpp index debd1073..4c0ee50f 100644 --- a/src/Utilities/BotTypes.cpp +++ b/src/Utilities/BotTypes.cpp @@ -5,9 +5,10 @@ constexpr Pair botTypeStrings[NUM_POSITIONS] = { { lineman, "lineman" }, { receiver, "receiver" }, - { running_back, "running_back" }, + { running_back, "running_back" }, { center, "center" }, { kicker, "kicker" }, + { kicker_old, "kicker_old" }, { mecanum_center, "mecanum_center" }, { quarterback_old, "quarterback_old" }, { quarterback_base, "quarterback_base" }, diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 213e0849..a33f0b1b 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -8,7 +8,7 @@ #include #include -#define NUM_POSITIONS 9 // number of members of eBOT_TYPE +#define NUM_POSITIONS 10 // number of members of BotType enum /** BotType * enum for the possible positions a robot can have on the field @@ -20,10 +20,11 @@ * 2: Runningback * 3: Center * 4: Kicker - * 5: Mecanum Center - * 6: Old Quarterback - * 7: Quarterback base - * 8: Quarterback turret + * 5: Old Kicker + * 6: Mecanum Center + * 7: Old Quarterback + * 8: Quarterback V3 Base + * 9: Quarterback V3 Turret */ typedef enum { lineman, @@ -31,6 +32,7 @@ typedef enum { running_back, center, kicker, + kicker_old, mecanum_center, quarterback_old, quarterback_base, @@ -61,7 +63,7 @@ typedef struct BotConfig { // BotType secondary_type; } bot_config_t; -#define NUM_BOTS 20 +#define NUM_BOTS 21 // Bot Aliases #define BOT_IPP 0 @@ -82,7 +84,7 @@ typedef struct BotConfig { #define BOT_QB 11 #define BOT_QB_OLD 11 #define BOT_THETA 12 -#define BOT_KICKER 12 +#define BOT_OLD_KICKER 12 #define BOT_MC 13 #define BOT_MECANUM_CENTER 13 #define BOT_QB_BASE 14 @@ -93,31 +95,34 @@ typedef struct BotConfig { #define BOT_420 17 #define BOT_24 18 #define BOT_25 19 +#define BOT_TAU 20 +#define BOT_KICKER 20 // PRESET BOT CONFIGURATIONS, MUST MATCH: // https://docs.google.com/spreadsheets/d/1DswoEAcry9L9t_4ouKL3mXFgDMey4KkjEPFXULQxMEQ/edit#gid=0 constexpr bot_config_t botConfigArray[NUM_BOTS] = { -// idx bot_name bot_type motor_type gear_ratio wheel_base r_min r_max - { 0, "i++", lineman, { small_ampflow, 0.6f, 12.25f, 9.00f, 36.00f }}, //* 0: i++ - { 1, "sqrt(-1)", lineman, { big_ampflow, 0.53333f, 11.25f, 9.00f, 36.00f }}, //* 1: sqrt(-1) - { 2, "pi", receiver, { small_ampflow, 0.46667f, 11.00f, 6.00f, 36.00f }}, //* 2: pi - { 3, "rho", lineman, { big_ampflow, 0.6f, 11.25f, 9.00f, 36.00f }}, //* 3: ρ - { 4, "2.72", lineman, { big_ampflow, 0.4f, 11.25f, 9.00f, 36.00f }}, //* 4: 2.72 - { 5, ":)", lineman, { big_ampflow, 1.0f, 9.75f, 9.00f, 36.00f }}, //* 5: :) - { 6, ">=", lineman, { small_ampflow, 1.0f, 10.00f, 6.00f, 27.00f }}, //* 6: >= - { 7, "32.2", receiver, { small_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 7: 32.2 - { 8, "9.8", lineman, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 8: 9.8 - { 9, "c", running_back, { falcon, 0.4f, 8.00f, 6.00f, 36.00f }}, //* 9: c - { 10, "phi", center, { small_ampflow, 0.6f, 11.50f, 9.00f, 36.00f }}, //* 10: Φ - { 11, "inf", quarterback_old, { small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f }}, //* 11: ∞ - { 12, "theta", kicker, { small_ampflow, 0.5f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ - { 13, "y=x", mecanum_center, { mecanum, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 13: y=x - { 14, "qb_base", quarterback_base, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 14: beta (bottom) - { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: beta (top) - { 16, "l-man-v1", lineman, { small_12v, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 16: generic lineman V1 - { 17, "420", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 17: 420 - { 18, "24", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 18: 24 - { 19, "25", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }} //* 19: 25 +// idx bot_name bot_type motor_type gear_ratio wheel_base r_min r_max + { 0, "i++", lineman, { small_ampflow, 0.6f, 12.25f, 9.00f, 36.00f }}, //* 0: i++ + { 1, "sqrt(-1)", lineman, { big_ampflow, 0.53333f, 11.25f, 9.00f, 36.00f }}, //* 1: sqrt(-1) + { 2, "pi", receiver, { small_ampflow, 0.46667f, 11.00f, 6.00f, 36.00f }}, //* 2: pi + { 3, "rho", lineman, { big_ampflow, 0.6f, 11.25f, 9.00f, 36.00f }}, //* 3: ρ + { 4, "2.72", lineman, { big_ampflow, 0.4f, 11.25f, 9.00f, 36.00f }}, //* 4: 2.72 + { 5, ":)", lineman, { big_ampflow, 1.0f, 9.75f, 9.00f, 36.00f }}, //* 5: :) + { 6, ">=", lineman, { small_ampflow, 1.0f, 10.00f, 6.00f, 27.00f }}, //* 6: >= + { 7, "32.2", receiver, { small_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 7: 32.2 + { 8, "9.8", lineman, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 8: 9.8 + { 9, "c", running_back, { falcon, 0.4f, 8.00f, 6.00f, 36.00f }}, //* 9: c + { 10, "phi", center, { small_ampflow, 0.6f, 11.50f, 9.00f, 36.00f }}, //* 10: Φ + { 11, "inf", quarterback_old, { small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f }}, //* 11: ∞ + { 12, "theta", kicker_old, { pancake_ampflow, 0.5f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ + { 13, "y=x", mecanum_center, { mecanum, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 13: y=x + { 14, "qb_base", quarterback_base, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 14: beta (bottom) + { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: beta (top) + { 16, "l-man-v1", lineman, { small_12v, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 16: generic lineman V1 + { 17, "420", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 17: 420 + { 18, "24", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 18: 24 + { 19, "25", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 19: 25 + { 20, "tau", kicker, { small_ampflow, 1.0f, 11.00f, 5.50f, 18.00f }} //* 20: tau }; //! Do not decrease r_min to less than half of the wheelbase, or the math might break diff --git a/src/main.cpp b/src/main.cpp index aed0c02b..f9ed2a21 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -88,6 +88,11 @@ void setup() { drive = new Drive(kicker, driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; + case kicker_old: + robot = new Kicker(SPECBOT_PIN1, SPECBOT_PIN2, ENC1_CHA, ENC1_CHB); + drive = new Drive(kicker, driveParams); + drive->setupMotors(M1_PIN, M2_PIN); + break; case quarterback_old: robot = new Quarterback(SPECBOT_PIN1, SPECBOT_PIN2, SPECBOT_PIN3); drive = new Drive(quarterback_old, driveParams); From b6540a2247a5b8c70609bbc73857709a3be44bf6 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 3 Feb 2025 20:23:41 -0500 Subject: [PATCH 13/23] remove usage of `#pragma once` --- src/Drive/Drive.h | 7 +++++-- src/Drive/DriveMecanum.h | 2 -- src/Robot/Center.h | 2 -- src/Robot/Kicker.h | 2 -- src/Robot/MecanumCenter.h | 2 -- src/Robot/MotorControl.h | 5 ++++- src/Robot/Quarterback.h | 2 -- src/Robot/QuarterbackBase.h | 2 -- src/Utilities/BotTypes.h | 2 -- src/Utilities/ConfigManager.h | 2 -- src/Utilities/MotorTypes.h | 2 -- src/Utilities/PrintSerial.h | 2 -- 12 files changed, 9 insertions(+), 23 deletions(-) diff --git a/src/Drive/Drive.h b/src/Drive/Drive.h index c55048ba..cb666762 100644 --- a/src/Drive/Drive.h +++ b/src/Drive/Drive.h @@ -1,4 +1,5 @@ -#pragma once +#ifndef DRIVE_H +#define DRIVE_H #include #include @@ -128,4 +129,6 @@ class Drive { // should be a value less than BIG_NORMAL_PCT, to slow down for precision maneuvering, QB needs this to be 0.3 float BIG_SLOW_PCT; -}; \ No newline at end of file +}; + +#endif // DRIVE_H \ No newline at end of file diff --git a/src/Drive/DriveMecanum.h b/src/Drive/DriveMecanum.h index a0b4f5f8..5f181906 100644 --- a/src/Drive/DriveMecanum.h +++ b/src/Drive/DriveMecanum.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef DRIVE_MECANUM_H #define DRIVE_MECANUM_H diff --git a/src/Robot/Center.h b/src/Robot/Center.h index e0bc7b41..df46c9c8 100644 --- a/src/Robot/Center.h +++ b/src/Robot/Center.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef OLD_CENTER_H #define OLD_CENTER_H diff --git a/src/Robot/Kicker.h b/src/Robot/Kicker.h index 61780cd2..daae340c 100644 --- a/src/Robot/Kicker.h +++ b/src/Robot/Kicker.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef KICKER_H #define KICKER_H diff --git a/src/Robot/MecanumCenter.h b/src/Robot/MecanumCenter.h index 43be5291..f9142e0e 100644 --- a/src/Robot/MecanumCenter.h +++ b/src/Robot/MecanumCenter.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef MECANUM_CENTER_H #define MECANUM_CENTER_H diff --git a/src/Robot/MotorControl.h b/src/Robot/MotorControl.h index 9cb46f22..13dac261 100644 --- a/src/Robot/MotorControl.h +++ b/src/Robot/MotorControl.h @@ -1,4 +1,5 @@ -#pragma once +#ifndef MOTOR_CONTROL_H +#define MOTOR_CONTROL_H #include #include @@ -58,3 +59,5 @@ class MotorControl { void readEncoder(); int calcSpeed(int current_count); }; + +#endif // MOTOR_CONTROL_H \ No newline at end of file diff --git a/src/Robot/Quarterback.h b/src/Robot/Quarterback.h index c88acb38..2b15d71e 100644 --- a/src/Robot/Quarterback.h +++ b/src/Robot/Quarterback.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef QUARTERBACK_H #define QUARTERBACK_H diff --git a/src/Robot/QuarterbackBase.h b/src/Robot/QuarterbackBase.h index 2024ec5e..9f3b01b7 100644 --- a/src/Robot/QuarterbackBase.h +++ b/src/Robot/QuarterbackBase.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef QUARTERBACK_BASE_H #define QUARTERBACK_BASE_H diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index a33f0b1b..9456376b 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef BOT_TYPES_H #define BOT_TYPES_H diff --git a/src/Utilities/ConfigManager.h b/src/Utilities/ConfigManager.h index 97ac56f5..8b9594ea 100644 --- a/src/Utilities/ConfigManager.h +++ b/src/Utilities/ConfigManager.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef CFG_MGR_H #define CFG_MGR_H diff --git a/src/Utilities/MotorTypes.h b/src/Utilities/MotorTypes.h index 22f77769..d488bce4 100644 --- a/src/Utilities/MotorTypes.h +++ b/src/Utilities/MotorTypes.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef MOTOR_TYPES_H #define MOTOR_TYPES_H diff --git a/src/Utilities/PrintSerial.h b/src/Utilities/PrintSerial.h index b830ec48..270adb5d 100644 --- a/src/Utilities/PrintSerial.h +++ b/src/Utilities/PrintSerial.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef PRINTSERIAL_H #define PRINTSERIAL_H From 4fea2db0db5ba7ca4759bc63eb798f7e2956aa51 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Wed, 5 Feb 2025 20:51:56 -0500 Subject: [PATCH 14/23] improved debouncer docs, added minor fn + example usage --- src/Utilities/Debouncer.cpp | 8 +++-- src/Utilities/Debouncer.h | 61 ++++++++++++++++++++++++++++++++----- 2 files changed, 60 insertions(+), 9 deletions(-) diff --git a/src/Utilities/Debouncer.cpp b/src/Utilities/Debouncer.cpp index 6082f021..a81dcb89 100644 --- a/src/Utilities/Debouncer.cpp +++ b/src/Utilities/Debouncer.cpp @@ -1,5 +1,7 @@ #include "Debouncer.h" +//* for how to use this class: see Debouncer.h + // based on: https://arduinogetstarted.com/tutorials/arduino-button-debounce // Input: debounce delay (milliseconds) @@ -21,8 +23,6 @@ Debouncer::Debouncer(unsigned long delay, bool activeLow) { } -// takes only input of 0 or 1, and outputs 0 or 1 -// @param inputState: "current" call to debounce uint8_t Debouncer::debounce(uint8_t inputState) { // Serial.print(F("start: l_stab:")); // Serial.print(lastStableState); @@ -68,6 +68,10 @@ uint8_t Debouncer::debounce(uint8_t inputState) { return lastStableState; } +uint8_t Debouncer::isActive() { + return lastStableState == ACTIVE_STATE; +} + uint8_t Debouncer::wasToggled() { return (lastLastStableState != lastStableState); } diff --git a/src/Utilities/Debouncer.h b/src/Utilities/Debouncer.h index a269e5f9..c9d7a866 100644 --- a/src/Utilities/Debouncer.h +++ b/src/Utilities/Debouncer.h @@ -18,30 +18,77 @@ class Debouncer { uint8_t BASE_STATE; uint8_t ACTIVE_STATE; public: - // construct debouncer with parameterized delay + // construct debouncer with parameterized delay (in milliseconds) // recommend using pointer to debouncer object + // default is active high ('1' is active state). set second argument to 'true' to make active low ('0') Debouncer(unsigned long delay, bool activeLow = false); - // use to obtain "current" debounced state of button - uint8_t debounce(uint8_t newState); + // must be called regularly (i.e., in a loop) + // whatever you call to check your button/sensor, wrap it in this + // takes only input of 0 or 1, and outputs 0 or 1 + /// @param inputState: "current" call to debounce + uint8_t debounce(uint8_t inputState); - // use to execute an action *once* when the button is toggled *after debouncing* + // should be used to obtain "current" debounced state of button + uint8_t isActive(); + + // should be used to enable execution of an action *once* when the button is toggled *after debouncing* + // in other words, it checks whether the button has changed debounced state // it is assumed that debounce() is being called regularly uint8_t wasToggled(); - // use to execute an action *once* when the button is toggled *after debouncing* + // should be used to enable execution of an action *once* when the button is toggled *after debouncing* // calls both debounce() and wasToggled() consecutively to avoid potential misses uint8_t debounceAndToggled(uint8_t inputState); - // use to execute an action when switched to a specific state *after debouncing* + // should be used to enable execution of an action when switched to a specific state *after debouncing* + // calls wasToggled() and checks it against the passed `state` parameter uint8_t wasSwitchedToState(DebouncerState state); - // use to execute an action when switched to a specific state *after debouncing* + // should be used to enable execution of an action when switched to a specific state *after debouncing* // calls both debounce() and wasSwitchedToState() consecutively to avoid potential misses + // not as useful as debounceAndPressed() because usually you have no reason to switch based on the inactive state uint8_t debounceAndSwitchedTo(uint8_t inputState, DebouncerState targetState); + //* usually you will use this function + // should be used to enable execution of an action when a button has been pressed (after debouncing) + //* it is assumed that the "active" state of the button corresponds to the "pressed" state // shorthand for debounceAndSwitchesTo(inputState, active) + // calls both debounce() and wasSwitchedToState() automatically uint8_t debounceAndPressed(uint8_t inputState); }; +//* Example Usage +#if 0 +// in header file +#define DB_EXAMPLE_DELAY 500L + +// in header file/class +Debouncer* dbExample; + +// in source file/constructor +dbExample = new Debouncer(DB_EXAMPLE_DELAY); + +// in source file/loop +//* use case 1 +if (dbExample->debounceAndPressed(ps5.Circle())) { + // do something... +} + +//* use case 2 +uint8_t debouncedValue = dbExample->debounce(ps5.Circle()); // run repeatedly +// check debouncedValue, do something with it, maybe graph it, idk... + +//* use case 3 +// this is basically a binary trigger +// it will toggle on a change in debounced state +if (dbExample->debounceAndToggled(ps5.Circle())) { + // do something + // you could also do something different depending on the current state + if (dbExample->isActive()) { + // do something else + } +} +#endif + #endif // DEBOUNCER_H \ No newline at end of file From fbbf4b5b717a691a976273115f1fbbdc9706140c Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 10 Feb 2025 19:37:39 -0500 Subject: [PATCH 15/23] update new kicker drive params --- src/Utilities/BotTypes.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 9456376b..82de5744 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -120,7 +120,7 @@ constexpr bot_config_t botConfigArray[NUM_BOTS] = { { 17, "420", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 17: 420 { 18, "24", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 18: 24 { 19, "25", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 19: 25 - { 20, "tau", kicker, { small_ampflow, 1.0f, 11.00f, 5.50f, 18.00f }} //* 20: tau + { 20, "tau", kicker, { small_ampflow, 0.5f, 9.00f, 9.00f, 36.00f }} //* 20: tau }; //! Do not decrease r_min to less than half of the wheelbase, or the math might break From 7ed623cbffee7b690b50ed69e8a422f1e37c76aa Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 10 Feb 2025 20:02:27 -0500 Subject: [PATCH 16/23] update qb gear ratios --- src/Utilities/BotTypes.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 82de5744..7fec34b1 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -114,8 +114,8 @@ constexpr bot_config_t botConfigArray[NUM_BOTS] = { { 11, "inf", quarterback_old, { small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f }}, //* 11: ∞ { 12, "theta", kicker_old, { pancake_ampflow, 0.5f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ { 13, "y=x", mecanum_center, { mecanum, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 13: y=x - { 14, "qb_base", quarterback_base, { big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 14: beta (bottom) - { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: beta (top) + { 14, "qb_base", quarterback_base, { big_ampflow, 0.7273f, 11.50f, 9.00f, 36.00f }}, //* 14: beta (bottom) + { 15, "qb_turret", quarterback_turret, { falcon, 1.0f, 11.50f, 9.00f, 36.00f }}, //* 15: beta (top) { 16, "l-man-v1", lineman, { small_12v, 1.0f, 11.00f, 9.00f, 36.00f }}, //* 16: generic lineman V1 { 17, "420", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 17: 420 { 18, "24", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 18: 24 From 173827f2233f5a294b0a18e484dbed87b8194587 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 10 Feb 2025 20:27:14 -0500 Subject: [PATCH 17/23] don't search for a new controller if pairing to last worked --- src/Pairing/pairing.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/Pairing/pairing.cpp b/src/Pairing/pairing.cpp index 36fb1126..73f29d06 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -273,6 +273,8 @@ void activatePairing(int discoverTime, int rePairTime) { } #else // always try to re-pair first, then search for new controller if none is found pairToLastController(rePairTime, addrCharPtr); - searchForNewController(discoverTime, addrCharPtr); + if (!ps5.isConnected()) { + searchForNewController(discoverTime, addrCharPtr); + } #endif } \ No newline at end of file From 791ff36bf5451a1686f964a6be57e483a1dc578e Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Tue, 11 Feb 2025 13:28:22 -0500 Subject: [PATCH 18/23] change qb turret debug printouts (magnetometer) --- src/Robot/QuarterbackTurret.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/Robot/QuarterbackTurret.cpp b/src/Robot/QuarterbackTurret.cpp index 55d6cdc9..bbda7616 100644 --- a/src/Robot/QuarterbackTurret.cpp +++ b/src/Robot/QuarterbackTurret.cpp @@ -416,12 +416,12 @@ void QuarterbackTurret::moveTurretAndWait(int16_t heading, float power, bool rel void QuarterbackTurret::updateTurretMotionStatus() { // if (utmsCtr >= UTMS_CTR_MAX) { // utmsCtr = 0; - Serial.print(F("update called with ctec = ")); - Serial.print(currentTurretEncoderCount); - Serial.print(F("; ttec = ")); - Serial.print(targetTurretEncoderCount); - Serial.print(F("; error (ct) = ")); - Serial.println(fabs((currentTurretEncoderCount % QB_COUNTS_PER_TURRET_REV) - targetTurretEncoderCount)); + // Serial.print(F("update called with ctec = ")); + // Serial.print(currentTurretEncoderCount); + // Serial.print(F("; ttec = ")); + // Serial.print(targetTurretEncoderCount); + // Serial.print(F("; error (ct) = ")); + // Serial.println(fabs((currentTurretEncoderCount % QB_COUNTS_PER_TURRET_REV) - targetTurretEncoderCount)); // } else { // utmsCtr++; // } @@ -1260,16 +1260,16 @@ void QuarterbackTurret::calculateHeadingMag() { if (headingDeg > 360) headingDeg = ((int) headingDeg) % 360; /*DEBUGGING PRINTOUTS*/ - // Serial.print("X: "); Serial.print(lis3mdl.x); - // Serial.print("\tY: "); Serial.print(lis3mdl.y); - // Serial.print("\tMinX: "); Serial.print(mag_xMin); - // Serial.print("\tMaxX: "); Serial.print(mag_xMax); - // Serial.print("\tMinY: "); Serial.print(mag_yMin); - // Serial.print("\tMaxY: "); Serial.print(mag_yMax); - // Serial.print("\txAdapt: "); Serial.print(mag_xVal); - // Serial.print("\tyAdapt: "); Serial.print(mag_yVal); - // Serial.print("\tHeading [deg]: "); Serial.print(headingDeg); - // Serial.println(); + Serial.print("X: "); Serial.print(lis3mdl.x); + Serial.print("\tY: "); Serial.print(lis3mdl.y); + Serial.print("\tMinX: "); Serial.print(mag_xMin); + Serial.print("\tMaxX: "); Serial.print(mag_xMax); + Serial.print("\tMinY: "); Serial.print(mag_yMin); + Serial.print("\tMaxY: "); Serial.print(mag_yMax); + Serial.print("\txAdapt: "); Serial.print(mag_xVal); + Serial.print("\tyAdapt: "); Serial.print(mag_yVal); + Serial.print("\tHeading [deg]: "); Serial.print(headingDeg); + Serial.println(); } } #pragma endregion From b23ef3d32a9202416843aec057265c1485d22178 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Mon, 10 Feb 2025 20:56:56 -0500 Subject: [PATCH 19/23] fixed NPE fixed corbin's second mistake --- src/main.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index f9ed2a21..be548f6b 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -126,6 +126,7 @@ void setup() { ENC1_CHB, // turret encoder ENC2_CHB // zeroing laser ); + drive = new Drive(); // TODO: instantiate this to prevent PrintSerial from causing NPE/crash break; case quarterback_base: drive = new Drive(quarterback_base, driveParams); @@ -154,8 +155,6 @@ void setup() { printserial.setDriveObj(drive); - printserial.setDriveObj(drive); - ps5.attachOnConnect(onConnection); ps5.attachOnDisconnect(onDisconnect); } From ac600898cab35ce66ee07b71ac82b5db79c09918 Mon Sep 17 00:00:00 2001 From: rdavies02 Date: Wed, 5 Mar 2025 21:15:18 -0500 Subject: [PATCH 20/23] Don't try to re-pair forever Co-Authored-By: RyzenFromFire <16062019+RyzenFromFire@users.noreply.github.com> --- src/Pairing/pairing.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Pairing/pairing.h b/src/Pairing/pairing.h index 8ec7bd09..a8e04dd9 100644 --- a/src/Pairing/pairing.h +++ b/src/Pairing/pairing.h @@ -3,7 +3,7 @@ #include "PolarRobotics.h" #define DEFAULT_BT_DISCOVER_TIME 10000 // milliseconds -#define DEFAULT_BT_REPAIR_TIME 1000000 // milliseconds +#define DEFAULT_BT_REPAIR_TIME 10000 // milliseconds bool addressIsController(const char* addrCharPtr); bool startDiscovery(); void storeAddress(const char* addr, bool clear); From 62a50cd60b9bee11bf1b2c61a2749b011fcc0f32 Mon Sep 17 00:00:00 2001 From: Fc500 Date: Wed, 5 Mar 2025 21:22:11 -0500 Subject: [PATCH 21/23] Update QuarterbackTurret.h --- src/Robot/QuarterbackTurret.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Robot/QuarterbackTurret.h b/src/Robot/QuarterbackTurret.h index 89465ecc..955f5eec 100644 --- a/src/Robot/QuarterbackTurret.h +++ b/src/Robot/QuarterbackTurret.h @@ -467,7 +467,6 @@ class QuarterbackTurret : public Robot { void printDebug(); void testRoutine(); - //====================================// // Quarterback Subsystem Controls // //====================================// From 6901e6458b01494eb4b37bed7132bb5c5cfbd252 Mon Sep 17 00:00:00 2001 From: apotb Date: Mon, 24 Mar 2025 19:51:14 -0400 Subject: [PATCH 22/23] Codebase changes for upload GUI Update WriteBotInfo.cpp --- pio_build_script.py | 10 ++++++++++ src/Utilities/WriteBotInfo.cpp | 6 +++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/pio_build_script.py b/pio_build_script.py index 58ce3316..04442348 100644 --- a/pio_build_script.py +++ b/pio_build_script.py @@ -1,5 +1,6 @@ Import("env") import subprocess +import os from datetime import datetime # sources @@ -36,3 +37,12 @@ env.Append( CPPDEFINES=[("PR_CODEBASE_VERSION", env.StringifyMacro(version))], ) + +# Get the value of PR_GUI_BOT_INDEX' passed from the command line or environment variables +bot_index = os.environ.get('PR_GUI_BOT_INDEX', '0') # Default to 0 if not provided + +# Append the build flag to the environment +env.Append( + CPPDEFINES=[("BOT_INDEX", bot_index)], +) +print(f"bot index is: {bot_index}") \ No newline at end of file diff --git a/src/Utilities/WriteBotInfo.cpp b/src/Utilities/WriteBotInfo.cpp index 47d27193..faf8c9dc 100644 --- a/src/Utilities/WriteBotInfo.cpp +++ b/src/Utilities/WriteBotInfo.cpp @@ -17,7 +17,11 @@ void setup() { //! If you want to use a predefined robot from BotTypes.h, declare the index here: // based on https://docs.google.com/spreadsheets/d/1DswoEAcry9L9t_4ouKL3mXFgDMey4KkjEPFXULQxMEQ/edit#gid=0 //! Please reset to zero when you are done uploading to avoid merge conflicts. - uint8_t index = 0; // also handles bot name index + //* BOT_INDEX define configured using GUI via environment variable, do not define + #ifndef BOT_INDEX + #define BOT_INDEX 0 // Default to 0 if not defined + #endif + uint8_t index = BOT_INDEX; //* CUSTOM BOT CONFIGURATION //! If you want to set custom bot and motor type, assign index appropriately, then assign these: From 06b1c8411d74bec48f7fd872abd465ddb93dab88 Mon Sep 17 00:00:00 2001 From: RyzenFromFire Date: Wed, 2 Apr 2025 19:10:05 -0400 Subject: [PATCH 23/23] fix kicker v2 code --- src/Robot/Kicker.cpp | 89 +++++++++++++++++++++++++++++--------------- src/Robot/Kicker.h | 33 +++++++++------- 2 files changed, 78 insertions(+), 44 deletions(-) diff --git a/src/Robot/Kicker.cpp b/src/Robot/Kicker.cpp index ce7607e1..3f199135 100644 --- a/src/Robot/Kicker.cpp +++ b/src/Robot/Kicker.cpp @@ -17,17 +17,23 @@ * Intended behavior is for the robot to turn on, wind the arm until it hits the limit switch, and * then fall back a certain number of degrees (~15 degrees for now). * - * @param kickerPin The pin of the kicker arm's motor + * @param triggerMotorPin The pin of the kicker arm's motor * @param limitSwitchPin the pin of the kicker arm's limit switch * @param kickerEncoderPinA The pin of signal A from the encoder * @param kickerEncoderPinB THe pin of signal B from the encoder */ -Kicker::Kicker(uint8_t kickerPin, uint8_t limitSwitchPin, uint8_t kickerEncoderPinA, uint8_t kickerEncoderPinB) { +Kicker::Kicker(uint8_t triggerMotorPin, uint8_t limitSwitchPin, uint8_t kickerEncoderPinA, uint8_t kickerEncoderPinB) { + // Safety Setup enabled = false; - this->kickerPin = kickerPin; - this->limitSwitchPin = limitSwitchPin; - windupMotor.setup(kickerPin, small_12v); this->dbEnable = new Debouncer(KICKER_ENABLE_DB_DELAY); + this->dbHome = new Debouncer(KICKER_ENABLE_DB_DELAY); + + // Motor Setup + this->triggerMotorPin = triggerMotorPin; + this->limitSwitchPin = limitSwitchPin; + + //* Note: trigger motor is not actually a small_12v motor, but close enough at least for now + triggerMotor.setup(triggerMotorPin, small_12v); // Encoder Setup this->kickerEncoderPinA = kickerEncoderPinA; @@ -67,19 +73,31 @@ void Kicker::kickerEncoderISR() { void Kicker::action() { // Control the motor on the kicker manually if (enabled) { - if (dbEnable->debounceAndPressed(ps5.Circle())) + if (dbEnable->debounceAndPressed(ps5.Square())) { enabled = false; - else if (ps5.Triangle()) + } + // else if (dbHome->debounceAndPressed(ps5.Circle())) { + // homeTriggerMotor(); + // } + else if (ps5.Triangle()) { // not a macro turnForward(); - else if (ps5.Cross()) + } + else if (ps5.Cross()) { turnReverse(); - else + } + else { stop(); - - Serial.println(F("kicker enabled")); + } } else { - if (dbEnable->debounceAndPressed(ps5.Circle())) + if (dbEnable->debounceAndPressed(ps5.Square())) { enable(); + Serial.println(F("kicker enabled")); + } + // else if (dbHome->debounceAndPressed(ps5.Circle())) { + // enable(); + // Serial.println(F("kicker enabled")); + // homeTriggerMotor(); + // } } } @@ -93,54 +111,58 @@ void Kicker::enable() { } /** - * @brief Turns motor forward + * @brief Turns motor "forward" (away from the limit switch) * - * Turns the motor forward by writing the windupMotor SPECBOT_1 (D18) pin to -1 + * Turns the motor forward by writing the triggerMotor SPECBOT_1 (D18) pin to -1 */ void Kicker::turnForward() { if (enabled) { - windupMotor.write(-1); + triggerMotor.write(1); + printCurrentAngle(); } } /** - * @brief Turns motor reverse + * @brief Turns motor "backwards" (towards the limit switch) * - * Turns the motor backwards by writing the windupMotor SPECBOT_1 (D18) pin to 1 + * Turns the motor backwards by writing the triggerMotor SPECBOT_1 (D18) pin to 1 */ void Kicker::turnReverse() { if (enabled) { - windupMotor.write(1); + triggerMotor.write(-1); + printCurrentAngle(); } } /** * @brief Stop Motor * - * Stops the motor by writing the windupMotor SPECBOT_1 (D18) pin to 0 + * Stops the motor by writing the triggerMotor SPECBOT_1 (D18) pin to 0 */ void Kicker::stop() { if (enabled) { - windupMotor.write(0); + triggerMotor.write(0); } } /** - * @brief Automatically Wind on Startup + * @brief Home/zero trigger motor * @author Corbin Hibler * - * This function will run when the kicker starts. It will automatically wind the kicker arm - * to a certain level until it hits the limit switch. Then it will automatically adjust to a certain - * degree from the zero point (where the limit switch is). + * This function is used to home/zero the trigger motor (for the release mechanism) on startup. + * First, it will move the kicker motor until it hits the limit switch. + * Then it will adjust to a certain degree from the zero point (where the limit switch is). * */ -void Kicker::homeKickingArm() { - while(digitalRead(limitSwitchPin) == 0) { - windupMotor.write(0.5); +void Kicker::homeTriggerMotor() { + while (digitalRead(limitSwitchPin) == 0) { + triggerMotor.write(-KICKER_HOMING_SPEED); } stop(); - angleZero = getCurrentAngle(); - adjustAngle(15); + + // update angle, then back off 15 degrees. + triggerMotorHomeAngle = getCurrentAngle(); + adjustAngle(15); } /** @@ -161,11 +183,11 @@ void Kicker::homeKickingArm() { * @param angle The angle that you want to add to the current angle of the kicker arm. */ void Kicker::adjustAngle(int angle) { - uint16_t desiredAngle = angleZero + angle; + uint16_t desiredAngle = triggerMotorHomeAngle + angle; // Keep rotating until desiredAngle is reached while (getCurrentAngle() < desiredAngle) { - windupMotor.write(-0.5); + triggerMotor.write(KICKER_HOMING_SPEED); } stop(); } @@ -186,4 +208,9 @@ void Kicker::adjustAngle(int angle) { uint16_t Kicker::getCurrentAngle() { uint16_t currentAngle = currentKickerEncoderCount / KICKER_COUNTS_PER_ARM_DEGREE; return currentAngle; +} + +void Kicker::printCurrentAngle() { + Serial.print(F("Current Kicker Trigger Motor Encoder Angle:")); + Serial.println(getCurrentAngle()); } \ No newline at end of file diff --git a/src/Robot/Kicker.h b/src/Robot/Kicker.h index daae340c..6c2ecd10 100644 --- a/src/Robot/Kicker.h +++ b/src/Robot/Kicker.h @@ -13,6 +13,9 @@ #define KICKER_ENABLE_DB_DELAY 100L +// Motor Speeds +#define KICKER_HOMING_SPEED 0.5 + /** * @brief Kicker V2 Class * @@ -25,33 +28,37 @@ class Kicker : public Robot { private: bool enabled; // Safety feature to ensure robot does not act when it is not supposed to. - u_int16_t angleZero; // Angle of the limit switch. - uint8_t kickerPin; // Pin to control the motor of the kicker arm - uint8_t limitSwitchPin; // Pin to connect to the limit switch - static uint8_t kickerEncoderPinA; // Signal Pin for channel A of the encoder - static uint8_t kickerEncoderPinB; // Signal Pin for channel B of the encoder - static uint8_t kickerEncoderStateB; // Keeps track of the current state of channel B - static int32_t currentKickerEncoderCount; // Encoder count of kicker arm motor encoder - MotorControl windupMotor; // MotorControl instantation for the kicker arm motor - Debouncer* dbEnable; + Debouncer* dbHome; + + MotorControl triggerMotor; // on the Kicker V2, this is the motor that releases the mechanism to fire the arm + //* negative is towards limit switch, positive is away from limit switch + uint16_t triggerMotorHomeAngle; // Angle of the motor when at the limit switch (zeroed/homed). + uint8_t triggerMotorPin; // Pin to control the motor of the kicker arm + uint8_t limitSwitchPin; // Pin to connect to the limit switch + + static uint8_t kickerEncoderPinA; // Signal Pin for channel A of the encoder + static uint8_t kickerEncoderPinB; // Signal Pin for channel B of the encoder + static uint8_t kickerEncoderStateB; // Keeps track of the current state of channel B + static int32_t currentKickerEncoderCount; // Encoder count of kicker arm motor encoder public: Kicker( - uint8_t kickerPin, // Pin to control the motor of the kicker arm - u_int8_t limitSwitchPin, // Pin to connect to the limit switch + uint8_t triggerMotorPin, // Pin to control the motor of the kicker arm + uint8_t limitSwitchPin, // Pin to connect to the limit switch uint8_t kickerEncoderPinA, // Signal Pin for channel A of the encoder - u_int8_t kickerEncoderPinB // Signal Pin for channel B of the encoder + uint8_t kickerEncoderPinB // Signal Pin for channel B of the encoder ); void action() override; //! robot subclass must override action void enable(); void turnForward(); void turnReverse(); void stop(); - void homeKickingArm(); + void homeTriggerMotor(); void adjustAngle(int angle); static void kickerEncoderISR(); uint16_t getCurrentAngle(); + void printCurrentAngle(); }; #endif // KICKER_H