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 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/platformio.ini b/platformio.ini index fce84212..f7b9f9ba 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/Drive/Drive.cpp b/src/Drive/Drive.cpp index 889c3764..e72f4aad 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 @@ -406,7 +339,7 @@ void Drive::printCsvInfo() { */ 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/Drive/Drive.h b/src/Drive/Drive.h index 67046505..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 @@ -114,8 +115,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 @@ -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/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 fdc2ea85..73f29d06 100644 --- a/src/Pairing/pairing.cpp +++ b/src/Pairing/pairing.cpp @@ -58,11 +58,12 @@ 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 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) @@ -129,45 +130,40 @@ void getAddress(const char* &addr) { else addr = &str.c_str()[0]; // get value of char ptr string } -/// @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) { - Serial.begin(115200); - // pinMode(LED_BUILTIN, OUTPUT); +/// @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) { + Serial.print(F("Connecting to PS5 Controller @ ")); + Serial.println(addrCharPtr); + ps5.begin(addrCharPtr); + int timer = 0; - // 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. - const char* addrCharPtr = nullptr; - getAddress(addrCharPtr); + // wait until discovery time passes or we connect to a controller + while (timer < time && !ps5.isConnected()) { + delay(LOOP_DELAY); + timer += LOOP_DELAY; - 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(); - } + // 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 - } - } + // return if we get a connection at this point + if (ps5.isConnected()) { + Serial.println(F("PS5 Controller Connected!")); + yeet; + } + } +} +/// @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)) { Serial.println(F("SerialBT failed!")); // function returns false if failed @@ -176,14 +172,14 @@ void activatePairing(bool doRePair, int discoverTime) { 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()) { int timer = 0; // recall foundController is set by the callback in `startDiscovery` when a valid PS5 controller is found - while (timer < discoverTime && !foundController) { + while (timer < time && !foundController) { delay(LOOP_DELAY); timer += LOOP_DELAY; Lights::getInstance().updateLEDS(); @@ -195,7 +191,7 @@ void activatePairing(bool doRePair, int discoverTime) { 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(); } @@ -205,7 +201,7 @@ void activatePairing(bool doRePair, int discoverTime) { 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:")); @@ -253,4 +249,32 @@ void activatePairing(bool doRePair, int discoverTime) { Serial.println(F("Asynchronous discovery failed.")); setBuiltInLED(false); } +} + +/// @brief Search for PS5 Controllers and pair to the first one found +/// @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(int discoverTime, int rePairTime) { + Serial.begin(115200); + + // 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. + 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 + searchForNewController(discoverTime, addrCharPtr); + } else { //! if PAIRING_PIN is LOW, reconnect to previous controller + pairToLastController(rePairTime, addrCharPtr); + } + #else // always try to re-pair first, then search for new controller if none is found + pairToLastController(rePairTime, addrCharPtr); + if (!ps5.isConnected()) { + searchForNewController(discoverTime, addrCharPtr); + } + #endif } \ No newline at end of file diff --git a/src/Pairing/pairing.h b/src/Pairing/pairing.h index a317d3a9..a8e04dd9 100644 --- a/src/Pairing/pairing.h +++ b/src/Pairing/pairing.h @@ -2,11 +2,12 @@ #define PAIRING_H #include "PolarRobotics.h" -#define DEFAULT_BT_DISCOVER_TIME 15000 -bool addressIsController(const char * addrCharPtr); +#define DEFAULT_BT_DISCOVER_TIME 10000 // milliseconds +#define DEFAULT_BT_REPAIR_TIME 10000 // 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); +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 75f5eb7f..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 @@ -46,9 +50,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/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.cpp b/src/Robot/Kicker.cpp index cb36330d..3f199135 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" /** @@ -9,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 limitswitchPin the pin of the kicker arm's limit switch + * @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; @@ -55,27 +69,35 @@ 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 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(); + // } } } @@ -89,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); } /** @@ -157,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(); } @@ -182,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 da4e6634..6c2ecd10 100644 --- a/src/Robot/Kicker.h +++ b/src/Robot/Kicker.h @@ -1,5 +1,3 @@ -#pragma once - #ifndef KICKER_H #define KICKER_H @@ -15,8 +13,11 @@ #define KICKER_ENABLE_DB_DELAY 100L +// Motor Speeds +#define KICKER_HOMING_SPEED 0.5 + /** - * @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 @@ -27,34 +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 test(); 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 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/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/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 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 // //====================================// diff --git a/src/Utilities/BotTypes.cpp b/src/Utilities/BotTypes.cpp index a5929707..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" }, - { runningback, "runningback" }, + { 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 30aed855..7fec34b1 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 @@ -8,7 +6,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,17 +18,19 @@ * 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, receiver, - runningback, + running_back, center, kicker, + kicker_old, mecanum_center, quarterback_old, quarterback_base, @@ -61,7 +61,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 +82,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 +93,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", runningback, { 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: unassigned - { 15, "qb_turret", quarterback_turret, { falcon, 0.5f, 11.50f, 9.00f, 36.00f }}, //* 15: unassigned - { 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.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 + { 19, "25", lineman, { small_12v, 1.0f, 11.00f, 5.50f, 18.00f }}, //* 19: 25 + { 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 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/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 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.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..270adb5d --- /dev/null +++ b/src/Utilities/PrintSerial.h @@ -0,0 +1,32 @@ +#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/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: diff --git a/src/main.cpp b/src/main.cpp index 30966701..be548f6b 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 @@ -65,6 +68,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(); @@ -84,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); @@ -99,9 +108,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: @@ -117,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); @@ -139,10 +149,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); } @@ -195,7 +207,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); @@ -225,6 +237,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();