Dev/experimental

README.md

@@ -59,4 +59,4 @@ src
 │   ├── ReadBotInfo.cpp
 │   └── WriteBotInfo.cpp
 └── main.cpp
-```
+```

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}")

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 = 
+; 	+<main.cpp>
+; 	+<PolarRobotics.h>
+; 	+<Drive/Drive.cpp>
+; 	+<Drive/Drive.h>
+; 	+<Robot/MotorControl.h>
+;   +<Robot/MotorControl.cpp>
+;   +<Robot/Lights.h>
+;   +<Robot/builtInLED.h>
+;   +<Robot/builtInLED.cpp>	
+;   +<Pairing/pairing.h>
+;   +<Pairing/pairing.cpp>
+;   +<Utilities/BotTypes.h>
+;   +<Utilities/BotTypes.cpp>
+;   +<Utilities/MotorTypes.h>
+;   +<Utilities/MotorTypes.cpp>
+;   +<Utilities/ConfigManager.h>
+;   +<Utilities/ConfigManager.cpp>
+;   +<Utilities/Pair.h>
+; -<depairingStation.cpp>
+; -<Drive/DriveQuick.cpp>
+; -<Drive/DriveQuick.h>
+; -<Robot/Quarterback_old.cpp>
 
 [env:robot]
 build_src_filter = 

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();

src/Drive/Drive.h

@@ -1,4 +1,5 @@
-#pragma once
+#ifndef DRIVE_H
+#define DRIVE_H
 
 #include <Arduino.h>
 #include <Robot/MotorControl.h>
@@ -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;
-};
+};
+
+#endif // DRIVE_H

src/Drive/DriveMecanum.h

@@ -1,5 +1,3 @@
-#pragma once
-
 #ifndef DRIVE_MECANUM_H
 #define DRIVE_MECANUM_H
 

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"));
 

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
 }

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

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,

src/Robot/Center.h

@@ -1,5 +1,3 @@
-#pragma once
-
 #ifndef OLD_CENTER_H
 #define OLD_CENTER_H