diff --git a/platformio.ini b/platformio.ini index 5f10e7f..c669ace 100644 --- a/platformio.ini +++ b/platformio.ini @@ -16,53 +16,54 @@ board = nodemcu-32s board_build.mcu = esp32 upload_protocol = esptool lib_deps = - fastled/FastLED@^3.5.0 - https://github.com/PolarRobotics/PR-Lib.git + fastled/FastLED @ ~3.6.0 + https://github.com/PolarRobotics/PR-Lib.git + adafruit/Adafruit LIS3MDL@^1.2.4 [env:robot] -build_src_filter = - +<*> - - - - - - - - - - +build_src_filter = + +<*> + - + - + - + - + - [env:depairing] build_src_filter = - -<*> - + - + - + - + - + - + - + - + + -<*> + + + + + + + + + + + + + + + + [env:write_bot_info] build_flags = -D CFG_WRITABLE=1 -build_src_filter = - -<*> - + - + - + - + - + - + - + - + - + +build_src_filter = + -<*> + + + + + + + + + + + + + + + + + + [env:read_bot_info] -build_src_filter = - -<*> - + - + - + - + - + - + - + - + - + \ No newline at end of file +build_src_filter = + -<*> + + + + + + + + + + + + + + + + + + diff --git a/src/Drive/Drive.cpp b/src/Drive/Drive.cpp index e0a18d1..b32b417 100644 --- a/src/Drive/Drive.cpp +++ b/src/Drive/Drive.cpp @@ -48,7 +48,7 @@ Drive::Drive(BotType botType, MotorType motorType, drive_param_t driveParams, bo this->R_Min = driveParams.r_min; this->R_Max = driveParams.r_max; - if (botType == quarterback) { + if (botType == quarterback_old) { this->BIG_BOOST_PCT = 0.8; this->BIG_NORMAL_PCT = 0.4; this->BIG_SLOW_PCT = 0.3; @@ -440,5 +440,17 @@ void Drive::update() { M1.write(requestedMotorPower[0]); M2.write(requestedMotorPower[1]); + + trackingMotorPower[0] = requestedMotorPower[0]; + trackingMotorPower[1] = requestedMotorPower[1]; +} + +int Drive::getMotorWifiValue(int motorRequested) { + int valueToReturn = 0; + if (motorRequested >= 0 && motorRequested < NUM_MOTORS) { + float value = trackingMotorPower[motorRequested] * 100; + valueToReturn = value; + } + return valueToReturn; } diff --git a/src/Drive/Drive.h b/src/Drive/Drive.h index 21b23aa..92f0be3 100644 --- a/src/Drive/Drive.h +++ b/src/Drive/Drive.h @@ -99,6 +99,7 @@ class Drive { float turnPower; float requestedMotorPower[NUM_MOTORS]; + float trackingMotorPower[NUM_MOTORS]; float lastRampPower[NUM_MOTORS]; float turnMotorValues[NUM_MOTORS]; @@ -128,6 +129,7 @@ class Drive { void printSetup(); virtual void printDebugInfo(); virtual void printCsvInfo(); + int getMotorWifiValue(int motorRequested); //* The following variables are initialized in the constructor // maximum speed for these is 1.0 diff --git a/src/PolarRobotics.h b/src/PolarRobotics.h index 1db4bf5..b05306c 100644 --- a/src/PolarRobotics.h +++ b/src/PolarRobotics.h @@ -13,7 +13,7 @@ #include #include -#define PR_CODEBASE_VERSION "2.3.2 @ production" +#define PR_CODEBASE_VERSION "2.4.0 @ dev/quarterback" // [PIN DECLARATIONS] // please follow: @@ -36,8 +36,8 @@ // Encoder pin definitions #define ENC1_CHA 35 #define ENC1_CHB 34 -#define ENC2_CHA VP -#define ENC2_CHB VN +#define ENC2_CHA 36 // VP +#define ENC2_CHB 39 // VN // pin for ws2812 LEDs to indicate positions #define LED_PIN 4 diff --git a/src/Robot/Quarterback.cpp b/src/Robot/Quarterback.cpp index 85191d9..b97db28 100644 --- a/src/Robot/Quarterback.cpp +++ b/src/Robot/Quarterback.cpp @@ -189,4 +189,5 @@ void Quarterback::changeFWSpeed(SpeedStatus speed) { lastDBFWChange = millis(); } -} \ No newline at end of file +} + diff --git a/src/Robot/QuarterbackBase.cpp b/src/Robot/QuarterbackBase.cpp new file mode 100644 index 0000000..c901032 --- /dev/null +++ b/src/Robot/QuarterbackBase.cpp @@ -0,0 +1,29 @@ +#include "QuarterbackBase.h" + +//This for some reason has to be declared in the .cpp file and not the .h file so that it does not conflict with the same declaration in other .h files +HardwareSerial Uart_Base(2); // UART2 + +QuarterbackBase::QuarterbackBase(Drive* drive) { + this->drive = drive; + + // Setup digital WiFi pin + pinMode(WIFI_PIN, OUTPUT); + + //Setup UART Pins + Uart_Base.begin(115200, SERIAL_8N1, RX2, TX2); +} + +void QuarterbackBase::action() { +// Send data to wifi ESP + updateWriteMotorValues(); +} + +void QuarterbackBase::updateWriteMotorValues() { + motor1Value = drive->getMotorWifiValue(0); + motor2Value = drive->getMotorWifiValue(1); + UARTMessage = String(motor1Value) + "&" + String(motor2Value); + UARTMessage = UARTMessage + "~"; + Uart_Base.print(UARTMessage); + //Serial.print("Sent Message To ESP: "); + Serial.println(UARTMessage); +} \ No newline at end of file diff --git a/src/Robot/QuarterbackBase.h b/src/Robot/QuarterbackBase.h new file mode 100644 index 0000000..2024ec5 --- /dev/null +++ b/src/Robot/QuarterbackBase.h @@ -0,0 +1,44 @@ +#pragma once + +#ifndef QUARTERBACK_BASE_H +#define QUARTERBACK_BASE_H + +#include +#include +#include + +// Wifi Connectivity Pin +#define WIFI_PIN 18 + +//UART Pins +#define RX2 16 +#define TX2 17 + +#include + + +/** + * @brief Quarterback Base Subclass Header + * @authors Max Phillips, George Rak, Corbin Hibler + */ +class QuarterbackBase : public Robot { + private: + + Drive* drive; + + /* VARIABLES FOR WIFI CONNECTIVITY + - motor1Value motor2Value: The current drive motor values + - UARTMessage: The string version of the message to send from the base of the QB to it's server ESP also mounted on the base + */ + int motor1Value = 0; + int motor2Value = 0; + String UARTMessage = ""; + + public: + QuarterbackBase (Drive* drive); + void action() override; //! robot subclass must override action + void updateWriteMotorValues(); + void bottomQBSetup(); +}; + +#endif // QUARTERBACK_H diff --git a/src/Robot/QuarterbackTurret.cpp b/src/Robot/QuarterbackTurret.cpp new file mode 100644 index 0000000..8f1a6a2 --- /dev/null +++ b/src/Robot/QuarterbackTurret.cpp @@ -0,0 +1,1263 @@ +#include "QuarterbackTurret.h" + +//This for some reason has to be declared in the .cpp file and not the .h file so that it does not conflict with the same declaration in other .h files +HardwareSerial Uart_Turret(2); // UART2 + +// "define" static members to satisfy linker +uint8_t QuarterbackTurret::turretEncoderPinA; +uint8_t QuarterbackTurret::turretEncoderPinB; +uint8_t QuarterbackTurret::turretEncoderStateB; +int32_t QuarterbackTurret::currentTurretEncoderCount; + +void QuarterbackTurret::turretEncoderISR() { + turretEncoderStateB = digitalRead(turretEncoderPinB); + + if (turretEncoderStateB == 1) { + currentTurretEncoderCount++; + } else if (turretEncoderStateB == 0) { + currentTurretEncoderCount--; + } +} + +QuarterbackTurret::QuarterbackTurret( + uint8_t flywheelLeftPin, // M1 + uint8_t flywheelRightPin, // M2 + uint8_t cradlePin, // M3 + uint8_t turretPin, // M4 + uint8_t assemblyStepPin, // S1 + uint8_t assemblyDirPin, // S2 + uint8_t magnetometerSdaPin, // S3 + uint8_t magnetometerSclPin, // S4 + uint8_t turretEncoderPinA, // E1A + uint8_t turretEncoderPinB, // E1B + uint8_t turretLaserPin // E2A +) { + + // set all state variables to default values, + // except currentAssemblyAngle, currentRelativeHeading, currentRelativeTurretCount + // the positions of these mechanisms are initially unknown and assigned upon reset/homing + + this->enabled = false; // initially disable robot for safety + this->initialized = false; + this->runningMacro = false; + this->targetAssemblyAngle = straight; // while the initial state is unknown, we want it to be straight + this->assemblyMoving = false; // it is safe to assume the assembly is not moving + + this->currentCradleState = forward; // in case the startup state is strange, force the cradle to move back once on startup + this->targetCradleState = back; + this->cradleMoving = false; + this->cradleStartTime = 0; + + this->mode = manual; // start in manual mode by default, auto mode can be enabled by selecting a target + this->target = receiver_1; // the default target is receiver 1, but this has no effect until the mode is switched to automatic + + this->currentFlywheelStage = stopped; // it is safe to assume the flywheels are stopped + this->targetFlywheelStage = stopped; + + this->currentFlywheelSpeed = 0; // it is safe to assume the flywheels are stopped + this->targetFlywheelSpeed = 0; + + this->flywheelManualOverride = false; // until/unless the left stick is active, this is false + + this->currentTurretSpeed = 0; // it is safe to assume the turret is stopped + this->targetTurretSpeed = 0; + + this->targetRelativeHeading = 0; // while the initial heading is unknown, we want the heading to be zero + this->targetTurretEncoderCount = 0; + + this->turretMoving = false; + + this->manualHeadingIncrementCount = 0; + + this->currentAbsoluteHeading = 0; + this->targetAbsoluteHeading = 0; + + this->stickFlywheel = 0; + this->stickTurret = 0; + + // turret laser setup + this->turretLaserPin = turretLaserPin; + this->turretLaserState = 0; + pinMode(turretLaserPin, INPUT_PULLUP); //! will be 1 when at home position or main power is off (the latter is electrically unavoidable) + + // encoder setup + QuarterbackTurret::turretEncoderPinA = turretEncoderPinA; + QuarterbackTurret::turretEncoderPinB = turretEncoderPinB; + QuarterbackTurret::currentTurretEncoderCount = 0; + pinMode(turretEncoderPinA, INPUT_PULLUP); + pinMode(turretEncoderPinB, INPUT); + attachInterrupt(turretEncoderPinA, turretEncoderISR, RISING); + + // initiate motor objects + // TODO: initiate assembly/tilter stepper motor with lib + cradleActuator.setup(cradlePin, big_ampflow); // TODO: change to MotorInterface when merged + turretMotor.setup(turretPin, falcon); // TODO: add encoder + flywheelLeftMotor.setup(flywheelLeftPin, falcon); + flywheelRightMotor.setup(flywheelRightPin, falcon); + + + + // initialize debouncers + this->dbOptions = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + this->dbSquare = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + this->dbDpadUp = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + this->dbDpadDown = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + + this->dbCircle = new Debouncer(QB_CIRCLE_HOLD_DELAY); + this->dbTriangle = new Debouncer(QB_TRIANGLE_HOLD_DELAY); + this->dbCross = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + + this->dbTurretInterpolator = new Debouncer(QB_TURRET_INTERPOLATION_DELAY); + + magnetometerSetup(); + + Uart_Turret.begin(115200, SERIAL_8N1, RX2, TX2); +} + +void QuarterbackTurret::action() { + //! Control Schema + //* Touchpad: Emergency Stop + //* Square: Toggle Flywheels/Turret On/Off (Safety Switch) + // above two inputs are registered in `testForDisableOrStop()` since this is re-used in blocking routines + //! Ignore non-emergency inputs if running a macro + + if (!testForDisableOrStop() && !runningMacro) { + //* Circle: Startup and Home (Reset or Zero Turret) + if (dbCircle->debounceAndPressed(ps5.Circle())) { + if (!initialized) { + reset(); + } else { + zeroTurret(); + } + } + //* Triangle: Macro 1 - load from center + else if (dbTriangle->debounceAndPressed(ps5.Triangle())) { + loadFromCenter(); + } + //* Cross: Macro 2 - handoff to runningback + else if (dbCross->debounceAndPressed(ps5.Cross())) { + handoff(); + } + else if (ps5.Left()) { + testRoutine(); + } + //* Manual and Automatic Controls + else { + //* Right Trigger (R2): Fire (cradle/grabber forward) + // Do not fire unless moving forward (do not fire when intaking or stopped) + if (currentFlywheelSpeed > STICK_DEADZONE && ps5.R2()) { + moveCradle(forward); + } else { + moveCradle(back); + } + + //* Options (Button): Switch Mode (toggle between auto/manual targeting) + if (QB_AUTO_ENABLED && dbOptions->debounceAndPressed(ps5.Options())) { + switchMode(); + } + //* Left Button (L1): Switch Target to Receiver 1 + else if (QB_AUTO_ENABLED && ps5.L1()) { + switchTarget(receiver_1); + } + //* Right Button (R1): Switch Target to Receiver 2 + else if (QB_AUTO_ENABLED && ps5.R1()) { + switchTarget(receiver_2); + } + //* Auto Mode + else if (QB_AUTO_ENABLED && mode == automatic) { + // TODO: Implement auto mode + // do something based on current value of 'targetReceiver' + } + //* Manual Controls + else { + stickFlywheel = (ps5.LStickY() / 127.5f); + stickTurret = (ps5.RStickX() / 127.5f); + + //* Right Stick X: Turret Control + // Left = CCW, Right = CW + if (fabs(stickTurret) > STICK_DEADZONE) { + //Check if magnetometer functionality is enabled + if (useMagnetometer && holdTurretStillEnabled) { + // only change position every 4 loops + if (manualHeadingIncrementCount == 0) { + targetAbsoluteHeading += (1 * copysign(1, stickTurret)); + targetAbsoluteHeading %= 360; + } else { + manualHeadingIncrementCount++; + manualHeadingIncrementCount %= 4; + } + //Serial.print(F("--target abs heading: ")); + //Serial.println(targetAbsoluteHeading); + calculateHeadingMag(); + holdTurretStill(); + } else { + setTurretSpeed(stickTurret * QB_TURRET_STICK_SCALE_FACTOR); + } + } else { + //Check if magnetometer functionality is enabled + if (useMagnetometer && holdTurretStillEnabled) { + calculateHeadingMag(); + holdTurretStill(); + } else { + setTurretSpeed(0); + } + updateTurretMotionStatus(); + } + + // updateTurretMotionStatus(); + + //* Left Stick Y: Flywheel Override + if (fabs(stickFlywheel) > STICK_DEADZONE) { + setFlywheelSpeed(stickFlywheel); + } else { + //* D-Pad Up: Increase flywheel speed by one stage + if (dbDpadUp->debounceAndPressed(ps5.Up())) { + adjustFlywheelSpeedStage(INCREASE); + } + //* D-Pad Down: Decrease flywheel speed by one stage + else if (dbDpadDown->debounceAndPressed(ps5.Down())) { + adjustFlywheelSpeedStage(DECREASE); + } + else { + setFlywheelSpeedStage(currentFlywheelStage); + } + } + } + } + } + + updateReadMotorValues(); + + printDebug(); +} + +// note that because the direction is flipped to be more intuitive for the driver, +// the "positive" direction is reversal/red on the falcon, and the "negative" direction is forwards/green +// positive direction is also positive encoder direction, and vice versa +void QuarterbackTurret::setTurretSpeed(float absoluteSpeed, bool overrideEncoderTare) { + if (enabled) { + targetTurretSpeed = constrain(absoluteSpeed, -1.0, 1.0); + turretMotor.write(-targetTurretSpeed); // flip direction so that + is CW and - is CCW + + // handle mechanical slop when changing directions + if (!overrideEncoderTare) { + turretDirectionChanged(); + } + + currentTurretSpeed = targetTurretSpeed; //! for now, will probably need to change later, like an interrupt + } else { + turretMotor.write(0); + } +} + +void QuarterbackTurret::moveTurret(int16_t heading, bool relativeToRobot, bool ramp) { + moveTurret(heading, degrees, QB_HOME_PCT, relativeToRobot, ramp); +} + +void QuarterbackTurret::moveTurret(int16_t heading, float power, bool relativeToRobot, bool ramp) { + moveTurret(heading, degrees, power, relativeToRobot, ramp); +} + +void QuarterbackTurret::moveTurret(int16_t heading, TurretUnits units, float power, bool relativeToRobot, bool ramp) { + Serial.print(F("moveTurret called with heading =")); + Serial.print(heading); + Serial.print(F(", units = ")); + if (units == degrees) { Serial.print(F("degrees, rel = ")); } else {Serial.print(F("counts, rel = ")); } + Serial.println(relativeToRobot); + if (enabled) { + + // todo + if (relativeToRobot) { + // todo: use encoder + laser to determine position + targetRelativeHeading = heading; + if (units == degrees) { + moveTurret(heading, counts, power, relativeToRobot, ramp); + } else if (units == counts) { + int8_t sign = copysign(1, currentTurretEncoderCount); + // currentTurretEncoderCount = abs(currentTurretEncoderCount); + // currentTurretEncoderCount %= 360; + // currentTurretEncoderCount *= sign; + + targetTurretEncoderCount = heading * QB_COUNTS_PER_TURRET_DEGREE; + turretMoving = true; + // sign now used for target instead of current + sign = 1; // 1 when positive, -1 when negative + // if the current turret encoder count is over halfway to a full rotation in either direction + // if (abs(currentTurretEncoderCount) > (QB_COUNTS_PER_TURRET_REV / 2)) { + // // ensure that the target and current counts are as close as possible + // // if adding one rotation's worth of counts would help, do so + // if (currentTurretEncoderCount - (targetTurretEncoderCount + QB_COUNTS_PER_TURRET_REV) < currentTurretEncoderCount - targetTurretEncoderCount) { + // targetTurretEncoderCount += QB_COUNTS_PER_TURRET_REV; + // } + // } + + if (targetTurretEncoderCount < currentTurretEncoderCount) { + sign = -1; + } + + setTurretSpeed(QB_HANDOFF/4 * sign); //! temp constant, implement P loop soon + delay(100); + setTurretSpeed(QB_HANDOFF/3 * sign); //! temp constant, implement P loop soon + delay(100); + setTurretSpeed(QB_HANDOFF/2 * sign); //! temp constant, implement P loop soon + delay(100); + setTurretSpeed(QB_HANDOFF * sign); //! temp constant, implement P loop soon + + // currentTurretEncoderCount = targetTurretEncoderCount; // currentTurretEncoderCount is updated by interrupt + } + currentRelativeHeading = targetRelativeHeading; + } else { // relative to field + // todo: use magnetometer + } + } else { + setTurretSpeed(0); + } +} + +void QuarterbackTurret::moveTurretAndWait(int16_t heading, float power, bool relativeToRobot, bool ramp) { + moveTurret(heading, power, relativeToRobot, ramp); + while (turretMoving && !testForDisableOrStop()) { + updateTurretMotionStatus(); + delay(10); + } +} + +void QuarterbackTurret::updateTurretMotionStatus() { + // Serial.print(F("update called with ctec = ")); + // Serial.print(currentTurretEncoderCount); + // Serial.print(F("; ttec = ")); + // Serial.println(targetTurretEncoderCount); + // determines if encoder is within "spec" + if (turretMoving && fabs((currentTurretEncoderCount % QB_COUNTS_PER_TURRET_REV) - targetTurretEncoderCount) < QB_TURRET_THRESHOLD) { + turretMoving = false; + setTurretSpeed(0); + } +} + +void QuarterbackTurret::turretDirectionChanged() { + if (currentTurretSpeed > 0 && targetTurretSpeed < 0) { // going CW, trying to go CCW + currentTurretEncoderCount -= slopError; + targetTurretEncoderCount -= slopError; + } else if (currentTurretSpeed < 0 && targetTurretSpeed > 0) { // going CCW, trying to go CW + currentTurretEncoderCount += slopError; + targetTurretEncoderCount += slopError; + } +} + +int16_t QuarterbackTurret::getCurrentHeading() { + return (currentTurretEncoderCount / QB_COUNTS_PER_TURRET_DEGREE) % 360; +} + +// Function to normalize an angle to the range [0, 360) +int QuarterbackTurret::NormalizeAngle(int angle) { + while (angle < 0) { + angle+=360; + } + angle%= 360; // Ensure angle is within [0, 360) range + return angle; +} + +// Function to calculate the shortest rotation direction +// Returns -1 for counterclockwise, 1 for clockwise, or 0 if no rotation needed +int QuarterbackTurret::CalculateRotation(float currentAngle, float targetAngle) { + currentAngle = NormalizeAngle(currentAngle); + targetAngle = NormalizeAngle(targetAngle); + + int positiveDegreeCount = currentAngle; + int negativeDegreeCount = currentAngle; + int iter = 0; + + while (NormalizeAngle(negativeDegreeCount) != targetAngle && NormalizeAngle(positiveDegreeCount) != targetAngle) { + positiveDegreeCount++; + negativeDegreeCount--; + iter++; + } + + if (iter == 0) { + return 0; + } + else if (NormalizeAngle(negativeDegreeCount) == targetAngle) { + return iter; + } else { + return iter*-1; + } +} + +int16_t QuarterbackTurret::findNearestHeading(int16_t targetHeading, int16_t currentHeading) { + // assuming targetHeading is positive + int16_t positiveHeading = targetHeading; + + // if targetHeading is negative, convert to a positive heading + if (targetHeading < 0) { + positiveHeading = targetHeading + 360; + } + + // properly constrain headings to be within (-360, +360) + positiveHeading %= 360; + int16_t negativeHeading = positiveHeading - 360; + if (negativeHeading == -360) // same as if (positiveHeading == 0) + negativeHeading = 0; + + // calculate which heading is closer to the current heading + int16_t adjustedCurrentHeading = currentHeading % 360; + + if (abs(adjustedCurrentHeading - positiveHeading) < abs(adjustedCurrentHeading - negativeHeading)) { + // negative heading is closer + return (adjustedCurrentHeading - positiveHeading); + } else { + // positive heading is closer + return (adjustedCurrentHeading - negativeHeading); + } +} + +int16_t QuarterbackTurret::findNearestHeading(int16_t targetHeading) { + return findNearestHeading(targetHeading, currentRelativeHeading); +} + +void QuarterbackTurret::aimAssembly(AssemblyAngle angle) { + // todo + if (enabled) { + // go to angle + } else { + // write 0 + } +} + +void QuarterbackTurret::moveCradleSubroutine() { + // Serial.print(F("target neq current | ")); + if (targetCradleState == forward) { + // move forwards + cradleActuator.write(1.0); + cradleStartTime = millis(); + cradleMoving = true; + // Serial.print(F("cradle moving forward | ")); + } else if (targetCradleState == back) { + // move backwards + cradleActuator.write(-1.0); + cradleStartTime = millis(); + cradleMoving = true; + // Serial.print(F("cradle moving backward | ")); + } +} + +void QuarterbackTurret::moveCradle(CradleState state, bool force) { + if (enabled) { + if (!cradleMoving) { + targetCradleState = state; + + // Serial.print(F("current state: ")); + // if (currentCradleState == forward) { + // Serial.print(F("forward | ")); + // } else if (currentCradleState == back) { + // Serial.print(F("backward | ")); + // } + + // Serial.print(F("target state: ")); + // if (targetCradleState == forward) { + // Serial.print(F("forward | ")); + // } else if (targetCradleState == back) { + // Serial.print(F("backward | ")); + // } + + if (targetCradleState != currentCradleState || force) { + moveCradleSubroutine(); + } + + //* force is a blocking routine to ensure it works without interruption + //* do not use force frequently as it can strain the actuator + //* this should only be used on startup + if (force) { + // also allow emergency stop + while ((millis() - cradleStartTime) <= QB_CRADLE_TRAVEL_DELAY && !testForDisableOrStop()) { + NOP(); + } + currentCradleState = targetCradleState; + cradleMoving = false; + cradleActuator.write(0); + } + + // Serial.print(F("past delay? ")); + // Serial.print((millis() - cradleStartTime) > QB_CRADLE_TRAVEL_DELAY); + // Serial.print(F(" | ")); + + } else if ((millis() - cradleStartTime) > QB_CRADLE_TRAVEL_DELAY) { + currentCradleState = targetCradleState; + cradleMoving = false; + cradleActuator.write(0); + // Serial.print(F("cradle stopped | ")); + } + } else { + cradleActuator.write(0); + } + + // Serial.println(); +} + +void QuarterbackTurret::setFlywheelSpeed(float absoluteSpeed) { + // update the motors so they are spinning at the new speed + if (enabled) { + // if current speed is not the passed speed, change the motor speed. this is only to avoid unnecessary writes + if (fabs(currentFlywheelSpeed - absoluteSpeed) > STICK_DEADZONE) { + // constrain to the first and last values of the flywheel speed array. + // the first value should be the slow intake speed -- the flywheels should NEVER spin more quickly *inwards* than this. + // the last value should be the maximum speed (ordinarily 1, but we may change this). + targetFlywheelSpeed = constrain(absoluteSpeed, flywheelSpeeds[0], flywheelSpeeds[QB_TURRET_NUM_SPEEDS - 1]); + flywheelLeftMotor.write(targetFlywheelSpeed); + flywheelRightMotor.write(-targetFlywheelSpeed); + currentFlywheelSpeed = targetFlywheelSpeed; //! for now, will probably need to change later, like an interrupt + } + } else { + flywheelLeftMotor.write(0); + flywheelRightMotor.write(0); + } +} + +void QuarterbackTurret::setFlywheelSpeedStage(FlywheelSpeed stage) { + targetFlywheelStage = stage; + setFlywheelSpeed(flywheelSpeeds[static_cast(targetFlywheelStage)]); + currentFlywheelStage = targetFlywheelStage; +} + +void QuarterbackTurret::adjustFlywheelSpeedStage(SpeedStatus speed) { + uint8_t idx = static_cast(currentFlywheelStage); + + // Change the speed stage based on whether the user wants to increase or decrease + if (speed == INCREASE && idx < QB_TURRET_NUM_SPEEDS - 1) { + idx++; + } else if (speed == DECREASE && idx > 0) { + idx--; + } + + setFlywheelSpeedStage(static_cast(idx)); +} + +void QuarterbackTurret::switchMode() { + if (mode == manual) { + switchMode(automatic); + } else if (mode == automatic) { + switchMode(manual); + } +} + +void QuarterbackTurret::switchMode(TurretMode mode) { + this->mode = mode; +} + +void QuarterbackTurret::switchTarget(TargetReceiver target) { + switchMode(automatic); + this->target = target; + // todo: not sure if this needs more functionality? +} + +void QuarterbackTurret::loadFromCenter() { + this->runningMacro = true; + aimAssembly(straight); + setFlywheelSpeedStage(slow_inwards); + moveCradle(back); + // zeroTurret(); + this->runningMacro = false; +} + +void QuarterbackTurret::handoff() { + this->runningMacro = true; + aimAssembly(straight); + int16_t targetHeading = (getCurrentHeading() + 130) % 360; + calculateHeadingMag(); + targetAbsoluteHeading = headingdeg +180; + targetAbsoluteHeading%=360; + + if (useMagnetometer) { + //moveTurretAndWait(targetHeading); + //Use the magnetometer to make sure we get close to the requested angle + //calculateHeadingMag(); + //holdTurretStill(); + //cradleActuator.write(1.0); + setFlywheelSpeedStage(slow_outwards); + long currentTime = millis(); + while ((currentTime + 4000) > millis()) { + calculateHeadingMag(); + turretPIDSpeed = turretPIDController(targetAbsoluteHeading, .01, 0, 0, .25); + setTurretSpeed(turretPIDSpeed, true); + } + cradleActuator.write(1.0); + delay(2000); + } else { + targetHeading += 10; + targetHeading %= 360; + moveTurretAndWait(targetHeading); + cradleActuator.write(1.0); + setFlywheelSpeedStage(slow_outwards); + delay(2000); + setFlywheelSpeedStage(stopped); + } + cradleActuator.write(-1); + delay(2000); + cradleActuator.write(0); + this->runningMacro = false; +} + +void QuarterbackTurret::testRoutine() { + this->runningMacro = true; + Serial.println(F("test routine called")); + Serial.println(F("initial ctec: ")); + Serial.println(currentTurretEncoderCount); + moveTurretAndWait(90); + delay(500); + Serial.println(F("ctec after turn to 90 deg: ")); + Serial.println(currentTurretEncoderCount); + moveTurretAndWait(-90); + delay(500); + Serial.println(F("ctec after turn to -90 deg: ")); + Serial.println(currentTurretEncoderCount); + moveTurretAndWait(180); + delay(500); + Serial.println(F("ctec after turn to 180 deg: ")); + Serial.println(currentTurretEncoderCount); + this->runningMacro = false; +} + +void QuarterbackTurret::zeroTurret() { + this->runningMacro = true; + Serial.println(F("zero called")); + Serial.print(F("STARTING count: ")); + Serial.println(currentTurretEncoderCount); + Serial.println(F("Resetting count to 0")); + currentTurretEncoderCount = 0; + targetRelativeHeading = 0; + + // set speed + setTurretSpeed(QB_HOME_PCT); + + // pin will only read high if the main power is off or the laser sensor is triggered + while ( + digitalRead(turretLaserPin) == LOW && // routine will exit when the laser sensor is triggered + currentTurretEncoderCount < (2 * QB_COUNTS_PER_TURRET_REV) && // routine will exit if it spins around twice without triggering + !testForDisableOrStop() // routine will exit if emergency stop or disable buttons are triggered + ) { + Serial.print(F("zeroing, read = ")); + Serial.print(digitalRead(turretLaserPin)); + Serial.print(F("; cte_count: ")); + Serial.println(currentTurretEncoderCount); + } + + Serial.print(F("laser should read high rn: read = ")); + Serial.println(digitalRead(turretLaserPin)); + + // get values as soon as the laser reads high + int32_t risingEdgeEncoderCount = currentTurretEncoderCount; + int32_t risingEdgeTimestamp = millis(); + + while ( + digitalRead(turretLaserPin) == HIGH && // exit when the laser sensor goes low + (currentTurretEncoderCount - risingEdgeEncoderCount) < (QB_COUNTS_PER_TURRET_REV / 2) && // exit if traveled half a rotation without triggering + !testForDisableOrStop() // exit if emergency stop or disable buttons are triggered + ) { + Serial.print(F("zeroing (stage 2), read = ")); + Serial.print(digitalRead(turretLaserPin)); + Serial.print(F("; cte_count: ")); + Serial.println(currentTurretEncoderCount); + } + + Serial.print(F("laser should read low rn: read = ")); + Serial.println(digitalRead(turretLaserPin)); + + int32_t fallingEdgeEncoderCount = currentTurretEncoderCount; + int32_t fallingEdgeTimestamp = millis(); + + // stop turret + setTurretSpeed(0); + + Serial.print(F("rising: count: ")); + Serial.print(risingEdgeEncoderCount); + Serial.print(F(", time: ")); + Serial.println(risingEdgeTimestamp); + Serial.print(F("falling: count: ")); + Serial.print(fallingEdgeEncoderCount); + Serial.print(F(", time: ")); + Serial.println(fallingEdgeTimestamp); + + // Here lastTurretEncoderCount is used as the value of currentTurretEncoderCount in the previous iteration of the loop + int32_t lastTurretEncoderCount = -currentTurretEncoderCount; // just something different than current + uint16_t stopCounter = 0; + + // wait until turret is stopped + while ( + stopCounter < (QB_TURRET_STOP_THRESHOLD_MS / QB_TURRET_STOP_LOOP_DELAY_MS) && + !testForDisableOrStop() + ) { + // run a counter for how many loop iterations that the last and current are the same. + // if they are the same for a predetermined amount of time (QB_TURRET_STOP_THRESHOLD_MS), + // we assume that the motor has actually stopped. + if (lastTurretEncoderCount == currentTurretEncoderCount) { + stopCounter++; + } else { + stopCounter = 0; + } + + Serial.print(F("last ct: ")); + Serial.print(lastTurretEncoderCount); + Serial.print(F(", current ct: ")); + Serial.print(currentTurretEncoderCount); + Serial.print(F(", stopCt: ")); + Serial.println(stopCounter); + + lastTurretEncoderCount = currentTurretEncoderCount; // update last count + + delay(QB_TURRET_STOP_LOOP_DELAY_MS); // then delay to wait for encoder to update + } + + int32_t restEncoderCount = currentTurretEncoderCount; + int32_t restTimestamp = millis(); + + // at this point, there should be 3 points of data, in increasing order as follows: + // - the point at which the laser was first triggered (when it first went high) + // - the point at which the laser stopped being triggered (when it went low after being high) + // - the point at which the motor stopped moving after being commanded to stop + + // the first point is the point of reference. + // the middle of the first and second points is the target point, where we assume the true zero is. + // the third point tells us how far we are from the second point, i.e., the error caused by the motor not stopping perfectly. + // - this is not needed between the first and second points because the motor does not stop moving. + + // the third point also helps us overcome the mechanical slop issue when changing directions on the turret, + // since we assume that the turret rotates outside the laser triggering range when it is told to stop. + // so, we start rotating in the other direction, then when the laser triggers again, we can account for the slop + // near-perfectly by forcing the current encoder count to the second point when the laser first triggered, + // then continue moving until reaching the halfway point PLUS the difference between the second and third point, + // the latter of which is the number of counts the motor took to stop, so that it should stop exactly on the halfway mark. + + // now, to actually do this, we start moving the motor (which was stopped), but in the opposite direction. + + Serial.println(F("motor stopped, now moving in opposite direction")); + + setTurretSpeed(-QB_HOME_PCT); + + // moving with a positive power increases the current encoder count, and vice versa + // since we are moving with a negative power, the encoder count will be decreasing + + // wait for the laser to trigger (go high) again, then measure the difference + // between the current encoder count and the known falling edge count. + // this value represents the mechanical slop, which can be used later. + // after that, tare the value of the current encoder count to the falling edge count. + + while ( + digitalRead(turretLaserPin) == LOW && // exit when the laser sensor is triggered + !testForDisableOrStop() // exit if emergency stop or disable buttons are triggered + ) { + Serial.print(F("zeroing (stage 3), read = ")); + Serial.print(digitalRead(turretLaserPin)); + Serial.print(F("; cte_count: ")); + Serial.print(currentTurretEncoderCount); + Serial.print(F("; fall_ct: ")); + Serial.println(fallingEdgeEncoderCount); + delay(5); + } + + // at this point, we will record the difference between the current count (physically, at the second point or falling edge) + // and the rest count (third point or stopping point). the current encoder count should be less than the falling edge count + // (past it, if it were to be physically translated) due to the mechanical slop + // int32_t reEntryEncoderCount = currentTurretEncoderCount; + // int32_t reEntryTimestamp = millis(); + + // the error only due to the motor not stopping perfectly is then found by the difference between the first and third points + stopError = restEncoderCount - risingEdgeEncoderCount; + + // calculate the error due to slop + slopError = fallingEdgeEncoderCount - currentTurretEncoderCount; + + + Serial.print(F("stop error: ")); + Serial.print(stopError); + Serial.print(F("; slop error: ")); + Serial.println(slopError); + + // then, we tare the current count to the third point (falling edge), since we assume it is there + currentTurretEncoderCount = fallingEdgeEncoderCount; + + // find the theoretical midpoint, then add the stop error to get the target count + // int32_t targetCount = ((fallingEdgeEncoderCount + risingEdgeEncoderCount) / 2) - stopError; // for if we change directions again + int32_t targetCount = ((fallingEdgeEncoderCount + risingEdgeEncoderCount) / 2) + (stopError * QB_TURRET_HOME_STOP_FACTOR); + + Serial.print(F("target count: ")); + Serial.println(targetCount); + + // finally, move to the target count, then stop + // setTurretSpeed(QB_HOME_PCT); + + + while ( + // currentTurretEncoderCount < targetCount && + currentTurretEncoderCount > targetCount && + !testForDisableOrStop() + ) { + Serial.print(F("zeroing (stage 4), read = ")); + Serial.print(digitalRead(turretLaserPin)); + Serial.print(F("; cte_count: ")); + Serial.print(currentTurretEncoderCount); + Serial.print(F("; target_ct: ")); + Serial.print(targetCount); + Serial.print(F("; current_ct > target_ct? = ")); + Serial.println(currentTurretEncoderCount > targetCount); + delay(5); + } + + // stop turret and tare everything + setTurretSpeed(0); + currentRelativeHeading = 0; + currentTurretEncoderCount = 0; + Serial.println(F("zeroed")); + + //Now that the encoder is zeroed we can just zero the magnetometer + if (useMagnetometer) { + delay(250); + calibMagnetometer(); + } + + this->runningMacro = false; +} + +void QuarterbackTurret::reset() { + this->enabled = true; + this->runningMacro = true; + moveCradle(back, true); // force + // loadFromCenter(); + zeroTurret(); // temp: just zero + this->initialized = true; + this->runningMacro = false; +} + +bool QuarterbackTurret::testForDisableOrStop() { + //* Touchpad: Emergency Stop + if (ps5.Touchpad()) { + emergencyStop(); + Serial.println(F("emergency stopping")); + return true; + } + //* Square: Toggle Flywheels/Turret On/Off (Safety Switch) + else if (dbSquare->debounceAndPressed(ps5.Square())) { + if (!enabled) { + setEnabled(true); + Serial.println(F("setting enabled")); + } else { + setEnabled(false); + Serial.println(F("setting disabled")); + } + return true; + } + else { + // Serial.println(F("not disabling or stopping")); + return false; + } +} + +void QuarterbackTurret::setEnabled(bool enabled) { + this->enabled = enabled; +} + +void QuarterbackTurret::emergencyStop() { + this->enabled = false; + setFlywheelSpeed(0); // this will not change the state variables since the bot is disabled + setTurretSpeed(0); + cradleActuator.write(0); + // TODO: stop assembly stepper motor +} + +void QuarterbackTurret::printDebug() { + /* + Serial.print(F("enabled: ")); + Serial.print(enabled); + Serial.print(F(" | stickTurret: ")); + Serial.print(stickTurret); + Serial.print(F(" | stickFlywheel: ")); + Serial.print(stickFlywheel); + Serial.print(F(" | currentTurretSpeed: ")); + Serial.println(currentTurretSpeed); + */ + if (enabled) { + /* + Serial.print(F("turretLaserState: ")); + Serial.print(digitalRead(turretLaserPin)); + Serial.print(F("; currentTurretEncoderCount: ")); + Serial.println(currentTurretEncoderCount); + */ + } +} + +/** + * @brief Sets up magnetometer + * @authors Rhys Davies, Corbin Hibler + * @date 2024-01-03 +*/ +void QuarterbackTurret::magnetometerSetup() { + if (! lis3mdl.begin_I2C()) { + // hardware I2C mode, can pass in address & alt Wire + //if (! lis3mdl.begin_SPI(LIS3MDL_CS)) { // hardware SPI mode + //if (! lis3mdl.begin_SPI(LIS3MDL_CS, LIS3MDL_CLK, LIS3MDL_MISO, LIS3MDL_MOSI)) { // soft SPI + Serial.println("Failed to find LIS3MDL chip"); + } + Serial.println("LIS3MDL Found!"); + + lis3mdl.setPerformanceMode(LIS3MDL_MEDIUMMODE); + Serial.print("Performance mode set to: "); + switch (lis3mdl.getPerformanceMode()) { + case LIS3MDL_LOWPOWERMODE: Serial.println("Low"); break; + case LIS3MDL_MEDIUMMODE: Serial.println("Medium"); break; + case LIS3MDL_HIGHMODE: Serial.println("High"); break; + case LIS3MDL_ULTRAHIGHMODE: Serial.println("Ultra-High"); break; + } + + lis3mdl.setOperationMode(LIS3MDL_CONTINUOUSMODE); + Serial.print("Operation mode set to: "); + // Single shot mode will complete conversion and go into power down + switch (lis3mdl.getOperationMode()) { + case LIS3MDL_CONTINUOUSMODE: Serial.println("Continuous"); break; + case LIS3MDL_SINGLEMODE: Serial.println("Single mode"); break; + case LIS3MDL_POWERDOWNMODE: Serial.println("Power-down"); break; + } + + lis3mdl.setDataRate(LIS3MDL_DATARATE_155_HZ); + // You can check the datarate by looking at the frequency of the DRDY pin + Serial.print("Data rate set to: "); + switch (lis3mdl.getDataRate()) { + case LIS3MDL_DATARATE_0_625_HZ: Serial.println("0.625 Hz"); break; + case LIS3MDL_DATARATE_1_25_HZ: Serial.println("1.25 Hz"); break; + case LIS3MDL_DATARATE_2_5_HZ: Serial.println("2.5 Hz"); break; + case LIS3MDL_DATARATE_5_HZ: Serial.println("5 Hz"); break; + case LIS3MDL_DATARATE_10_HZ: Serial.println("10 Hz"); break; + case LIS3MDL_DATARATE_20_HZ: Serial.println("20 Hz"); break; + case LIS3MDL_DATARATE_40_HZ: Serial.println("40 Hz"); break; + case LIS3MDL_DATARATE_80_HZ: Serial.println("80 Hz"); break; + case LIS3MDL_DATARATE_155_HZ: Serial.println("155 Hz"); break; + case LIS3MDL_DATARATE_300_HZ: Serial.println("300 Hz"); break; + case LIS3MDL_DATARATE_560_HZ: Serial.println("560 Hz"); break; + case LIS3MDL_DATARATE_1000_HZ: Serial.println("1000 Hz"); break; + } + + lis3mdl.setRange(LIS3MDL_RANGE_4_GAUSS); + Serial.print("Range set to: "); + switch (lis3mdl.getRange()) { + case LIS3MDL_RANGE_4_GAUSS: Serial.println("+-4 gauss"); break; + case LIS3MDL_RANGE_8_GAUSS: Serial.println("+-8 gauss"); break; + case LIS3MDL_RANGE_12_GAUSS: Serial.println("+-12 gauss"); break; + case LIS3MDL_RANGE_16_GAUSS: Serial.println("+-16 gauss"); break; + } + + lis3mdl.setIntThreshold(500); + lis3mdl.configInterrupt(false, false, true, // enable z axis + true, // polarity + false, // don't latch + true); // enabled! +} + +/** + * @brief Spins the turret 360 degrees slowly to allow magnetometer to calibrate itself on startup + * @author George Rak + * @date 4-9-2024 +*/ +void QuarterbackTurret::calibMagnetometer() { + + yVal = 0; + xVal = 0; + maxX = -1000000; + minX = 1000000; + xHalf = 0; + maxY = -1000000; + minY = 1000000; + yHalf = 0; + xsign = false; + ysign = false; + + northHeadingDegrees = 0; + + int degreesMove = 360; + targetTurretEncoderCount = degreesMove * QB_COUNTS_PER_TURRET_DEGREE; + turretMoving = true; + setTurretSpeed(QB_HOME_MAG * copysign(1, degreesMove), true); + //Loop until the target encoder count has been achieved + while (currentTurretEncoderCount < targetTurretEncoderCount && !testForDisableOrStop()){ + // get X Y and Z data all at once + lis3mdl.read(); + + //Constantly looking for min and max values of X + if (lis3mdl.x < minX && lis3mdl.x != -1 && lis3mdl.x != 0) {minX = lis3mdl.x;} + else if (lis3mdl.x > maxX && lis3mdl.x != -1 && lis3mdl.x != 0) {maxX = lis3mdl.x;} + + //Adjusting X values to range from + or - values rather than all positive + xHalf = abs(maxX) - abs(minX); + xHalf/= 2; + xHalf+= abs(minX); + + //Constantly looking for min and max values of Y + if (lis3mdl.y < minY && lis3mdl.y != -1 && lis3mdl.y != 0 && lis3mdl.y != 10) {minY = lis3mdl.y;} + else if (lis3mdl.y > maxY && lis3mdl.y != -1 && lis3mdl.y != 0 && lis3mdl.y != 10) {maxY = lis3mdl.y;} + + //Adjusting Y values to range from + or - values rather than all positive + yHalf = abs(maxY) - abs(minY); + yHalf/= 2; + yHalf+= abs(minY); + + /*DEBUGGING PRINTOUTS*/ + //Serial.print("X: "); Serial.print(lis3mdl.x); + //Serial.print("\tY: "); Serial.print(lis3mdl.y); + //Serial.print("\tMinX: "); Serial.print(minX); + //Serial.print("\tMaxX: "); Serial.print(maxX); + //Serial.print("\tMinY: "); Serial.print(minY); + //Serial.print("\tMaxY: "); Serial.print(maxY); + //Serial.println(); + } + + setTurretSpeed(0, true); + + //Updating variables that will be used to handle other two possible sign cases for each value + if ((maxX+minX) < 0) { + xsign = true; + } + if ((maxY+minY) < 0) { + ysign = true; + } + // + + calculateHeadingMag(); + + Serial.print(F("Magnetometer reading after calib: ")); + Serial.print(headingdeg); + Serial.print(F("\tEncoder after calib:")); + Serial.print(currentTurretEncoderCount); + + // delay(5000); + + currentTurretEncoderCount = 0; + targetTurretEncoderCount = 0; + + turretMoving = true; + moveTurretAndWait(0, true); // go to zero of the encoder + + magnetometerCalibrated = true; + + calculateHeadingMag(); // calculate current value of magnetometer (headingdeg) + + Serial.print("Target Abs Heading Before 0: "); + Serial.print(targetAbsoluteHeading); + Serial.print("\tNorth Heading Degrees: "); + this->northHeadingDegrees = headingdeg;// + 45; + Serial.print(northHeadingDegrees); + Serial.println(); + + // delay(2000); + + // from here on out, headingdeg and targetAbsoluteHeading are offset by northHeadingDegrees + // headingdeg = 0; + targetAbsoluteHeading = 0; + + Serial.println("Magnetometer has been calibrated!"); + eIntegral = 0; + previousTime = millis(); + + // delay(5000); +} + +/** + * @brief Uses the data collected at calibration to calculate the current heading relative to magnetic north + * @author George Rak + * @date 4-9-2024 +*/ +void QuarterbackTurret::calculateHeadingMag() { + //Only run the code in here if the calibration has been done to the magnetometer + if (magnetometerCalibrated) { + lis3mdl.read(); + //Calculate the current angle of the turret based on the calibration data + if (xsign) { + xVal = lis3mdl.x + xHalf; + } else { + xVal = lis3mdl.x - xHalf; + } + + if (ysign) { + yVal = lis3mdl.y + yHalf; + } else { + yVal = lis3mdl.y - yHalf; + } + + //Evaluate both ranges of X and Y then scale the smaller value to be within the same range as the larger + if (yHalf > xHalf) { + xVal = (double)((double)xVal/((double)xHalf))*(double)yHalf; + } else if (xHalf > yHalf) { + yVal = (double)((double)yVal/((double)yHalf))*(double)xHalf; + } + + //Calculate angle in radians + if (xVal != -1 && xVal !=0 && yVal != 0 && yVal != -1) { + headingrad = atan2(yVal, xVal); + } + + //Convert to degrees + headingdeg = headingrad*180/M_PI; + + //If the degrees are negative then they just need inversed plus 180 + if (headingdeg < 0) { + headingdeg += 360; + } + + // integrate offset into measurement + headingdeg = ((int) headingdeg) /*+ 180 /*- QB_NORTH_OFFSET -*/ + northHeadingDegrees; + 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(minX); + //Serial.print("\tMaxX: "); Serial.print(maxX); + //Serial.print("\tMinY: "); Serial.print(minY); + //Serial.print("\tMaxY: "); Serial.print(maxY); + //Serial.print("\txAdapt: "); Serial.print(xVal); + //Serial.print("\tyAdapt: "); Serial.print(yVal); + //Serial.print("\tHeading [deg]: "); Serial.print(headingdeg); + //Serial.println(); + } +} + +/** + * @brief Checks if the turret should be held still and runs the PID loop setting turret speed equal to PWM value calculated + * @author George Rak + * @date 4-9-2024 +*/ +void QuarterbackTurret::holdTurretStill() { + if (magnetometerCalibrated) { + int maxSpeed = .2; + if (motor1Value > 25 || motor2Value > 25) { + //We should limit the rotation rate of the turret since the base is moving as well and we don't want the robot to flip + maxSpeed = .125; + } + turretPIDSpeed = turretPIDController(targetAbsoluteHeading, kp, kd, ki, maxSpeed); + setTurretSpeed(turretPIDSpeed, true); + } +} + +/** + * @brief PID controller to hold the turret still (gains tuned, not calculated) + * @author George Rak + * @date 4-9-2024 +*/ +float QuarterbackTurret::turretPIDController(int setPoint, float kp, float kd, float ki, float maxSpeed) { + if (maxSpeed > .5) { maxSpeed = .5; } + else if (maxSpeed < -.5) { maxSpeed = -.5; } + + //Measure the time elapsed since last iteration + long currentTime = millis(); + float deltaT = ((float)(currentTime - previousTime)); + + //PID loops should update as fast as possible but if it waits too long this could be a problem + if (deltaT > QB_TURRET_PID_MIN_DELTA_T && deltaT < QB_TURRET_PID_MAX_DELTA_T) { + + //Find which direction will be closer to requested angle + int e = CalculateRotation(headingdeg, targetAbsoluteHeading); + + //Taking the average of the error + prevErrorVals[prevErrorIndex] = e; + prevErrorIndex++; + prevErrorIndex %= errorAverageLength; + + //For the first one populate the average so it does not freak out + if (firstAverage) { + for (int i = 0; i maxSpeed) { u = maxSpeed; } + else if (u < -maxSpeed) { u = -maxSpeed; } + + // If PWM value is less than the minimum PWM value needed to move the robot, + if (abs(u) < QB_MIN_PWM_VALUE) { u = 0.0; } + + // If the robot gets within an acceptable range then send error etc to 0 + if (abs(e) < QB_TURRET_PID_THRESHOLD) { + e = 0; + eDerivative = 0; + eIntegral = 0; + ePrevious = 0; + } + + //Serial.print("DeltaT: "); Serial.print(deltaT); + Serial.print("\tError: [deg]: "); Serial.print(e); + Serial.print("\tP: "); Serial.print((kp*e), 4); + Serial.print("\tI: "); Serial.print((ki * eIntegral), 4); + //Serial.print("\tD:\t"); Serial.print((kd*eDerivative) , 4); + Serial.print("\tPWM Value: "); Serial.print(u , 4); + Serial.print("\tCurrent [deg]: "); Serial.print(headingdeg, 0); + Serial.print("\tTarget [deg]: "); Serial.print(targetAbsoluteHeading); + Serial.println(); + + // Update variables for next iteration + previousTime = currentTime; + ePrevious = e; + + // Constrain the values that are sent to the motor while keeping sign + u = copysign(constrain(abs(u), 0, 1), u); // TODO: maybe not necessary? + if (e == 0) { u = 0.0; } + + return -u; + } else if (deltaT > QB_TURRET_PID_BAD_DELTA_T) { + //Drop the value if the time since last loop is too high so that errors don't spike + previousTime = currentTime; + return turretPIDSpeed; + } else { + //If the loop runs faster than the minimum time just return the last value and wait for next loop + return turretPIDSpeed; + } +} + +/** + * @brief Reads a UART communication from the other ESP mounted to the turret. This ESP currently provides the speed of both motors on the drivetrain so we know if the robot is moving + * @author George Rak + * @date 5-14-2024 +*/ +void QuarterbackTurret::updateReadMotorValues() { + recievedMessage = ""; + //While there are characters available in the buffer read each one individually + while (Uart_Turret.available()) { + char character = Uart_Turret.read(); + //Added a delimeter between messages since loop times are different and multiple messages might come in before they are read and the buffer is cleared + //Since they are coming so fast and there is no need to remember past values only the most recent is kept + if (character == '~') { + if (Uart_Turret.available()) { + recievedMessage = ""; + } + } else { + recievedMessage += character; + } + } + //The Server client relationship between the ESPs knows if they disconnect so it is possible that they might send DISCONNECTED over the communication instead of values, in this case set the value to the max so that the turret spins slower + if (recievedMessage!="") { + if (recievedMessage == "DISCONNECTED") { + motor1Value = 100; + motor2Value = 100; + } else { + //Doing some string formatting here, a delimiter was added between the data to help keep them separate for motor #1 and motor #2 + motor1Value = (recievedMessage.substring(0, recievedMessage.indexOf('&'))).toInt(); + motor2Value = (recievedMessage.substring(recievedMessage.indexOf('&') + 1)).toInt(); + } + } + Serial.print("Motor1: "); + Serial.print(motor1Value); + Serial.print("\tMotor2: "); + Serial.print(motor2Value); + Serial.println(); +} \ No newline at end of file diff --git a/src/Robot/QuarterbackTurret.h b/src/Robot/QuarterbackTurret.h new file mode 100644 index 0000000..b4dc30a --- /dev/null +++ b/src/Robot/QuarterbackTurret.h @@ -0,0 +1,444 @@ +/*Max Statements + Question: Why are there so many #defines commented out? + Answer: Documentation purposes. + + Question: Why not just use the #defines as they are written to generate the values? + Answer: I don't trust the preprocessor. -MP + +*/ + +#pragma once + +#ifndef QUARTERBACK_TURRET_H +#define QUARTERBACK_TURRET_H + +#include +#include +#include // ESP PS5 library, access using global instance `ps5` +#include +#include +#include + +enum TurretMode { + manual, automatic +}; + +enum TurretUnits { + degrees, counts +}; + +enum AssemblyAngle { + straight, angled +}; + +enum CradleState { + forward, back +}; + +enum TargetReceiver { + receiver_1, receiver_2 +}; + +enum FlywheelSpeed { + slow_inwards, stopped, slow_outwards, lvl1_outwards, lvl2_outwards, lvl3_outwards, maximum +}; +#define QB_TURRET_NUM_SPEEDS 7 +const float flywheelSpeeds[QB_TURRET_NUM_SPEEDS] = {-0.1, 0, 0.1, 0.3, 0.5, 0.7, 1.0}; + +/*DEBOUNCE & OTHER DELAY CONSTANTS + - 50 ms for default delay (50L) + - 750 ms to fully extend or retract the linear actuator +*/ +#define QB_BASE_DEBOUNCE_DELAY 50L +#define QB_CRADLE_TRAVEL_DELAY 750L +#define QB_CIRCLE_HOLD_DELAY 750L +#define QB_TRIANGLE_HOLD_DELAY 200L +#define QB_CROSS_HOLD_DELAY 200L +#define QB_TURRET_INTERPOLATION_DELAY 5L +#define QB_TURRET_THRESHOLD 35 +#define QB_TURRET_STICK_SCALE_FACTOR 0.25 + +/*SPEED CONSTANTS*/ +#define QB_MIN_PWM_VALUE 0.08 +#define QB_HOME_PCT 0.125 +#define QB_HANDOFF 0.3 +#define QB_HOME_MAG 0.1 + +/*TURRET ANGLE CALCULATION CONSTANTS + - QB_COUNTS_PER_ENCODER_REV Number of ticks per encoder revolution + - QB_COUNTS_PER_TURRET_REV 27:1 falcon to turret | 5:1 falcon to encoder (12t driving sprocket on 60t gear) | Encoder spins 5.4 times (27/5) for every turret revolution = 5400 ticks per rev + - QB_COUNTS_PER_TURRET_DEGREE (5400/360) = 15 ticks per degree + - QB_TURRET_SLOP_COUNTS Backlash between input and output on the turret is high -> leads to problems when switching directions = 540 ticks on encoder before turret actually starts to move (Empirically measured) +*/ +#define QB_COUNTS_PER_ENCODER_REV 1000 +#define QB_COUNTS_PER_TURRET_REV 5400 +#define QB_COUNTS_PER_TURRET_DEGREE 15 +#define QB_TURRET_SLOP_COUNTS 540 +// #define QB_FALCON_TO_TURRET_RATIO 27 / 1 +// #define QB_ENCODER_TO_FALCON_RATIO 5 / 1 +// #define QB_ENCODER_TO_TURRET_RATIO /* = */ QB_FALCON_TO_TURRET_RATIO / QB_ENCODER_TO_FALCON_RATIO // (27 / 5) = 5.4 +// #define QB_COUNTS_PER_TURRET_REV /* = */ QB_ENCODER_TO_TURRET_RATIO * QB_COUNTS_PER_ENCODER_REV // 5400 +// #define QB_COUNTS_PER_TURRET_DEGREE /* = */ QB_COUNTS_PER_TURRET_REV / 360 +// #define QB_TURRET_SLOP_GEAR_TEETH /* = */ 10 // slop is about 10 gear teeth +// #define QB_TURRET_SLOP_PCT /* = */ QB_DIRECTION_CHANGE_SLOP_GEAR_TEETH / 100 +// #define QB_TURRET_SLOP_COUNTS /* = */ QB_DIRECTION_CHANGE_SLOP_PCT * QB_COUNTS_PER_TURRET_REV + +/*TURRET HOMING CONSTANTS + -QB_TURRET_STOP_LOOP_DELAY_MS + -QB_TURRET_STOP_THRESHOLD_MS must be a multiple of QB_TURRET_STOP_LOOP_DELAY_MS + -QB_TURRET_HOME_STOP_FACTOR correction constant for homing, multiplied into stop counts in final homing + -QB_TURRET_MANUAL_CONTROL_FACTOR higher values = less sensitive during manual control (Basically divides the clock of the loop) +*/ +#define QB_TURRET_STOP_LOOP_DELAY_MS 10 +#define QB_TURRET_STOP_THRESHOLD_MS 500 +#define QB_TURRET_HOME_STOP_FACTOR 0.5 +#define QB_TURRET_MANUAL_CONTROL_FACTOR 4 + +/*TURRET PID CONTROLLER CONSTANTS + -QB_TURRET_PID_THRESHOLD Acceptable error in position control (degrees) that will zero the error constants / build up + -QB_TURRET_PID_MIN_DELTA_T If PID error values are not updated fast enough example the controller hangs up for half a second then the PWM values will be massive compared to what they should be so just kind of reset everything + -QB_TURRET_PID_MAX_DELTA_T Maximum time between updates + -QB_TURRET_PID_BAD_DELTA_T Maximum time before we need to zero things out + -QB_NORTH_OFFSET Added to deal with the problems of zeroing the turret but then holding a set angle afterwards +*/ +#define QB_TURRET_PID_THRESHOLD 3 +#define QB_TURRET_PID_MIN_DELTA_T 5 +#define QB_TURRET_PID_MAX_DELTA_T 25 +#define QB_TURRET_PID_BAD_DELTA_T 250 +#define QB_NORTH_OFFSET 0 + +// Enable or Disable Auto Mode for testing +#define QB_AUTO_ENABLED false + +/*UART COMMUNICATION PINS + -RX2 Used to Recieve data + -TX2 Used to Transmit data +*/ +#define RX2 16 +#define TX2 17 + + +/** + * @brief Quarterback Turret Subclass Header + * @authors Maxwell Phillips + */ +class QuarterbackTurret : public Robot { + private: + + /*MOTOR INSTANCES*/ + MotorControl cradleActuator; + MotorControl turretMotor; + MotorControl flywheelLeftMotor; + MotorControl flywheelRightMotor; + + /*PIN DECLARATIONS (PERSISTENT)*/ + static uint8_t turretEncoderPinA; + static uint8_t turretEncoderPinB; + uint8_t turretLaserPin; + + /*JOYSTICK INPUTS + -stickTurret used to normalize stick input from [0, 255] to [-1.0, 1.0] + -stickFlywheel same as above + */ + float stickTurret; + float stickFlywheel; + + /*MODES AND CAPSTONE INTEGRATIONS + -mode default manual + -target default receiver_1 + */ + TurretMode mode; + TargetReceiver target; + + /*MSC VARIABLES + -enabled default false, set true when homing / reset complete (toggleable via function) + -initialized default false, set true when homing / reset complete (stays true) -> decides whether or not to just run zero turret or entire setup routine + -runningMacro default false, set true while a macro is running + */ + bool enabled; + bool initialized; + bool runningMacro; + + /*FALCON PYLONS + -currentAssemblyAngle initial state is unknown, will reset to straight + -targetAssemblyAngle defaults to straight + -assemblyMoving defaults to false + */ + AssemblyAngle currentAssemblyAngle; + AssemblyAngle targetAssemblyAngle; + bool assemblyMoving; + + /*CRADEL + -currentCradleState initial state is unknown, will reset to back + -targetCradleState defaults to back + -cradleMoving defaults to false + -cradleStartTime + */ + CradleState currentCradleState; + CradleState targetCradleState; + bool cradleMoving; + uint32_t cradleStartTime; + + + /*FLYWHEEL + -currentFlywheelStage defaults to stopped + -targetFlywheelStage defaults to stopped + -currentFlywheelSpeed defaults to 0 + -targetFlywheelsSpeed defaults to 0 + -flywheelManualOverride defaults to false, true when stick is controlling flywheel + */ + FlywheelSpeed currentFlywheelStage; + FlywheelSpeed targetFlywheelStage; + float currentFlywheelSpeed; + float targetFlywheelSpeed; + bool flywheelManualOverride; + + /*TURRET + -currentTurretSpeed default = 0 + -targetTurretSpeed default = 0 + */ + float currentTurretSpeed; + float targetTurretSpeed; + + /*ROBOT RELATIVE HEADINGS + -currentRelativeHeading default is undefined + -targetrelativeHeading default = 0 + -currentRelativeTurretCount default is undefined + -targetTurretEncoderCount default = 0 + -errorEncoderCount the error (current - target) + -slopError any error due to mechanical backlash in the gears, set by robot during homing / reset + -stopError number of encoder counts needed for motor to stop moving + -turretMoving set to true when the turret is moving asynchronously or in the normal program + -manualHeadingIncrementCount default = 0 + */ + int16_t currentRelativeHeading; + int16_t targetRelativeHeading; + int32_t currentRelativeTurretCount; + int32_t targetTurretEncoderCount; + int32_t errorEncoderCount; + int32_t slopError; + int32_t stopError; + bool turretMoving; + uint8_t manualHeadingIncrementCount; + + /*WORLD RELATIVE HEADINGS + -currentAbsoluteHeading default = 0 + -targetAbsoluteHeading default = 0 + -turretLaserState triggered or not + */ + int16_t currentAbsoluteHeading; + int16_t targetAbsoluteHeading; + uint8_t turretLaserState; + + /*DEBOUNCERS*/ + Debouncer* dbOptions; + Debouncer* dbSquare; + Debouncer* dbDpadUp; + Debouncer* dbDpadDown; + + //for delay + Debouncer* dbCircle; + Debouncer* dbTriangle; + Debouncer* dbCross; + Debouncer* dbTurretInterpolator; + + /******* MAGNETOMETER ******/ + Adafruit_LIS3MDL lis3mdl; + bool useMagnetometer = true; //Change this if you want to use the magnetometer or any of it's functions + bool holdTurretStillEnabled = true; //Change this if you only want to use the magnetometer for the handoff and not the hold steady + + /* Magnetometer calibration variables used at startup each time + - yVal, xVal: The current x and y values read by the magnetometer (after adjustments) + - maxX, minX: During calibration the max and min X values are recorded + - maxY, minY: During calibration the max and min Y values are recorded + - xHalf, yHalf: Half of the total range of expected x and Y values (Usesd to shift values back to origin [0,0]) + - xsign, ysign: Determines whether to add to the half value or subtract from it when shifting values back to origin (true = subtract) + */ + int yVal = 0; + int xVal = 0; + int maxX = -1000000; + int minX = 1000000; + int xHalf = 0; + int maxY = -1000000; + int minY = 1000000; + int yHalf = 0; + bool xsign = false; + bool ysign = false; + int northHeadingDegrees = 0; + + /* Magnetometer current heading calculations + - headingrad: The current calculated heading in radians using the X and Y values after calibration + - headingdeg: The current calculated headign in degrees -> uses headingrad + */ + float headingrad; + float headingdeg; + + /* Magnetometer PID calculations + - previousTime: Used to calcualte errors and deltaT in PID function + - ePrevious: The error builds as time goes on, this is used to record the previous and new error values + - eIntegral: The current error integral calculated, will be added to ePrevious in PID loop + - kp: Proportional gain used in PID + - ki: Integral gain used in PID + - kd: Derivative gain used in PID + - turretPIDSpeed: The calculated PWM value used in the PID loop + - prevErrorVals: Array of previous error values used to average the last few together to smooth out random spikes in readings from magnetometer + - prevErrorIndex: The current index gets replaced with the new error value cycling through the entire array over time + - errorAverageLength: The length of the array used for averaging the error + - firstAverage: On the first error calculations the array is filled with zeros so this accounts for that by filling the entire array with the current reading + - minMagSpeed: Small PWM signals fail to make the motor turn leading to error in PID calculations, this sets a bottom bound on the PWM signal that can be calculated by the PID loop + */ + long previousTime = 0; + float ePrevious = 0; + float eIntegral = 0; + float kp = .005; + float ki = 0.0012; + float kd = 0.0; + float turretPIDSpeed = 0; + int prevErrorVals[5] = {0,0,0,0,0}; + int prevErrorIndex = 0; + int errorAverageLength = 5; + bool firstAverage = true; + float minMagSpeed = .075; + + /* Magnetometer functions: + - magnetometerSetup Sets some of the parameters runs once + - calibMagnetometer Rotates turret once on startup recording readings and setting values that will move and scale outputs to be correct angle + - calculateHeadingMag Uses the values calculated during calibration to find the current heading of the turret with respect to the original 0 position + - holdTurretStill Checks if the calibration has been completed and hold turret stil is enabled then enables the PID loop + - turretPIDController PID loop that drives turret to the current setpoint + */ + void magnetometerSetup(); + void calibMagnetometer(); + void calculateHeadingMag(); + void holdTurretStill(); + float turretPIDController(int setPoint, float kp, float kd, float ki, float maxSpeed); + + /* MAGNETOMETER CURRENT STATE NOTES / PLAN + - the PID controller works pretty well, tested on table rotating quickly + - Tested PID controller on field and it immidiately flipped so we need to tune it so that the robot does not tip itself over + - Magnetometer mount needs redesigned and printed to be more stable / protective of the magnetometer and wires (Should probably solder wires to magnetometer for best reliability) + - Ability to change holding angle of turret with joystick needs evaluated for correctness + - Turret flywheel equation needs generated for requested distance + - Nathan capstone integration needs done to control angle and thorw distance + - Testing needs done to see how much flywheels beign on affects magnetometer + - Relative velocities should be taken into account with trajectory calculations + */ + + /*UART VARIABLES*/ + String recievedMessage = ""; + + + /*MSC PRIVATE FUNCTION DECLARATIONS + -moveCradleSubroutine Avoids code duplication on ball fondler + -moveTurret Moves turret / turntable to specific heading (relative to robot not the field) + -updateTurretMotionStatus Allows for asynchronous turret movements (Stops when turret reaches target position) + -getCurrentHeading Calculates current heading from the current encoder count + -findNearestHeading Finds the smallest absolute value difference from either the current or provided angle (overloaded function) [Ex: findNearestHeading(270, 0) returns -90] + -NormalizeAngle Used in angle calculations to take the angle back to 0-360 value (can't use modulus on negative numbers without adverse effects) + -CalculateRotation helperfunction for findNearestHeading + */ + void moveCradleSubroutine(); + void moveTurret(int16_t heading, TurretUnits units, float power = QB_HOME_PCT, bool relativeToRobot = true, bool ramp = false); + void updateTurretMotionStatus(); + int16_t getCurrentHeading(); + int16_t findNearestHeading(int16_t targetHeading, int16_t currentHeading); + int16_t findNearestHeading(int16_t targetHeading); + int NormalizeAngle(int angle); + int CalculateRotation(float currentAngle, float targetAngle); + + public: + QuarterbackTurret( + uint8_t flywheelLeftPin, // M1 + uint8_t flywheelRightPin, // M2 + uint8_t cradlePin, // M3 + uint8_t turretPin, // M4 + uint8_t assemblyStepPin, // S1 + uint8_t assemblyDirPin, // S2 + uint8_t magnetometerSdaPin, // S3 + uint8_t magnetometerSclPin, // S4 + uint8_t turretEncoderPinA, // E1A + uint8_t turretEncoderPinB, // E1B + uint8_t turretLaserPin // E2A + ); + + bool magnetometerCalibrated = false; + + /*PUBLIC MSC AND SETUP FUNCTION DECLARATIONS + -action robot sublass must override action + -zeroTurret Runs through the routine to zero the turret + -reset zero turret, aim down (straight), and set flywheels to slow intake + -setEnabled toggles turret and flywheel movement + -emergencyStop If the E-Stop button on controller is pressed this should override every other motor function and force them to stop + -testForDisableOrStop Tests what state the motors should be in + -printDebug Consolidating printouts for the different functions into one locations for debugging purposes + */ + void action() override; + void zeroTurret(); + void reset(); + void setEnabled(bool enabled); + void emergencyStop(); + bool testForDisableOrStop(); + void printDebug(); + + /*QB DIRECT CONTROL FUNCTION DELCARATIONS + -setTurretSpeed Moves the turret at a specified speed (open loop) + -moveTurret Turn the turret to a specific heading (currently relative to robot) -> asynchronous + -moveTurretAndWait Moves the turret and waits until the heading is reached -> synchronous + -aimAssembly Moves the falcon pylon assembly up and down between two states + -moveCradle Moves teh cradel between two states throw and hold + -setFlywheelSpeed Sets the speed of the flywheels using stick inputs + -setFlywheelSpeedStage Set the speed as one of the defined enums + -adjustFlywheelSpeedStage Move to the next level up or down in the list of speeds + */ + void setTurretSpeed(float absoluteSpeed, bool overrideEncoderTare = false); + void moveTurret(int16_t heading, bool relativeToRobot = true, bool ramp = true); + void moveTurret(int16_t heading, float power = QB_HOME_PCT, bool relativeToRobot = true, bool ramp = true); + void moveTurretAndWait(int16_t heading, float power = QB_HOME_PCT, bool relativeToRobot = true, bool ramp = true); + void aimAssembly(AssemblyAngle angle); + void moveCradle(CradleState state, bool force = false); + void setFlywheelSpeed(float absoluteSpeed); + void setFlywheelSpeedStage(FlywheelSpeed stage); + void adjustFlywheelSpeedStage(SpeedStatus speed); + + /*QB AUTOMATIC TARGETING + -switchMode Used to switch between manual and automatic mode (overloaded function) + -switchTarget Switch between reciever1 or reciever2 + + */ + void switchMode(TurretMode mode); + void switchMode(); + void switchTarget(TargetReceiver target); + + /*QB MACROS + -loadFromCenter Prepared the QB to accept the ball from the center + -handoff Hands the ball to the runningback + -testRoutine used for testing different functions + */ + void loadFromCenter(); + void handoff(); + void testRoutine(); + + + + /*ENCODER VARIABLES + -currentTurretEncoderCount + -turretEncoderStateB The A channel will be 1 when the interrupt triggers + */ + static int32_t currentTurretEncoderCount; + static uint8_t turretEncoderStateB; + + /*ENCODER FUNCTIONS + -turretEncoderISR + -turretDirectionChanged IF the direction changes then some things need to happen differently because of the incredible amount of backlash + */ + static void turretEncoderISR(); + void turretDirectionChanged(); + + /* UART Communication Variables and Functions + */ + int motor1Value = 0; + int motor2Value = 0; + void updateReadMotorValues(); +}; + +#endif // QUARTERBACK_TURRET_H diff --git a/src/Utilities/BotTypes.cpp b/src/Utilities/BotTypes.cpp index e14815d..dc052bf 100644 --- a/src/Utilities/BotTypes.cpp +++ b/src/Utilities/BotTypes.cpp @@ -3,13 +3,15 @@ // constexpr to be evaluated at compile time constexpr Pair botTypeStrings[NUM_POSITIONS] = { - { lineman, "lineman" }, - { receiver, "receiver" }, - { runningback, "runningback" }, - { center, "center" }, - { mecanum_center, "mecanum_center" }, - { quarterback, "quarterback" }, - { kicker, "kicker" } + { lineman, "lineman" }, + { receiver, "receiver" }, + { runningback, "runningback" }, + { center, "center" }, + { mecanum_center, "mecanum_center" }, + { quarterback_old, "quarterback_old" }, + { quarterback_base, "quarterback_base" }, + { quarterback_turret, "quarterback_turret" }, + { kicker, "kicker" } }; // Function to map BotTypes to human-readable C-strings diff --git a/src/Utilities/BotTypes.h b/src/Utilities/BotTypes.h index 89dfeb8..8689722 100644 --- a/src/Utilities/BotTypes.h +++ b/src/Utilities/BotTypes.h @@ -8,7 +8,7 @@ #include #include -#define NUM_POSITIONS 7 // number of members of eBOT_TYPE +#define NUM_POSITIONS 9 // number of members of eBOT_TYPE /** BotType * enum for the possible positions a robot can have on the field @@ -29,7 +29,9 @@ typedef enum { runningback, center, mecanum_center, - quarterback, + quarterback_old, + quarterback_base, + quarterback_turret, kicker } BotType; @@ -58,47 +60,57 @@ typedef struct BotConfig { // BotType secondary_type; } bot_config_t; -#define NUM_BOTS 14 +#define NUM_BOTS 17 // Bot Aliases -#define BOT_IPP 0 -#define BOT_I_PLUS_PLUS 0 -#define BOT_SQRT_NEG_1 1 -#define BOT_PI 2 -#define BOT_RHO 3 -#define BOT_2_72 4 -#define BOT_SMILEY 5 -#define BOT_GEQ 6 -#define BOT_32_2 7 -#define BOT_9_8 8 -#define BOT_C 9 -#define BOT_RB 9 -#define BOT_PHI 10 -#define BOT_CENTER 10 -#define BOT_INF 11 -#define BOT_QB 11 -#define BOT_THETA 12 -#define BOT_KICKER 12 -#define BOT_LINEMAN_V1 13 +#define BOT_IPP 0 +#define BOT_I_PLUS_PLUS 0 +#define BOT_SQRT_NEG_1 1 +#define BOT_PI 2 +#define BOT_RHO 3 +#define BOT_2_72 4 +#define BOT_SMILEY 5 +#define BOT_GEQ 6 +#define BOT_32_2 7 +#define BOT_9_8 8 +#define BOT_C 9 +#define BOT_RB 9 +#define BOT_PHI 10 +#define BOT_CENTER 10 +#define BOT_INF 11 +#define BOT_QB 11 +#define BOT_QB_OLD 11 +#define BOT_THETA 12 +#define BOT_KICKER 12 +#define BOT_MC 13 +#define BOT_MECANUM_CENTER 13 +#define BOT_QB_BASE 14 +#define BOT_QB_BOTTOM 14 +#define BOT_QB_TURRET 15 +#define BOT_QB_TOP 15 +#define BOT_LINEMAN_V1 16 // 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, small_ampflow, { 1.0f, 9.75f, 9.00f, 36.00f }}, //* 5: :) - { 6, ">=", lineman, big_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.5f, 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, small_ampflow, { 0.5625f, 11.50f, 9.00f, 36.00f }}, //* 11: ∞ - { 12, "theta", kicker, small_ampflow, { 0.34375f, 10.00f, 9.00f, 36.00f }}, //* 12: Θ - { 13, "l-man-v1", lineman, small_12v, { 1.0f, 11.00f, 9.00f, 36.00f }} //* 13: generic lineman +// 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, small_ampflow, { 1.0f, 9.75f, 9.00f, 36.00f }}, //* 5: :) + { 6, ">=", lineman, big_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.5f, 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, 36.00f }}, //* 11: ∞ + { 12, "theta", kicker, small_ampflow, { 0.34375f, 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 }; //! Do not decrease r_min to less than half of the wheelbase, or the math might break diff --git a/src/Utilities/Debouncer.cpp b/src/Utilities/Debouncer.cpp new file mode 100644 index 0000000..6082f02 --- /dev/null +++ b/src/Utilities/Debouncer.cpp @@ -0,0 +1,98 @@ +#include "Debouncer.h" + +// based on: https://arduinogetstarted.com/tutorials/arduino-button-debounce + +// Input: debounce delay (milliseconds) +Debouncer::Debouncer(unsigned long delay, bool activeLow) { + if (activeLow) { + BASE_STATE = HIGH; + ACTIVE_STATE = LOW; + } else { + BASE_STATE = LOW; + ACTIVE_STATE = HIGH; + } + + // Initialize Variables + this->lastLastStableState = BASE_STATE; + this->lastStableState = BASE_STATE; + this->lastUnstableState = BASE_STATE; + this->lastToggleTime = 0; + this->debounceDelay = delay; + +} + +// 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); + // Serial.print(F(", l_unst: ")); + // Serial.print(lastUnstableState); + // Serial.print(F(", input: ")); + // Serial.print(inputState); + + // if the switch was toggled, update the last toggle time + if (inputState != lastUnstableState) { + lastToggleTime = millis(); + lastUnstableState = inputState; + } + + // Serial.print(F(" | over delay?: ")); + // Serial.print((millis() - lastToggleTime) > debounceDelay); + + lastLastStableState = lastStableState; + + // test if the delay has been exceeded + if ((millis() - lastToggleTime) > debounceDelay) { + + // Serial.print(F(" | stab_st changed?: ")); + // Serial.print(lastStableState != inputState); + + // if the state has changed, update it + if (lastStableState != inputState) { + lastStableState = inputState; + + // Serial.print(F(" | inputState == ACTIVE_STATE: ")); + // Serial.print(inputState == ACTIVE_STATE); + + } + } + + // Serial.print(F(" | end: l_stab:")); + // Serial.print(lastStableState); + // Serial.print(F(", l_unst: ")); + // Serial.print(lastUnstableState); + // Serial.print(F(", input: ")); + // Serial.println(inputState); + + return lastStableState; +} + +uint8_t Debouncer::wasToggled() { + return (lastLastStableState != lastStableState); +} + +uint8_t Debouncer::debounceAndToggled(uint8_t inputState) { + this->debounce(inputState); + return this->wasToggled(); +} + +uint8_t Debouncer::wasSwitchedToState(DebouncerState state) { + if (this->wasToggled()) { + if (state == active) { + return lastStableState == ACTIVE_STATE; + } else { // state == base + return lastStableState == BASE_STATE; + } + } + return false; +} + +uint8_t Debouncer::debounceAndSwitchedTo(uint8_t inputState, DebouncerState targetState) { + this->debounce(inputState); + return this->wasSwitchedToState(targetState); +} + +uint8_t Debouncer::debounceAndPressed(uint8_t inputState) { + return this->debounceAndSwitchedTo(inputState, active); +} \ No newline at end of file diff --git a/src/Utilities/Debouncer.h b/src/Utilities/Debouncer.h new file mode 100644 index 0000000..a269e5f --- /dev/null +++ b/src/Utilities/Debouncer.h @@ -0,0 +1,47 @@ +#ifndef DEBOUNCER_H +#define DEBOUNCER_H + +#include + +enum DebouncerState { + base, active +}; + +class Debouncer { + private: + uint8_t lastUnstableState; // inputState from last iteration (may change sporadically) + uint8_t lastStableState; // last debounced state (output) + uint8_t lastLastStableState; // value of lastStableState from the last call to debounce(), used for wasToggled() + unsigned long lastToggleTime; // the last time the input was toggled + unsigned long debounceDelay; // the time an output must be stable before it is considered valid and is output + + uint8_t BASE_STATE; + uint8_t ACTIVE_STATE; + public: + // construct debouncer with parameterized delay + // recommend using pointer to debouncer object + Debouncer(unsigned long delay, bool activeLow = false); + + // use to obtain "current" debounced state of button + uint8_t debounce(uint8_t newState); + + // use to execute an action *once* when the button is toggled *after debouncing* + // 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* + // 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* + uint8_t wasSwitchedToState(DebouncerState state); + + // use to execute an action when switched to a specific state *after debouncing* + // calls both debounce() and wasSwitchedToState() consecutively to avoid potential misses + uint8_t debounceAndSwitchedTo(uint8_t inputState, DebouncerState targetState); + + // shorthand for debounceAndSwitchesTo(inputState, active) + uint8_t debounceAndPressed(uint8_t inputState); +}; + +#endif // DEBOUNCER_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index f56dbf6..9f002ff 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -23,6 +23,8 @@ #include #include #include +#include +#include // Drive Includes #include @@ -88,14 +90,13 @@ void setup() { drive = new Drive(kicker, motorType, driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; - case quarterback: + case quarterback_old: robot = new Quarterback(SPECBOT_PIN1, SPECBOT_PIN2, SPECBOT_PIN3); - drive = new Drive(quarterback, motorType, driveParams); + drive = new Drive(quarterback_old, motorType, driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; case mecanum_center: robot = new MecanumCenter(SPECBOT_PIN1, SPECBOT_PIN2); - //! TODO: compensate for MecanumCenter.setup() drive = new DriveMecanum(); ((DriveMecanum*) drive)->setupMotors(M1_PIN, M2_PIN, M3_PIN, M4_PIN); break; @@ -109,6 +110,26 @@ void setup() { drive = new DriveQuick(driveParams); drive->setupMotors(M1_PIN, M2_PIN); break; + case quarterback_turret: + robot = new QuarterbackTurret( + M1_PIN, // left flywheel + M2_PIN, // right flywheel + M3_PIN, // cradle + M4_PIN, // turret + SPECBOT_PIN1, // assembly stepper step + SPECBOT_PIN2, // assembly stepper dir + SPECBOT_PIN3, // magnetometer sda + SPECBOT_PIN4, // magnetometer scl + ENC1_CHA, // turret encoder + ENC1_CHB, // turret encoder + ENC2_CHA // zeroing laser + ); + break; + case quarterback_base: + drive = new Drive(quarterback_base, motorType, driveParams); + drive->setupMotors(M1_PIN, M2_PIN); + robot = new QuarterbackBase(drive); + break; case receiver: case lineman: default: // Assume lineman @@ -152,65 +173,78 @@ void loop() { if (ps5.isConnected()) { // Serial.print(F("\r\nConnected")); // ps5.setLed(255, 0, 0); // set LED red + + //* QBv3 Turret doesn't have drive, so this is a temporary measure to avoid NPEs and chaos + // TODO: find better solution + if (robotType != quarterback_turret) { + + if (robotType == mecanum_center) { + ((DriveMecanum*) drive)->setStickPwr(ps5.LStickX(), ps5.LStickY(), ps5.RStickX()); + } else { + drive->setStickPwr(ps5.LStickY(), ps5.RStickX()); + } - if (robotType == mecanum_center) { - ((DriveMecanum*) drive)->setStickPwr(ps5.LStickX(), ps5.LStickY(), ps5.RStickX()); - } else { - drive->setStickPwr(ps5.LStickY(), ps5.RStickX()); - } - - // determine BSN percentage (boost, slow, or normal) - if (ps5.Touchpad()){ - drive->emergencyStop(); - drive->setBSN(Drive::BRAKE); - } else if (ps5.R1()) { - drive->setBSN(Drive::BOOST); - // ps5.setLed(0, 255, 0); // set LED red - } else if (ps5.L1()) { - drive->setBSN(Drive::SLOW); - } else if (ps5.R2() && motorType == falcon) { - // used to calibrate the max pwm signal for the falcon 500 motors - drive->setBSNValue(FALCON_CALIBRATION_FACTOR); - } else { - drive->setBSN(Drive::NORMAL); - } + // determine BSN percentage (boost, slow, or normal) + if (ps5.Touchpad()){ + drive->emergencyStop(); + drive->setBSN(Drive::BRAKE); + } else if (ps5.R1()) { + drive->setBSN(Drive::BOOST); + // ps5.setLed(0, 255, 0); // set LED red + } else if (ps5.L1()) { + drive->setBSN(Drive::SLOW); + } else if (ps5.R2() && motorType == falcon) { + // used to calibrate the max pwm signal for the falcon 500 motors + drive->setBSNValue(FALCON_CALIBRATION_FACTOR); + } else { + drive->setBSN(Drive::NORMAL); + } - if (ps5.Share()) { - lights.setLEDStatus(Lights::DISCO); - } + if (ps5.Share()) { + lights.setLEDStatus(Lights::DISCO); + } - // Manual LED State Toggle (Defense/Offense) - if (ps5.Options()) { - lights.togglePosition(); - } + // Manual LED State Toggle (Defense/Offense) + if (ps5.Options()) { + lights.togglePosition(); + } - if (robotType != lineman) { // && lights.returnStatus() == lights.OFFENSE || lights.returnStatus() == lights.TACKLED - if (lights.returnStatus() == lights.OFFENSE && digitalRead(TACKLE_PIN) == LOW) { - lights.setLEDStatus(Lights::TACKLED); - lights.tackleTime = millis(); - } - // debounce the tackle sensor input - else if ((millis() - lights.tackleTime) >= lights.switchTime && - lights.returnStatus() == lights.TACKLED && digitalRead(TACKLE_PIN) == HIGH) { - lights.setLEDStatus(Lights::OFFENSE); + if (robotType != lineman) { // && lights.returnStatus() == lights.OFFENSE || lights.returnStatus() == lights.TACKLED + if (lights.returnStatus() == lights.OFFENSE && digitalRead(TACKLE_PIN) == LOW) { + lights.setLEDStatus(Lights::TACKLED); + lights.tackleTime = millis(); + } + // debounce the tackle sensor input + else if ((millis() - lights.tackleTime) >= lights.switchTime && + lights.returnStatus() == lights.TACKLED && digitalRead(TACKLE_PIN) == HIGH) { + lights.setLEDStatus(Lights::OFFENSE); + } } - } - //* Update the motors based on the inputs from the controller - //* Can change functionality depending on subclass, like robot.action() - drive->update(); - // drive->printDebugInfo(); // comment this line out to reduce compile time and memory usage - // drive->printCsvInfo(); // prints info to serial monitor in a csv (comma separated value) format + if (lights.returnStatus() == lights.DISCO) + lights.updateLEDS(); + + //* Update the motors based on the inputs from the controller + //* Can change functionality depending on subclass, like robot.action() + drive->update(); + // drive->printDebugInfo(); // comment this line out to reduce compile time and memory usage + // drive->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(); + delay(5); } else { // no response from PS5 controller within last 300 ms, so stop + if (robotType != quarterback_turret) { // Emergency stop if the controller disconnects drive->emergencyStop(); lights.setLEDStatus(Lights::UNPAIRED); + } else { + ((QuarterbackTurret*) robot)->emergencyStop(); + } } } @@ -218,12 +252,18 @@ void loop() { * @brief onConnection: Function to be called on controller connect */ void onConnection() { - if(ps5.isConnected()) { + if (ps5.isConnected()) { Serial.println(F("Controller Connected.")); // ps5.setLed(0, 255, 0); // set LED green lights.setLEDStatus(Lights::PAIRED); } - drive->emergencyStop(); + + // TODO: perm sln + if (robotType != quarterback_turret) { + drive->emergencyStop(); + } else { + ((QuarterbackTurret*) robot)->emergencyStop(); + } } /** @@ -232,5 +272,11 @@ void onConnection() { */ void onDisconnect() { Serial.println(F("Controller Disconnected.")); - drive->emergencyStop(); + + // TODO: perm sln + if (robotType != quarterback_turret) { + drive->emergencyStop(); + } else { + ((QuarterbackTurret*) robot)->emergencyStop(); + } }