diff --git a/components/Pairing/pairing.cpp b/components/Pairing/pairing.cpp index 6165e97..1e6d346 100644 --- a/components/Pairing/pairing.cpp +++ b/components/Pairing/pairing.cpp @@ -166,7 +166,11 @@ void getAddress(const char *&addr) /// @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); + // Ensure the built-in LED pin is configured before any digitalWrite() calls. + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); + + // Serial.begin(115200); // pinMode(LED_BUILTIN, OUTPUT); // if we just returned a char*, it would be deleted and point to nowhere useful diff --git a/components/Robot/Quarterback.cpp b/components/Robot/Quarterback.cpp index 5cdd10a..28e21c5 100644 --- a/components/Robot/Quarterback.cpp +++ b/components/Robot/Quarterback.cpp @@ -1,235 +1,1715 @@ #include +// 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 Quarterback::turretEncoderPinA; +uint8_t Quarterback::turretEncoderPinB; +uint8_t Quarterback::turretEncoderStateB; +int32_t Quarterback::currentTurretEncoderCount; + +void Quarterback::turretEncoderISR() +{ + turretEncoderStateB = digitalRead(turretEncoderPinB); + + if (turretEncoderStateB == 1) + { + currentTurretEncoderCount--; + } + else if (turretEncoderStateB == 0) + { + currentTurretEncoderCount++; + } +} + +#pragma region Constructor Quarterback::Quarterback( - uint8_t flywheelPin, - uint8_t conveyorPin, - uint8_t elevationPin) + uint8_t flywheelLeftPin, // M1 + uint8_t flywheelRightPin, // M2 + uint8_t cradlePin, // M3 + uint8_t turretPin, // M4 + uint8_t assemblyPin, // S1 + uint8_t magnetometerSdaPin, // S3 + uint8_t magnetometerSclPin, // S4 + uint8_t turretEncoderPinA, // E1A + uint8_t turretEncoderPinB, // E1B + uint8_t turretLaserPin // E2A +) { - // The conveyor and flywheels are assumed to be off initially - flywheelsOn = false; - conveyorOn = false; - // Initially we are not aiming up or down, and we assume the initial elevation is zero - aimingUp = false; - aimingDown = false; - currentElevation = 0; + // 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->currentAssemblyAngle = unknownAngle; + 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->assemblyTriggerToggled = false; + + 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; - // Set internal class fields from arguments - this->flywheelPin = flywheelPin; - this->conveyorPin = conveyorPin; - this->elevationPin = elevationPin; + this->flywheelManualOverride = false; // until/unless the left stick is active, this is false - // Attach the motors to their respective pins - flywheelMotor.setup(flywheelPin); - conveyorMotor.setup(conveyorPin); - elevationMotors.setup(elevationPin); + this->currentTurretSpeed = 0; // it is safe to assume the turret is stopped + this->targetTurretSpeed = 0; - // Set the conveyor motor to zero so it doesnt spin on startup - conveyorMotor.write(CONVEYOR_OFF); + this->targetRelativeHeading = 0; // while the initial heading is unknown, we want the heading to be zero + this->targetTurretEncoderCount = 0; - // Lower the Linear Actuators at start up so they are in the bottom position - // through the updateAim() function - setupMotors = true; - lastElevationTime = millis(); + 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); //! will be 1 when at home position or main power is off (the latter is electrically unavoidable) + + // encoder setup + Quarterback::turretEncoderPinA = turretEncoderPinA; + Quarterback::turretEncoderPinB = turretEncoderPinB; + Quarterback::currentTurretEncoderCount = 0; + pinMode(turretEncoderPinA, INPUT); + 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 + assemblyMotor.setup(assemblyPin, small_12v); + Serial.println("Quarterback Constructor"); + flywheelLeftMotor.setup(flywheelLeftPin, falcon); + flywheelRightMotor.setup(flywheelRightPin, falcon); + + // initialize debouncers + this->dbShare = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + 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->dbDpadLeft = new Debouncer(QB_BASE_DEBOUNCE_DELAY); + this->dbDpadRight = 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); } +#pragma endregion +#pragma region action() void Quarterback::action() { - // Update the bools within the class to see if the user wants to go up or down - if (ps5.Up()) - aim(QBAim::AIM_UP); - else if (ps5.Down()) - aim(QBAim::AIM_DOWN); + //! 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 - // Toogle the Conveyor and Flywheels - if (ps5.Square()) - toggleConveyor(); - else if (ps5.Circle()) - toggleFlywheels(); + if (!testForDisableOrStop() && !runningMacro) + { + //* Square: Toggle Driving/Turret Control + if (dbSquare->debounceAndPressed(ps5.Square())) + { + if (!enabled) + { + setEnabled(true); + Serial.println(F("Turret Control Enabled")); + } + else + { + setEnabled(false); + Serial.println(F("Turret Control Disabled")); + } + } + //* Circle: Startup and Home (Reset or Zero Turret) + else 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(); + } - // Change the flywheel speed - if (ps5.Triangle()) - changeFWSpeed(SpeedStatus::INCREASE); - else if (ps5.Cross()) - changeFWSpeed(SpeedStatus::DECREASE); + //* Right Trigger (R2): Fire (cradle/grabber forward) + // Do not fire unless moving forward (do not fire when intaking or stopped) + if (ps5.R2()) + { + if (currentFlywheelSpeed > STICK_DEADZONE) + { + moveCradle(forward); + } + else + { + moveCradle(back); + } + } + //* Left Trigger (L2): Toggle Assembly Angle + if (ps5.L2() && !assemblyTriggerToggled) + { + assemblyTriggerToggled = true; - // Update the aim and flywheels on quarterback to see if we need to stop or not - update(); + // if angled or unknown, move to straight angle. else, move to firing angle. + if (currentAssemblyAngle == unknownAngle || currentAssemblyAngle == angled) + { + aimAssembly(straight); + } + else if (currentAssemblyAngle == straight) + { + aimAssembly(angled); + } + } + else if (assemblyTriggerToggled && !ps5.L2()) + { + assemblyTriggerToggled = false; + } + else + { + aimAssembly(targetAssemblyAngle); + } + + if (dbShare->debounceAndPressed(ps5.Share())) + { + // TODO: Implement Automatic Targeting System when it is finished (capstone from build team) + // until then, this is here to ensure the automatic targeting system toggle works. + } + + //* Options (Button): Switch LED Color between Offense and Defense + if (dbOptions->debounceAndPressed(ps5.Options())) + { + // TODO: Implement LED color switching for offense and defense modes + } + + // else if (ps5.Left()) { + // testRoutine(); + // } + + //* Manual and Automatic Controls. + // ONLY FOR TURRET MODE + if (enabled) + { + + //* Manual Controls + + stickFlywheel = (ps5.LStickY() / 127.5f); + stickTurret = (ps5.RStickX() / 127.5f); + + //* NO option to set combine mode + // Could remove/implement in the future + if (mode == combine) + { + //* Combine "Macro" Mode + // Overrides turret control + // Allows switching between 3 different angles (left, straight, right) + // Flywheel control is available as normal (set powers with override via stick) + + //* D-Pad Left: Move left one position + if (dbDpadLeft->debounceAndPressed(ps5.Left())) + { + if (this->combinePosition == combineStraight) + { + this->combinePosition = combineLeft; + // moveTurretAndWait(-45); + targetRelativeHeading = -45; + } + else if (this->combinePosition == combineRight) + { + this->combinePosition = combineStraight; + // moveTurretAndWait(0); + targetRelativeHeading = 0; + } + } + //* D-Pad Right: Move right one position + else if (dbDpadRight->debounceAndPressed(ps5.Right())) + { + if (this->combinePosition == combineStraight) + { + this->combinePosition = combineRight; + // moveTurretAndWait(45); + targetRelativeHeading = 45; + } + else if (this->combinePosition == combineLeft) + { + this->combinePosition = combineStraight; + // moveTurretAndWait(0); + targetRelativeHeading = 0; + } + } + + // Run the PID loop + turretPIDSpeed = turretPIDController((float)getCurrentHeading(), (float)targetRelativeHeading, kp, kd, ki, .3); + setTurretSpeed(turretPIDSpeed); + + // if (utmsCtr <= UTMS_CTR_MAX) { + // utmsCtr = 0; + Serial.print(F("combine mode -- ctec = ")); + Serial.print(currentTurretEncoderCount); + Serial.print(F("; ttec = ")); + Serial.println(targetTurretEncoderCount); + // } else { + // utmsCtr++; + // } + } + else + { + //* Right Stick X: Turret Control + // Left = CCW, Right = CW + if (fabs(stickTurret) > STICK_DEADZONE) + { + //* Use absolute positioning and position-based control iff. 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(); + } + //* Use relative positioning and speed-based control + 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(); } +#pragma endregion -void Quarterback::toggleFlywheels() +// 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 Quarterback::setTurretSpeed(float absoluteSpeed, bool overrideEncoderTare) { - if (millis() - lastDBFW >= DEBOUNCE_WAIT) + // Serial.print(F("setTurretSpeed called with speed = ")); + // Serial.println(absoluteSpeed); + if (enabled) { - // Toggle the bool so we know if its on or not - flywheelsOn = !flywheelsOn; + 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(); + // } - lastDBFW = millis(); + currentTurretSpeed = targetTurretSpeed; //! for now, will probably need to change later, like an interrupt + } + else + { + turretMotor.write(0); } } -float Quarterback::rampFW(float requestedPower) +#pragma region Old Rel Turret +void Quarterback::moveTurret(int16_t heading, bool relativeToRobot, bool ramp) { + moveTurret(heading, degrees, QB_HOME_PCT, relativeToRobot, ramp); +} - if (millis() - lastFlywheelRampTime >= FW_TIME_INCREMENT) +void Quarterback::moveTurret(int16_t heading, float power, bool relativeToRobot, bool ramp) +{ + moveTurret(heading, degrees, power, relativeToRobot, ramp); +} + +void Quarterback::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) { - if (abs(requestedPower) < THRESHOLD) - { // if the input is effectively zero - // Experimental Braking Code - if (abs(currentFlywheelPower) < 0.1) - { // if the current power is very small just set it to zero - currentFlywheelPower = 0; + 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) + { + // old code that does not work well + // moveTurret(heading, counts, power, relativeToRobot, ramp); + // } else if (units == counts) { + int8_t sign = copysign(1, currentTurretEncoderCount); + // currentTurretEncoderCount = abs(currentTurretEncoderCount); + // currentTurretEncoderCount %= 360; + // currentTurretEncoderCount *= sign; + + targetTurretEncoderCount = (int)round((double)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 } - else + // currentRelativeHeading = targetRelativeHeading; + } + else + { // relative to field + // todo: use magnetometer + } + } + else + { + setTurretSpeed(0); + } +} + +void Quarterback::moveTurretAndWait(int16_t heading, float power, bool relativeToRobot, bool ramp) +{ + moveTurret(heading, power, relativeToRobot, ramp); + while (turretMoving && !testForDisableOrStop()) + { + updateTurretMotionStatus(); + delay(10); + } +} +#pragma endregion + +void Quarterback::updateTurretMotionStatus() +{ + // if (utmsCtr >= UTMS_CTR_MAX) { + // utmsCtr = 0; + Serial.print(F("update called with ctec = ")); + Serial.print(currentTurretEncoderCount); + Serial.print(F("; ttec = ")); + Serial.print(targetTurretEncoderCount); + Serial.print(F("; error (ct) = ")); + Serial.println(fabs((currentTurretEncoderCount % QB_COUNTS_PER_TURRET_REV) - targetTurretEncoderCount)); + // } else { + // utmsCtr++; + // } + // determines if encoder is within "spec" + if (turretMoving && fabs((currentTurretEncoderCount % QB_COUNTS_PER_TURRET_REV) - targetTurretEncoderCount) < QB_TURRET_THRESHOLD) + { + turretMoving = false; + setTurretSpeed(0); + } +} + +// deprecated +void Quarterback::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; + } +} + +//* get current heading in degrees +int16_t Quarterback::getCurrentHeading() +{ + return (int)((double)currentTurretEncoderCount / QB_COUNTS_PER_TURRET_DEGREE) % 360; +} + +// Function to normalize an angle to the range [0, 360) +int Quarterback::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 Quarterback::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; + } +} + +// not currently used +int16_t Quarterback::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); + } +} + +// not currently used +int16_t Quarterback::findNearestHeading(int16_t targetHeading) +{ + return findNearestHeading(targetHeading, currentRelativeHeading); +} + +#pragma region Assembly +void Quarterback::aimAssembly(AssemblyAngle angle, bool force) +{ + if (enabled) + { + if (!assemblyMoving) + { + targetAssemblyAngle = angle; + + if (targetAssemblyAngle != currentAssemblyAngle || force) { - currentFlywheelPower *= FW_BRAKE_PERCENTAGE; + moveAssemblySubroutine(); + } + + //* force is a blocking routine to ensure it works without interruption + //* do not use force frequently as it can strain the motor + //* this should only be used on startup + if (force) + { + // also allow emergency stop + while ((millis() - assemblyStartTime) <= QB_ASSEMBLY_TILT_DELAY && !testForDisableOrStop()) + { + NOP(); + } + currentAssemblyAngle = targetAssemblyAngle; + assemblyMoving = false; + assemblyMotor.write(0); } - lastFlywheelRampTime = millis(); } - else if (abs(requestedPower - currentFlywheelPower) < FW_ACCEL_RATE) - { // if the input is effectively at the current power - return requestedPower; + else if ((millis() - assemblyStartTime) > QB_ASSEMBLY_TILT_DELAY) + { + currentAssemblyAngle = targetAssemblyAngle; + assemblyMoving = false; + assemblyMotor.write(0); } - // if we need to increase speed and we are going forward - else if (requestedPower > currentFlywheelPower) + } + else + { + assemblyMotor.write(0); + } +} + +//! this is a dangerous function to call +// should only be called with known good state +void Quarterback::moveAssemblySubroutine() +{ + if (targetAssemblyAngle == straight) + { + assemblyMotor.write(QB_ASM_SPEED); + } + else if (targetAssemblyAngle == angled) + { + assemblyMotor.write(-1.25 * QB_ASM_SPEED); + } + assemblyStartTime = millis(); + assemblyMoving = true; +} +#pragma endregion + +#pragma region Cradle +//! this is a dangerous function to call +// should only be called with known good state +void Quarterback::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 Quarterback::moveCradle(CradleState state, bool force) +{ + if (enabled) + { + if (!cradleMoving) { - currentFlywheelPower = currentFlywheelPower + FW_ACCEL_RATE; - lastFlywheelRampTime = millis(); + 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(" | ")); } - // if we need to decrease speed and we are going forward - else if (requestedPower < currentFlywheelPower) + else if ((millis() - cradleStartTime) > QB_CRADLE_TRAVEL_DELAY) { - currentFlywheelPower = currentFlywheelPower - FW_ACCEL_RATE; - lastFlywheelRampTime = millis(); + currentCradleState = targetCradleState; + cradleMoving = false; + cradleActuator.write(0); + // Serial.print(F("cradle stopped | ")); } } + else + { + cradleActuator.write(0); + } - return currentFlywheelPower; + // Serial.println(); } +#pragma endregion -// Aiming related functions -void Quarterback::aim(QBAim dir) +#pragma region Flywheels +void Quarterback::setFlywheelSpeed(float absoluteSpeed) { - // Debounce for button press - if (millis() - lastDBElev >= DEBOUNCE_WAIT) + // update the motors so they are spinning at the new speed + if (enabled) { - // Check which direction the user wants and turn the other direction off - switch (dir) + // 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) { - case AIM_UP: - aimingUp = true; - aimingDown = false; - break; - case AIM_DOWN: - aimingDown = true; - aimingUp = false; - break; + // 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); + } +} - lastDBElev = millis(); +void Quarterback::setFlywheelSpeedStage(FlywheelSpeed stage) +{ + targetFlywheelStage = stage; + setFlywheelSpeed(flywheelSpeeds[static_cast(targetFlywheelStage)]); + currentFlywheelStage = targetFlywheelStage; +} + +void Quarterback::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)); +} +#pragma endregion + +#pragma region Auto Mode +void Quarterback::switchMode() +{ + if (mode == manual) + { + switchMode(automatic); + } + else if (mode == automatic) + { + switchMode(manual); + } +} + +void Quarterback::switchMode(TurretMode mode) +{ + this->mode = mode; +} + +void Quarterback::switchTarget(TargetReceiver target) +{ + switchMode(automatic); + this->target = target; + // todo: not sure if this needs more functionality? } +#pragma endregion -void Quarterback::update() +#pragma region Macros +void Quarterback::loadFromCenter() { - // Setup the motors on startup so they go down to absolute zero - if (setupMotors) + this->runningMacro = true; + aimAssembly(straight); + setFlywheelSpeedStage(slow_inwards); + moveCradle(back); + // zeroTurret(); + this->runningMacro = false; +} + +void Quarterback::handoff() +{ + this->runningMacro = true; + aimAssembly(straight); + int16_t targetHeading = (getCurrentHeading() + 130) % 360; + calculateHeadingMag(); + targetAbsoluteHeading = headingDeg + 180; + targetAbsoluteHeading %= 360; + + if (useMagnetometer) { - if (millis() - lastElevationTime >= 8000) + // 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()) { - setupMotors = false; + calculateHeadingMag(); + turretPIDSpeed = turretPIDController(headingDeg, (float)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 Quarterback::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 Quarterback::zeroTurret() +{ + this->runningMacro = true; + + // Printouts for starting zeroing + 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 { - elevationMotors.write(SERVO_SPEED_DOWN); + stopCounter = 0; } - // Control the motors based on if they want to aim up or down + + 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 } - else if (millis() - lastElevationTime >= ELEVATION_PERIOD) + + 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 Quarterback::reset() +{ + this->enabled = true; + this->runningMacro = true; + moveCradle(back, true); // force + aimAssembly(straight); + // loadFromCenter(); + zeroTurret(); // temp: just zero + this->initialized = true; + this->runningMacro = false; +} +#pragma endregion + +#pragma region Safety +bool Quarterback::testForDisableOrStop() +{ + //* Touchpad: Emergency Stop + if (ps5.Touchpad()) + { + emergencyStop(); + Serial.println(F("emergency stopping")); + return true; + } + // Remove the Square emergency stop to use for switching modes + else { - if (aimingUp && currentElevation < MAX_ELEVATION) + // Serial.println(F("not disabling or stopping")); + return false; + } +} + +void Quarterback::setEnabled(bool enabled) +{ + this->enabled = enabled; +} + +bool Quarterback::isEnabled() +{ + return this->enabled; +} + +void Quarterback::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 +} +#pragma endregion + +void Quarterback::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); + */ + } +} + +#pragma region Magnetometer +/** + * @brief Sets up magnetometer + * @authors Rhys Davies, Corbin Hibler + * @date 2024-01-03 + */ +void Quarterback::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 Quarterback::calibMagnetometer() +{ + + mag_yVal = 0; + mag_xVal = 0; + mag_xMax = -1000000; + mag_xMin = 1000000; + mag_xHalf = 0; + mag_yMax = -1000000; + mag_yMin = 1000000; + mag_yHalf = 0; + mag_xSign = false; + mag_ySign = false; + + northHeadingDegrees = 0; + + int degreesMove = 360; + targetTurretEncoderCount = (int)round((double)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 < mag_xMin && lis3mdl.x != -1 && lis3mdl.x != 0) { - // Set the elevation of the top to 50% higher - elevationMotors.write(SERVO_SPEED_UP); - currentElevation += 50; - aimingUp = false; - lastElevationTime = millis(); + mag_xMin = lis3mdl.x; } - else if (aimingDown && currentElevation > 0) + else if (lis3mdl.x > mag_xMax && lis3mdl.x != -1 && lis3mdl.x != 0) { - // Set the elevation of the bottom to 50% lower - elevationMotors.write(SERVO_SPEED_DOWN); - currentElevation -= 50; - aimingDown = false; - lastElevationTime = millis(); + mag_xMax = lis3mdl.x; } - else + + // Adjusting X values to range from + or - values rather than all positive + mag_xHalf = abs(mag_xMax) - abs(mag_xMin); + mag_xHalf /= 2; + mag_xHalf += abs(mag_xMin); + + // Constantly looking for min and max values of Y + if (lis3mdl.y < mag_yMin && lis3mdl.y != -1 && lis3mdl.y != 0 && lis3mdl.y != 10) { - // Stop the linear actuators if it is not supposed to move up or down anymore - elevationMotors.write(SERVO_SPEED_STOP); + mag_yMin = lis3mdl.y; } + else if (lis3mdl.y > mag_yMax && lis3mdl.y != -1 && lis3mdl.y != 0 && lis3mdl.y != 10) + { + mag_yMax = lis3mdl.y; + } + + // Adjusting Y values to range from + or - values rather than all positive + mag_yHalf = abs(mag_yMax) - abs(mag_yMin); + mag_yHalf /= 2; + mag_yHalf += abs(mag_yMin); + + /*DEBUGGING PRINTOUTS*/ + // Serial.print("X: "); Serial.print(lis3mdl.x); + // Serial.print("\tY: "); Serial.print(lis3mdl.y); + // Serial.print("\tMinX: "); Serial.print(mag_xMin); + // Serial.print("\tMaxX: "); Serial.print(mag_xMax); + // Serial.print("\tMinY: "); Serial.print(mag_yMin); + // Serial.print("\tMaxY: "); Serial.print(mag_yMax); + // Serial.println(); } - // update flywheels if needed - if (flywheelsOn) + setTurretSpeed(0, true); + + // Updating variables that will be used to handle other two possible sign cases for each value + if ((mag_xMax + mag_xMin) < 0) { - flywheelMotor.write(rampFW(FLYWHEEL_SPEED_FULL + flywheelSpeedFactor)); + mag_xSign = true; } - else + if ((mag_yMax + mag_yMin) < 0) { - flywheelMotor.write(rampFW(FLYWHEEL_STOP_SPEED)); + mag_ySign = true; } + // + + calculateHeadingMag(); + + Serial.print(F("Magnetometer reading after calib: ")); + Serial.print(headingDeg); + Serial.print(F("\tEncoder after calib:")); + Serial.println(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(); + + setTurretSpeed(0); + + // delay(5000); } -void Quarterback::toggleConveyor() +/** + * @brief Uses the data collected at calibration to calculate the current heading relative to magnetic north + * @author George Rak + * @date 4-9-2024 + */ +void Quarterback::calculateHeadingMag() { - // Debounce for button press - if (millis() - lastDBConv >= DEBOUNCE_WAIT) + // Only run the code in here if the calibration has been done to the magnetometer + if (magnetometerCalibrated) { - // Toggle the conveyor between on or off - if (!conveyorOn) + lis3mdl.read(); + // Calculate the current angle of the turret based on the calibration data + if (mag_xSign) { - conveyorMotor.write(CONVEYOR_ON); + mag_xVal = lis3mdl.x + mag_xHalf; } else { - conveyorMotor.write(CONVEYOR_OFF); + mag_xVal = lis3mdl.x - mag_xHalf; + } + + if (mag_ySign) + { + mag_yVal = lis3mdl.y + mag_yHalf; + } + else + { + mag_yVal = lis3mdl.y - mag_yHalf; + } + + // Evaluate both ranges of X and Y then scale the smaller value to be within the same range as the larger + if (mag_yHalf > mag_xHalf) + { + mag_xVal = (double)((double)mag_xVal / ((double)mag_xHalf)) * (double)mag_yHalf; + } + else if (mag_xHalf > mag_yHalf) + { + mag_yVal = (double)((double)mag_yVal / ((double)mag_yHalf)) * (double)mag_xHalf; + } + + // Calculate angle in radians + if (mag_xVal != -1 && mag_xVal != 0 && mag_yVal != 0 && mag_yVal != -1) + { + headingRad = atan2(mag_yVal, mag_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 + // + 180 - QB_NORTH_OFFSET + headingDeg = ((int)headingDeg) + 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(mag_xMin); + // Serial.print("\tMaxX: "); Serial.print(mag_xMax); + // Serial.print("\tMinY: "); Serial.print(mag_yMin); + // Serial.print("\tMaxY: "); Serial.print(mag_yMax); + // Serial.print("\txAdapt: "); Serial.print(mag_xVal); + // Serial.print("\tyAdapt: "); Serial.print(mag_yVal); + // Serial.print("\tHeading [deg]: "); Serial.print(headingDeg); + // Serial.println(); + } +} +#pragma endregion + +#pragma region PID +/** + * @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 Quarterback::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; } - // Toggle the bool so we know which mode it is in - conveyorOn = !conveyorOn; - lastDBConv = millis(); + + // Run the PID loop + turretPIDSpeed = turretPIDController(headingDeg, (float)targetAbsoluteHeading, kp, kd, ki, .2); + setTurretSpeed(turretPIDSpeed, true); } } -void Quarterback::changeFWSpeed(SpeedStatus speed) +/** + * @brief PID controller to hold the turret still (gains tuned, not calculated) + * @author George Rak + * @date 4-9-2024 + */ +float Quarterback::turretPIDController(float current, float target, float kp, float kd, float ki, float maxSpeed) { - // Debounce for button press - if (millis() - lastDBFWChange >= DEBOUNCE_WAIT) + if (maxSpeed > .5) { - // Change the speed factor based on whether the user wants to increase or decrease (HARD CODED) - switch (speed) + 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(current, target); + + // Taking the average of the error + prevErrorVals[prevErrorIndex] = e; + prevErrorIndex++; + prevErrorIndex %= PID_ERROR_AVG_ARRAY_LENGTH; + + // For the first one populate the average so it does not freak out + if (firstAverage) { - case INCREASE: - arrayPos++; - break; - case DECREASE: - arrayPos--; - break; + for (int i = 0; i < PID_ERROR_AVG_ARRAY_LENGTH; i++) + { + prevErrorVals[i] = e; + } + firstAverage = false; } - // Cap the arrayPos so it doesn't go out of bounds - arrayPos = constrain(arrayPos, 0, 2); + // Taking the avergage for error + int avgError = 0; + for (int i = 0; i < PID_ERROR_AVG_ARRAY_LENGTH; i++) + { + avgError += prevErrorVals[i]; + } + avgError /= PID_ERROR_AVG_ARRAY_LENGTH; + e = avgError; - // Set the flywheelSpeedFactor - flywheelSpeedFactor = speedFac[arrayPos]; + // Calculate the derivative and integral values + float eDerivative = (e - ePrevious); + eIntegral = eIntegral + e * .01; - // Update the motors if they are spinning for the new speed - // if (flywheelsOn){ - // flywheelMotor.write(FLYWHEEL_SPEED_FULL + flywheelSpeedFactor); - // } else { - // flywheelMotor.write(FLYWHEEL_STOP_SPEED); - // } + // Compute the PID control signal + float u = (kp * e) + (ki * eIntegral) + (kd * eDerivative); + + // Constrain output PWM values to -.2 to .2 + if (u > 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; + } - lastDBFWChange = millis(); + 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(current, 0); + Serial.print("\tTarget [deg]: "); + Serial.print(target); + 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; + } +} +#pragma endregion + +#pragma region Stabilization +/** + * @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 Quarterback::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(); } +#pragma endregion \ No newline at end of file diff --git a/components/Robot/QuarterbackOld.cpp b/components/Robot/QuarterbackOld.cpp new file mode 100644 index 0000000..8bed738 --- /dev/null +++ b/components/Robot/QuarterbackOld.cpp @@ -0,0 +1,235 @@ +#include + +QuarterbackOld::QuarterbackOld( + uint8_t flywheelPin, + uint8_t conveyorPin, + uint8_t elevationPin) +{ + // The conveyor and flywheels are assumed to be off initially + flywheelsOn = false; + conveyorOn = false; + + // Initially we are not aiming up or down, and we assume the initial elevation is zero + aimingUp = false; + aimingDown = false; + currentElevation = 0; + + // Set internal class fields from arguments + this->flywheelPin = flywheelPin; + this->conveyorPin = conveyorPin; + this->elevationPin = elevationPin; + + // Attach the motors to their respective pins + flywheelMotor.setup(flywheelPin); + conveyorMotor.setup(conveyorPin); + elevationMotors.setup(elevationPin); + + // Set the conveyor motor to zero so it doesnt spin on startup + conveyorMotor.write(CONVEYOR_OFF); + + // Lower the Linear Actuators at start up so they are in the bottom position + // through the updateAim() function + setupMotors = true; + lastElevationTime = millis(); +} + +void QuarterbackOld::action() +{ + // Update the bools within the class to see if the user wants to go up or down + if (ps5.Up()) + aim(QBAim::AIM_UP); + else if (ps5.Down()) + aim(QBAim::AIM_DOWN); + + // Toogle the Conveyor and Flywheels + if (ps5.Square()) + toggleConveyor(); + else if (ps5.Circle()) + toggleFlywheels(); + + // Change the flywheel speed + if (ps5.Triangle()) + changeFWSpeed(SpeedStatus::INCREASE); + else if (ps5.Cross()) + changeFWSpeed(SpeedStatus::DECREASE); + + // Update the aim and flywheels on quarterback to see if we need to stop or not + update(); +} + +void QuarterbackOld::toggleFlywheels() +{ + if (millis() - lastDBFW >= DEBOUNCE_WAIT) + { + // Toggle the bool so we know if its on or not + flywheelsOn = !flywheelsOn; + + lastDBFW = millis(); + } +} + +float QuarterbackOld::rampFW(float requestedPower) +{ + + if (millis() - lastFlywheelRampTime >= FW_TIME_INCREMENT) + { + if (abs(requestedPower) < THRESHOLD) + { // if the input is effectively zero + // Experimental Braking Code + if (abs(currentFlywheelPower) < 0.1) + { // if the current power is very small just set it to zero + currentFlywheelPower = 0; + } + else + { + currentFlywheelPower *= FW_BRAKE_PERCENTAGE; + } + lastFlywheelRampTime = millis(); + } + else if (abs(requestedPower - currentFlywheelPower) < FW_ACCEL_RATE) + { // if the input is effectively at the current power + return requestedPower; + } + // if we need to increase speed and we are going forward + else if (requestedPower > currentFlywheelPower) + { + currentFlywheelPower = currentFlywheelPower + FW_ACCEL_RATE; + lastFlywheelRampTime = millis(); + } + // if we need to decrease speed and we are going forward + else if (requestedPower < currentFlywheelPower) + { + currentFlywheelPower = currentFlywheelPower - FW_ACCEL_RATE; + lastFlywheelRampTime = millis(); + } + } + + return currentFlywheelPower; +} + +// Aiming related functions +void QuarterbackOld::aim(QBAim dir) +{ + // Debounce for button press + if (millis() - lastDBElev >= DEBOUNCE_WAIT) + { + // Check which direction the user wants and turn the other direction off + switch (dir) + { + case AIM_UP: + aimingUp = true; + aimingDown = false; + break; + case AIM_DOWN: + aimingDown = true; + aimingUp = false; + break; + } + + lastDBElev = millis(); + } +} + +void QuarterbackOld::update() +{ + // Setup the motors on startup so they go down to absolute zero + if (setupMotors) + { + if (millis() - lastElevationTime >= 8000) + { + setupMotors = false; + } + else + { + elevationMotors.write(SERVO_SPEED_DOWN); + } + // Control the motors based on if they want to aim up or down + } + else if (millis() - lastElevationTime >= ELEVATION_PERIOD) + { + if (aimingUp && currentElevation < MAX_ELEVATION) + { + // Set the elevation of the top to 50% higher + elevationMotors.write(SERVO_SPEED_UP); + currentElevation += 50; + aimingUp = false; + lastElevationTime = millis(); + } + else if (aimingDown && currentElevation > 0) + { + // Set the elevation of the bottom to 50% lower + elevationMotors.write(SERVO_SPEED_DOWN); + currentElevation -= 50; + aimingDown = false; + lastElevationTime = millis(); + } + else + { + // Stop the linear actuators if it is not supposed to move up or down anymore + elevationMotors.write(SERVO_SPEED_STOP); + } + } + + // update flywheels if needed + if (flywheelsOn) + { + flywheelMotor.write(rampFW(FLYWHEEL_SPEED_FULL + flywheelSpeedFactor)); + } + else + { + flywheelMotor.write(rampFW(FLYWHEEL_STOP_SPEED)); + } +} + +void QuarterbackOld::toggleConveyor() +{ + // Debounce for button press + if (millis() - lastDBConv >= DEBOUNCE_WAIT) + { + // Toggle the conveyor between on or off + if (!conveyorOn) + { + conveyorMotor.write(CONVEYOR_ON); + } + else + { + conveyorMotor.write(CONVEYOR_OFF); + } + // Toggle the bool so we know which mode it is in + conveyorOn = !conveyorOn; + lastDBConv = millis(); + } +} + +void QuarterbackOld::changeFWSpeed(SpeedStatus speed) +{ + // Debounce for button press + if (millis() - lastDBFWChange >= DEBOUNCE_WAIT) + { + // Change the speed factor based on whether the user wants to increase or decrease (HARD CODED) + switch (speed) + { + case INCREASE: + arrayPos++; + break; + case DECREASE: + arrayPos--; + break; + } + + // Cap the arrayPos so it doesn't go out of bounds + arrayPos = constrain(arrayPos, 0, 2); + + // Set the flywheelSpeedFactor + flywheelSpeedFactor = speedFac[arrayPos]; + + // Update the motors if they are spinning for the new speed + // if (flywheelsOn){ + // flywheelMotor.write(FLYWHEEL_SPEED_FULL + flywheelSpeedFactor); + // } else { + // flywheelMotor.write(FLYWHEEL_STOP_SPEED); + // } + + lastDBFWChange = millis(); + } +} diff --git a/components/Robot/include/Quarterback.h b/components/Robot/include/Quarterback.h index 9cfb736..c82c994 100644 --- a/components/Robot/include/Quarterback.h +++ b/components/Robot/include/Quarterback.h @@ -1,90 +1,568 @@ -#pragma once +/* + Question: Why are there so many #defines commented out? + Answer: Documentation purposes. -#ifndef QUARTERBACK_H -#define QUARTERBACK_H + Question: Why not just use the #defines as they are written to generate the values? + Answer: I don't trust the preprocessor. -MP +*/ + +//* Use https://blocks.jkniest.dev/ to generate comment block headers + +#ifndef QUARTERBACK_TURRET_H +#define QUARTERBACK_TURRET_H #include #include #include -#include // ESP PS5 library, access using global instance `ps5` +#include // ESP PS5 library, access using global instance `ps5` +#include // Magnetometer +#include +#include // For ESP-to-ESP UART -// Flywheel defines -#define FLYWHEEL_SPEED_FULL 0.4 -#define FLYWHEEL_STOP_SPEED 0 - -#define FW_TIME_INCREMENT 25 -#define FW_BRAKE_PERCENTAGE 0.9 -#define FW_ACCEL_RATE .025 +enum TurretMode +{ + manual, + automatic, + combine +}; -// Elevation (linear actuators) defines -#define SERVO_SPEED_UP 1 -#define SERVO_SPEED_STOP 0 -#define SERVO_SPEED_DOWN -1 -#define MAX_ELEVATION 100 -#define ELEVATION_PERIOD 3750 +enum TurretUnits +{ + degrees, + counts +}; -// Conveyor defines -#define CONVEYOR_ON 1 -#define CONVEYOR_OFF 0 +enum AssemblyAngle +{ + straight, + angled, + unknownAngle +}; -// Debounce Vars -#define DEBOUNCE_WAIT 250 +enum CradleState +{ + forward, + back +}; -#define NUM_SPEED_INCREMENTS 4 +enum TargetReceiver +{ + receiver_1, + receiver_2 +}; -// Enum for whether to aim up or aim down -enum QBAim +enum CombinePosition { - AIM_UP, - AIM_DOWN + combineLeft, + combineStraight, + combineRight }; -enum QBElevation +enum FlywheelSpeed { - LOW_ELEVATION, - HIGH_ELEVATION + 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.225, 0.35, 0.45, 1.0}; // with top prongs +const float flywheelSpeeds[QB_TURRET_NUM_SPEEDS] = {-0.1, 0, 0.1, 0.215, 0.31, 0.3875, 1.0}; // without top prongs + +//================================// +// Debounce and 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_ASSEMBLY_TILT_DELAY 200L +#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.15 // was 0.25, turned down for combine + +//================================// +// Speed Constants // +//================================// +#define QB_MIN_PWM_VALUE 0.1 +#define QB_HOME_PCT 0.125 +#define QB_HANDOFF 0.3 +#define QB_HOME_MAG 0.1 +#define QB_ASM_SPEED 0.3 + +//========================================// +// Turret Angle Calculation Constants // +//========================================// +// QB_COUNTS_PER_ENCODER_REV Number of ticks per encoder revolution +// QB_COUNTS_PER_TURRET_REV Encoder Gear: 18t; Turret Track: 152t. Encoder spins 8.4 times for every turret revolution = 8500 ticks/rev +// QB_COUNTS_PER_TURRET_DEGREE (8500/360) = 23.61... 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 1250 +#define QB_COUNTS_PER_TURRET_REV 10556 // actually 10555.55555...6, but rounded up +#define QB_COUNTS_PER_TURRET_DEGREE 29.321 +#define QB_TURRET_SLOP_COUNTS 540 // deprecated +// #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 // 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, for example if 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 +// TODO: maybe unnecessary now? +#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 or Combine Mode for testing +#define QB_AUTO_ENABLED false + +//===============================// +// UART Communication Pins // +//===============================// +// TODO: move to appropriate area and pass into constructor +// #define RX2 16 // reciever pin +// #define TX2 17 // transmitter pin + +// ???????? +#ifdef RX2 +#undef RX2 +#endif +#ifdef TX2 +#undef TX2 +#endif +#define RX2 16 +#define TX2 17 /** - * @brief Quarterback Subclass Header - * @authors Rhys Davies + * @brief Quarterback Turret Subclass Header + * @authors Maxwell Phillips, George Rak */ class Quarterback : public Robot { + +#pragma region Private + //============================// + //| |// + //| PRIVATE |// + //| |// + //============================// private: - uint8_t flywheelPin; - uint8_t conveyorPin; - uint8_t elevationPin; - int arrayPos = 0; - PWMMotor flywheelMotor; - PWMMotor conveyorMotor; - PWMMotor elevationMotors; - bool flywheelsOn, conveyorOn; - bool aimingUp, aimingDown; - bool raise, lower; - bool setupMotors; - uint8_t currentElevation, targetElevation; - unsigned long lastElevationTime = 0; - unsigned long lastFlywheelToggleTime = 0; - unsigned long lastFlywheelRampTime = 0; - float currentFlywheelPower = 0; - float flywheelSpeedFactor = 0; - unsigned long lastDBElev = 0, lastDBFW = 0, lastDBFWChange = 0, lastDBConv = 0; // DB is for debounce - const float speedFac[NUM_SPEED_INCREMENTS] = {0.0, 0.45, 0.75}; // the power levels are truly 0.3, 0.75, 1 because its FLYWHEEL_SPEED_FULL + value in array + //==============================// + // PWM Motor Instances // + //==============================// + PWMMotor cradleActuator; + PWMMotor turretMotor; + PWMMotor assemblyMotor; + PWMMotor flywheelLeftMotor; + PWMMotor flywheelRightMotor; + + //==============================// + // Pin Declarations // + //==============================// + 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; + + //==============================// + // Autonomous Targeting // + //==============================// + // mode: manual or autonomous (or combine) + // target: reciever1 or reciever2 + TurretMode mode; + TargetReceiver target; + CombinePosition combinePosition; + + //==============================// + // Setup and Status 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; + + //===============================// + // Assembly Movement // + //===============================// + // currentAssemblyAngle initial state is unknown, will reset to [straight]. options: [straight, angled]. + // targetAssemblyAngle defaults to [straight]. options: [straight, angled]. + // assemblyMoving defaults to false + AssemblyAngle currentAssemblyAngle; + AssemblyAngle targetAssemblyAngle; + bool assemblyMoving; + uint32_t assemblyStartTime; + bool assemblyTriggerToggled; + + //===============================// + // Ball Cradle State Variables // + //===============================// + // currentCradleState initial state is unknown, will reset to [back]. options: [forward, back]. + // targetCradleState defaults to [back]. options: [forward, back]. + // cradleMoving defaults to [false] + // cradleStartTime + CradleState currentCradleState; + CradleState targetCradleState; + bool cradleMoving; + uint32_t cradleStartTime; + + //===============================// + // Flywheel State // + //===============================// + // 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 State // + //===============================// + // currentTurretSpeed default = 0 + // targetTurretSpeed default = 0 + float currentTurretSpeed; + float targetTurretSpeed; + int8_t utmsCtr = 0; // temp ctr for debugging so updateTurretMotionStatus doesn't spam as much +#define UTMS_CTR_MAX 15 + + //=====================================// + // Private Encoder State Variables // + //=====================================// + // * see also header [[Public Encoder State Variables for ISR]] + // targetTurretEncoderCount default = 0. complementary to [currentTurretEncoderCount] + // errorEncoderCount the error [currentTurretEncoderCount - targetTurretEncoderCount] + // 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 + int32_t targetTurretEncoderCount; + int32_t errorEncoderCount; + int32_t slopError; // TODO: remove with new encoder + int32_t stopError; // TODO: remove with new encoder (?) + bool turretMoving; + uint8_t manualHeadingIncrementCount; + + //===============================// + // Robot Relative Headings // + //===============================// + int16_t currentRelativeHeading; // default is undefined + int16_t targetRelativeHeading; // default = 0 //* as of 2024-11-15, in degrees only + int32_t currentRelativeTurretCount; // default is undefined + + //========================================// + // Absolute (World Relative) Headings // + //========================================// + // currentAbsoluteHeading + // targetAbsoluteHeading + // turretLaserState + int16_t currentAbsoluteHeading; // default = 0 + // TODO: is unused, merge with headingRad/headingDeg + int16_t targetAbsoluteHeading; // default = 0 + uint8_t turretLaserState; // triggered or not + // TODO: seems to be unused, remove? + + //================================// + // Control Input Debouncers // + //================================// + Debouncer *dbShare; + Debouncer *dbOptions; + Debouncer *dbSquare; + Debouncer *dbDpadUp; + Debouncer *dbDpadDown; + Debouncer *dbDpadLeft; + Debouncer *dbDpadRight; + + //===============================// + // Macro Button Debouncers // + //===============================// + //* used to set a delay that a button must be held in order to trigger a macro + Debouncer *dbCircle; + Debouncer *dbTriangle; + Debouncer *dbCross; + Debouncer *dbTurretInterpolator; // TODO: seems to be unused, remove? + +#pragma region Magnetometer + //==========================// + //| |// + //| Magnetometer |// + //| |// + //==========================// + Adafruit_LIS3MDL lis3mdl; // magnetometer object + bool useMagnetometer = false; // set 'false' to disable the magnetometer and its functions + bool holdTurretStillEnabled = true; // set 'false' if you only want to use the magnetometer for the handoff and not the hold steady + + //============================// + // Magnetometer Calibration // + //============================// + //* used at each startup + // TODO: make a class or struct for all these + // mag_xVal, mag_yVal: The current x and y values read by the magnetometer (after adjustments) + // mag_xMax, mag_xMin: During calibration the max and min X values are recorded + // mag_yMax, mag_yMin: During calibration the max and min Y values are recorded + // mag_xHalf, mag_yHalf: Half of the total range of expected x and Y values (Used to shift values back to origin [0,0]) + // mag_xSign, mag_ySign: Determines whether to add to the half value or subtract from it when shifting values back to origin (true = subtract) + int mag_yVal = 0; + int mag_xVal = 0; + int mag_xMax = -1000000; + int mag_xMin = 1000000; + int mag_xHalf = 0; + int mag_yMax = -1000000; + int mag_yMin = 1000000; + int mag_yHalf = 0; + bool mag_xSign = false; + bool mag_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 heading in degrees -> uses headingRad + */ + float headingRad; // TODO: merge with robot abs pot vars + float headingDeg; // TODO: merge with robot abs pot vars + +//==================================// +// Magnetometer PID Variables // +//==================================// +// PID_ERROR_AVG_ARRAY_LENGTH The length of the array used for averaging the error + +// prevErrorVals Array of previous error values used to average the last few error values 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 +// firstAverage On the first error calculations, the array is filled with zeros. +// This accounts for that by filling the entire array with the current reading. + +// previousTime: Used to calculate 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 +// 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 +#define PID_ERROR_AVG_ARRAY_LENGTH 5 + // TODO: convert to appropriate (u)int#_t types + int prevErrorVals[PID_ERROR_AVG_ARRAY_LENGTH] = {0, 0, 0, 0, 0}; + int prevErrorIndex = 0; + bool firstAverage = true; + long previousTime = 0; + float ePrevious = 0; + float eIntegral = 0; + float kp = 0.005; + float ki = 0.0012; + float kd = 0.0; + float turretPIDSpeed = 0; + float minMagSpeed = .075; + + //============================// + // Magnetometer Functions // + //============================// + // magnetometerSetup Sets some of the parameters. Runs once on startup. + // calibMagnetometer Rotates turret once on startup, records readings and sets values that will scale/transform + // outputs to the correct angles + // 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 + void magnetometerSetup(); + void calibMagnetometer(); + void calculateHeadingMag(); + void holdTurretStill(); + float turretPIDController(float current, float target, float kp, float kd, float ki, float maxSpeed); + + /* MAGNETOMETER CURRENT STATE NOTES / PLAN (from April 9th, 2024 7:56 PM) + - 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 + - Scan and Score capstone integration needs done to control angle and throw distance + - Testing needs done to see how much flywheels beign on affects magnetometer + - Relative velocities should be taken into account with trajectory calculations + */ +#pragma endregion + + //====================================// + // Private UART Communication // + //====================================// + String recievedMessage = ""; + + //=============================// + // Misc. Private Functions // + //=============================// + void moveAssemblySubroutine(); + 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); +#pragma endregion + +#pragma region Public + //===========================// + //| |// + //| PUBLIC |// + //| |// + //===========================// public: + //========================// + // Constructor // + //========================// Quarterback( - uint8_t flywheelPin, - uint8_t conveyorPin, - uint8_t elevationPin); - void setup(); - void action() override; //! robot subclass must override action - void toggleFlywheels(); - float rampFW(float requestedpwr); - void aim(QBAim dir); - void toggleConveyor(); - void changeFWSpeed(SpeedStatus speed); - void update(); + uint8_t flywheelLeftPin, // M1 + uint8_t flywheelRightPin, // M2 + uint8_t cradlePin, // M3 + uint8_t turretPin, // M4 + uint8_t assemblyPin, // S1 + uint8_t magnetometerSdaPin, // S3 + uint8_t magnetometerSclPin, // S4 + uint8_t turretEncoderPinA, // E1A + uint8_t turretEncoderPinB, // E1B + uint8_t turretLaserPin // E2A + ); + + bool magnetometerCalibrated = false; + + //===================================// + // Quarterback General Functions // + //===================================// + // action robot sublass must override action + void action() override; + + // [Startup] + // zeroTurret Runs through the routine to zero the turret + // reset zero turret, aim down (straight), and set flywheels to slow intake + void zeroTurret(); + void reset(); + + // [Safety] + // 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 Used for safety, returns false if the robot is disabled or should be stopped + void setEnabled(bool enabled); + bool isEnabled(); + void emergencyStop(); + bool testForDisableOrStop(); + + // [Debug] + // printDebug Consolidating printouts for the different functions into one locations for debugging purposes + // testRoutine Used for debugging + void printDebug(); + void testRoutine(); + + //====================================// + // Quarterback Subsystem Controls // + //====================================// + // 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 the cradle 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, bool force = false); + void moveCradle(CradleState state, bool force = false); + void setFlywheelSpeed(float absoluteSpeed); + void setFlywheelSpeedStage(FlywheelSpeed stage); + void adjustFlywheelSpeedStage(SpeedStatus speed); + + //===================================// + // Quarterback Automatic Targeting // + //===================================// + // switchMode Used to switch between manual and automatic mode (overloaded function) + // switchTarget Switch between receiver1 or receiver2 + void switchMode(TurretMode mode); + void switchMode(); + void switchTarget(TargetReceiver target); + + //==================================// + // Quarterback Strategic Macros // + //==================================// + // loadFromCenter Prepares the QB to accept the ball from the center + // handoff Hands the ball to the runningback + void loadFromCenter(); + void handoff(); + + //==========================================// + // Public Encoder State Variables for ISR // + //==========================================// + // * see also header [[Private Encoder State Variables]] + // * any variables used in the turretEncoderISR() (interrupt service routine) must be public static + static int32_t currentTurretEncoderCount; // updated by encoder ISR (interrupt service routine). complementary to [targetTurretEncoderCount]. + static uint8_t turretEncoderStateB; // A channel will always be 1 when interrupt triggers, so only care about channel B + + //===============================================// + // Encoder Interrupt Service Routine (ISR) // + //===============================================// + // must be static. simple function called when encoder interrupts are triggered. updates [currentTurretEncoderCount]. + static void turretEncoderISR(); + + // If the direction changes then some things need to happen differently because of the incredible amount of backlash + // TODO: remove? may not be needed with new encoder system + void turretDirectionChanged(); + + //===================================// + // Public UART Communication // + //===================================// + int motor1Value = 0; + int motor2Value = 0; + void updateReadMotorValues(); + +#pragma endregion }; -#endif // QUARTERBACK_H +#endif // QUARTERBACK_TURRET_H diff --git a/components/Robot/include/QuarterbackOld.h b/components/Robot/include/QuarterbackOld.h new file mode 100644 index 0000000..d4b2687 --- /dev/null +++ b/components/Robot/include/QuarterbackOld.h @@ -0,0 +1,90 @@ +#pragma once + +#ifndef QUARTERBACK_H +#define QUARTERBACK_H + +#include +#include +#include +#include // ESP PS5 library, access using global instance `ps5` + +// Flywheel defines +#define FLYWHEEL_SPEED_FULL 0.4 +#define FLYWHEEL_STOP_SPEED 0 + +#define FW_TIME_INCREMENT 25 +#define FW_BRAKE_PERCENTAGE 0.9 +#define FW_ACCEL_RATE .025 + +// Elevation (linear actuators) defines +#define SERVO_SPEED_UP 1 +#define SERVO_SPEED_STOP 0 +#define SERVO_SPEED_DOWN -1 +#define MAX_ELEVATION 100 +#define ELEVATION_PERIOD 3750 + +// Conveyor defines +#define CONVEYOR_ON 1 +#define CONVEYOR_OFF 0 + +// Debounce Vars +#define DEBOUNCE_WAIT 250 + +#define NUM_SPEED_INCREMENTS 4 + +// Enum for whether to aim up or aim down +enum QBAim +{ + AIM_UP, + AIM_DOWN +}; + +enum QBElevation +{ + LOW_ELEVATION, + HIGH_ELEVATION +}; + +/** + * @brief Quarterback Subclass Header + * @authors Rhys Davies + */ +class QuarterbackOld : public Robot +{ +private: + uint8_t flywheelPin; + uint8_t conveyorPin; + uint8_t elevationPin; + int arrayPos = 0; + PWMMotor flywheelMotor; + PWMMotor conveyorMotor; + PWMMotor elevationMotors; + bool flywheelsOn, conveyorOn; + bool aimingUp, aimingDown; + bool raise, lower; + bool setupMotors; + uint8_t currentElevation, targetElevation; + unsigned long lastElevationTime = 0; + unsigned long lastFlywheelToggleTime = 0; + unsigned long lastFlywheelRampTime = 0; + float currentFlywheelPower = 0; + float flywheelSpeedFactor = 0; + unsigned long lastDBElev = 0, lastDBFW = 0, lastDBFWChange = 0, lastDBConv = 0; // DB is for debounce + const float speedFac[NUM_SPEED_INCREMENTS] = {0.0, 0.45, 0.75}; // the power levels are truly 0.3, 0.75, 1 because its FLYWHEEL_SPEED_FULL + value in array + +public: + QuarterbackOld( + uint8_t flywheelPin, + uint8_t conveyorPin, + uint8_t elevationPin); + void setup(); + void action() override; //! robot subclass must override action + void toggleFlywheels(); + float rampFW(float requestedpwr); + void aim(QBAim dir); + void toggleConveyor(); + void changeFWSpeed(SpeedStatus speed); + void update(); +}; + +#endif // QUARTERBACK_H diff --git a/components/Types/BotTypes.cpp b/components/Types/BotTypes.cpp index 7bbb7e3..d6ee2e8 100644 --- a/components/Types/BotTypes.cpp +++ b/components/Types/BotTypes.cpp @@ -11,8 +11,7 @@ constexpr Pair {center_conversion, "center_conversion"}, {kicker, "kicker"}, {quarterback_old, "quarterback_old"}, - {quarterback_base, "quarterback_base"}, - {quarterback_turret, "quarterback_turret"}}; + {quarterback, "quarterback"}}; // Function to map BotTypes to human-readable C-strings const char *getBotTypeString(BotType type) diff --git a/components/Types/include/BotTypes.h b/components/Types/include/BotTypes.h index d2bcab3..cf5353a 100644 --- a/components/Types/include/BotTypes.h +++ b/components/Types/include/BotTypes.h @@ -8,7 +8,7 @@ #include #include -#define NUM_POSITIONS 9 // number of members of eBOT_TYPE +#define NUM_POSITIONS 8 // number of members of eBOT_TYPE /** BotType * enum for the possible positions a robot can have on the field @@ -19,10 +19,10 @@ * 1: Receiver * 2: Runningback * 3: Center - * 4: Kicker - * 5: Old Quarterback - * 6: Quarterback base - * 7: Quarterback turret + * 4: Center Conversion + * 5: Kicker + * 6: Old Quarterback + * 7: Quarterback */ typedef enum { @@ -33,8 +33,7 @@ typedef enum center_conversion, kicker, quarterback_old, - quarterback_base, - quarterback_turret + quarterback } BotType; /** @@ -80,20 +79,18 @@ typedef struct BotConfig #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_QB_BASE 14 -#define BOT_QB_BOTTOM 14 -#define BOT_QB_TURRET 15 -#define BOT_QB_TOP 15 -#define BOT_LINEMAN_V1 16 -#define BOT_420 17 -#define BOT_24 18 -#define BOT_25 19 +#define BOT_QB 13 +#define BOT_BETA 13 +#define BOT_LINEMAN_V1 14 +#define BOT_420 15 +#define BOT_24 16 +#define BOT_25 17 // PRESET BOT CONFIGURATIONS, MUST MATCH: +// TODO: bogConfigArray set up for new Quarterback // 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 @@ -110,12 +107,11 @@ constexpr bot_config_t botConfigArray[NUM_BOTS] = { {10, "phi", center, {small_ampflow, 0.6f, 11.50f, 9.00f, 36.00f}}, //* 10: Φ {11, "inf", quarterback_old, {small_ampflow, 0.5625f, 11.50f, 9.00f, 24.00f}}, //* 11: ∞ {12, "theta", kicker, {small_ampflow, 0.5f, 10.00f, 9.00f, 36.00f}}, //* 12: Θ - {14, "qb_base", quarterback_base, {big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f}}, //* 14: unassigned - {15, "qb_turret", quarterback_turret, {falcon, 0.5f, 11.50f, 9.00f, 36.00f}}, //* 15: unassigned - {16, "l-man-v1", lineman, {small_12v, 1.0f, 11.00f, 9.00f, 36.00f}}, //* 16: generic lineman V1 - {17, "420", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}}, //* 17: 420 - {18, "24", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}}, //* 18: 24 - {19, "25", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}} //* 19: 25 + {13, "beta", quarterback, {big_ampflow, 0.5f, 11.50f, 9.00f, 36.00f}}, //* 13: beta + {14, "l-man-v1", lineman, {small_12v, 1.0f, 11.00f, 9.00f, 36.00f}}, //* 14: generic lineman V1 + {15, "420", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}}, //* 15: 420 + {16, "24", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}}, //* 16: 24 + {17, "25", lineman, {small_12v, 1.0f, 11.00f, 5.50f, 18.00f}} //* 17: 25 }; //! Do not decrease r_min to less than half of the wheelbase, or the math might break diff --git a/components/Utilities/ConfigManager.cpp b/components/Utilities/ConfigManager.cpp index 7645667..8dadde1 100644 --- a/components/Utilities/ConfigManager.cpp +++ b/components/Utilities/ConfigManager.cpp @@ -35,6 +35,7 @@ void ConfigManager::read() this->config->bot_type = (BotType)preferences.getUChar("bot_type"); // read drive parameters + // TODO: might need to refactor to fit quarterback this->config->drive_params.motor_type = (MotorType)preferences.getUChar("motor_type"); this->config->drive_params.gear_ratio = (float)preferences.getFloat("gear_ratio"); this->config->drive_params.wheel_base = (float)preferences.getFloat("wheel_base"); diff --git a/main/main_robot.cpp b/main/main_robot.cpp index dc63ead..8846447 100644 --- a/main/main_robot.cpp +++ b/main/main_robot.cpp @@ -26,6 +26,7 @@ #include // Pairing Includes + #include // Robot Includes @@ -34,9 +35,8 @@ #include #include #include +#include #include -#include -#include // Types Includes #include @@ -75,30 +75,14 @@ void onConnection() // ps5.setLed(0, 255, 0); // set LED green } - // TODO: perm sln - if (robotType != quarterback_turret) - { - drive->emergencyStop(); - } - else - { - ((QuarterbackTurret *)robot)->emergencyStop(); - } + drive->emergencyStop(); } void onDisconnect() { Serial.println(F("Controller Disconnected.")); - // TODO: perm sln - if (robotType != quarterback_turret) - { - drive->emergencyStop(); - } - else - { - ((QuarterbackTurret *)robot)->emergencyStop(); - } + drive->emergencyStop(); } extern "C" void main_app(void) @@ -121,6 +105,8 @@ extern "C" void main_app(void) pinMode(LED_BUILTIN, OUTPUT); pinMode(TACKLE_PIN, OUTPUT); // Try INPUT_PULLUP digitalWrite(TACKLE_PIN, 0); // Initially sets tackle sensor to home + digitalWrite(LED_BUILTIN, LOW); + // digitalMode(LED_BUILTIN, OUTPUT); // Initialize debouncer dbOptions = new Debouncer(TACKLE_SENSOR_DELAY); @@ -151,7 +137,7 @@ extern "C" void main_app(void) drive->setupMotors(DRIVE_M1, DRIVE_M2); break; case quarterback_old: - robot = new Quarterback(SPECBOT_PIN1, SPECBOT_PIN2, SPECBOT_PIN3); + robot = new QuarterbackOld(SPECBOT_PIN1, SPECBOT_PIN2, SPECBOT_PIN3); drive = new Drive(quarterback_old, driveParams); drive->setupMotors(DRIVE_M1, DRIVE_M2); break; @@ -170,10 +156,13 @@ extern "C" void main_app(void) drive = new Drive(runningback, driveParams); drive->setupMotors(DRIVE_M1, DRIVE_M2); break; - case quarterback_turret: - robot = new QuarterbackTurret( - M1_IDX, // left flywheel - M2_IDX, // right flywheel + case quarterback: + drive = new Drive(quarterback, driveParams); + /* USE SPECIFIC PINS FOR QUARTERBACK */ + drive->setupMotors(M1_IDX, M2_IDX); + robot = new Quarterback( + M1_PWM, // left flywheel + M2_PWM, // right flywheel M3_PIN, // cradle M4_PIN, // turret SPECBOT_PIN1, // assembly motor @@ -184,11 +173,6 @@ extern "C" void main_app(void) ENC2_CHB // zeroing laser ); break; - case quarterback_base: - drive = new Drive(quarterback_base, driveParams); - drive->setupMotors(DRIVE_M1, DRIVE_M2); - robot = new QuarterbackBase(drive); - break; case receiver: case lineman: default: // Assume lineman @@ -202,7 +186,6 @@ extern "C" void main_app(void) } drive->printSetup(); - //! Activate Pairing Process: this code is BLOCKING, not instantaneous activatePairing(); @@ -233,12 +216,11 @@ extern "C" void main_app(void) // 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) + // Drive controls for non-QB + if (robotType != quarterback) { + // Do all normal drive functions as usual drive->setStickPwr(ps5.LStickY(), ps5.RStickX()); - // determine BSN percentage (boost, slow, or normal) if (ps5.Touchpad()) { @@ -264,18 +246,35 @@ extern "C" void main_app(void) drive->setSpeedScalar(Drive::NORMAL); } - // Manual Home / Away Position Setting - if (dbOptions->debounceAndPressed(ps5.Options())) - { - switchTackleSensor(); - } - //* 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 } + + // Drive controls for QB only in drive mode (i.e. when not enabled) + // Should only get to this point if the robot is a QB, so we can cast robot as a Quarterback without issue + else if (!((Quarterback *)robot)->isEnabled()) + { + // If the QB is not enabled, allow driving but not manual turret or flywheel movement + drive->setStickPwr(ps5.LStickY(), ps5.RStickX()); + + // avoid using R1, L1, touchpad, etc. as they are used in Quarterback control scheme + // for different functions like changing recievers + drive->setSpeedScalar(Drive::NORMAL); + + //* 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 + } // else robot is quarterback and also drive is disbled! SO DO NOTHING! + + // Manual Home / Away Position Setting + // if (ps5.Options()) + // ; + //! Performs all special robot actions depending on the instantiated Robot subclass robot->action(); @@ -287,15 +286,8 @@ extern "C" void main_app(void) } else { // no response from PS5 controller within last 300 ms, so stop - if (robotType != quarterback_turret) - { - // Emergency stop if the controller disconnects - drive->emergencyStop(); - } - else - { - ((QuarterbackTurret *)robot)->emergencyStop(); - } + // Emergency stop if the controller disconnects + drive->emergencyStop(); } } @@ -309,4 +301,4 @@ extern "C" void main_app(void) */ // WARNING: if program reaches end of function app_main() the MCU will restart. -} \ No newline at end of file +} diff --git a/main/main_write_bot_info.cpp b/main/main_write_bot_info.cpp index 0775017..0ef502c 100644 --- a/main/main_write_bot_info.cpp +++ b/main/main_write_bot_info.cpp @@ -17,7 +17,7 @@ extern "C" void main_app(void) #ifndef BOT_INDEX #define BOT_INDEX 0 // Default to 0 if not defined #endif - uint8_t index = 3; + uint8_t index = 13; //* CUSTOM BOT CONFIGURATION BotType bot_type = lineman;