From 4b0716300997f37e773fac098477a743a17667fb Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:13:23 -0700 Subject: [PATCH 01/49] full target tuning, power control button, override button, enable intakes --- src/main/deploy/launcher_data.csv | 23 ++++++++++--- .../paths/LowSpeakerToMiddleLowNote.path | 2 +- .../java/frc/team2412/robot/Controls.java | 4 ++- .../java/frc/team2412/robot/Hardware.java | 8 ++--- .../commands/launcher/ManualAngleCommand.java | 18 ++++++++-- .../robot/sensors/AprilTagsProcessor.java | 3 +- .../robot/subsystems/IntakeSubsystem.java | 33 ++++++++++--------- .../robot/subsystems/LauncherSubsystem.java | 30 ++++++++++++++--- .../team2412/robot/util/auto/AutoLogic.java | 8 +++-- 9 files changed, 93 insertions(+), 36 deletions(-) diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data.csv index 526a3b0d..52de7dab 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data.csv @@ -7,17 +7,30 @@ # Front subwoofer 1.30, 254, 3500 # Front bumper at autoline -2.13, 250, 3500 +2.13, 234, 3500 + +# dcmps + +# On Autoline N2 +2.81, 223, 3600 + + + + + +# bad values? + # Near podium -5.66, 246, 3600 +# 5.66, 225, 3600 # Auto point 1 (X 2.870, Y 2.196, theta 130.6) -4.46, 242, 3600 +# 4.46, 230, 3600 # After this, these are setpoint angles # Auto point 2 (X 5.699, Y 7.501, theta -162.8) -5.55, 240.3, 4400 +# 5.55, 226, 4400 # Auto point 3 (X 4.299, Y 5.134, theta 176.222) -4.32, 242, 4300 # Might be a little low, and could probably lower RPM- Ran out of time to fully test +# 4.32, 232, 4300 # Might be a little low, and could probably lower RPM- Ran out of time to fully test + # Pre 3/28 values: # 1.397, 254, 4500 diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index b89a2a3c..a1bdcba2 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -103,5 +103,5 @@ "rotation": 120.0, "velocity": 0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index d502bf73..70ed6385 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -218,7 +218,9 @@ private void bindLauncherControls() { s.launcherSubsystem, () -> MathUtil.applyDeadband(codriveController.getLeftY(), 0.1) - * LauncherSubsystem.ANGLE_MAX_SPEED)); + * LauncherSubsystem.ANGLE_MAX_SPEED, + codriveController.leftBumper(), + codriveController.back())); launcherLowerPresetButton.onTrue( s.launcherSubsystem diff --git a/src/main/java/frc/team2412/robot/Hardware.java b/src/main/java/frc/team2412/robot/Hardware.java index f5af0645..5101e2b6 100644 --- a/src/main/java/frc/team2412/robot/Hardware.java +++ b/src/main/java/frc/team2412/robot/Hardware.java @@ -22,8 +22,8 @@ public class Hardware { // intake [40 - 49] public static final int INTAKE_MOTOR_FRONT = 40; - public static final int INTAKE_MOTOR_BACK = 41; - public static final int INTAKE_MOTOR_LEFT = 42; + public static final int INTAKE_MOTOR_BACK = 42; + public static final int INTAKE_MOTOR_LEFT = 41; public static final int INTAKE_MOTOR_RIGHT = 43; public static final int INGEST_MOTOR = 44; @@ -31,8 +31,8 @@ public class Hardware { public static final int INDEX_MOTOR_UPPER = 45; public static final int FEEDER_MOTOR = 46; - // LED strip is PWM port 8 - public static final int BLINKIN_LED = 8; + // LED strip is PWM port 2 + public static final int BLINKIN_LED = 2; // intake sensors (Digital IO) public static final int INDEX_SENSOR = 1; diff --git a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index d7b04974..ff4da61b 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -2,22 +2,36 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.team2412.robot.subsystems.LauncherSubsystem; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; // this command adjusts the angle using a speed that is input by the user public class ManualAngleCommand extends Command { private final LauncherSubsystem launcherSubsystem; private final DoubleSupplier launcherAngle; + private final BooleanSupplier powerControl; + private final BooleanSupplier ignoreLimits; - public ManualAngleCommand(LauncherSubsystem launcherSubsystem, DoubleSupplier angleSpeed) { + public ManualAngleCommand( + LauncherSubsystem launcherSubsystem, + DoubleSupplier angleSpeed, + BooleanSupplier powerControl, + BooleanSupplier ignoreLimits) { this.launcherSubsystem = launcherSubsystem; this.launcherAngle = angleSpeed; + this.powerControl = powerControl; + this.ignoreLimits = ignoreLimits; addRequirements(launcherSubsystem); } @Override public void execute() { - launcherSubsystem.setAngleManual(launcherAngle.getAsDouble()); + launcherSubsystem.setAngleManual(launcherAngle.getAsDouble(), powerControl.getAsBoolean(), ignoreLimits.getAsBoolean()); + } + + @Override + public void end(boolean interrupted) { + launcherSubsystem.restoreLimits(); } @Override diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index b2b4f1c4..ac3fdc60 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -50,7 +50,8 @@ private static class FilterInfo {} new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0)); // TODO Measure these - private static final Vector STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(30)); + private static final Vector STANDARD_DEVS = + VecBuilder.fill(0.7, 0.7, Units.degreesToRadians(30)); private static final double MAX_POSE_AMBIGUITY = 0.1; diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 266e67fc..5bef335c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -35,7 +35,7 @@ public class IntakeSubsystem extends SubsystemBase { public static final double FEEDER_IN_SPEED = 0.65; public static final double FEEDER_REVERSE_SPEED = -0.3; - private static final boolean enableFrontAndSideIntakes = false; + private static final boolean enableFrontAndSideIntakes = true; // Motors private final CANSparkMax intakeMotorFront; @@ -51,7 +51,7 @@ public class IntakeSubsystem extends SubsystemBase { private final SparkLimitSwitch indexSensor; private final SparkLimitSwitch feederSensor; private final DigitalInput feederSensorIR; - private final SparkLimitSwitch intakeFrontSensor; + // private final SparkLimitSwitch intakeFrontSensor; // debounce ! ! private final Debouncer intakeFrontSensorDebouncer; @@ -97,7 +97,8 @@ public IntakeSubsystem() { feederSensor = feederMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); feederSensorIR = new DigitalInput(FEEDER_SENSOR); - intakeFrontSensor = intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeFrontSensor = + // intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeBackSensor = // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); @@ -137,11 +138,11 @@ private void configureMotor(CANSparkBase motor, boolean invert) { } private void resetMotors() { - configureMotor(intakeMotorFront, true); - configureMotor(intakeMotorLeft, true); - configureMotor(intakeMotorRight, true); + configureMotor(intakeMotorFront, 25, true); + configureMotor(intakeMotorLeft, 25, true); + configureMotor(intakeMotorRight, 25, true); - configureMotor(ingestMotor, true); + configureMotor(ingestMotor, false); configureMotor(indexMotorUpper, 40, false); configureMotor(feederMotor, 40, true); @@ -274,7 +275,8 @@ public boolean feederSensorHasNote() { } public boolean intakeFrontSeesNote() { - return intakeFrontSensor.isPressed(); + return false; + // return intakeFrontSensor.isPressed(); } public boolean intakeLeftSeesNote() { @@ -290,21 +292,24 @@ public boolean debouncedIntakeFrontSensor() { // if (intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed())) { // return true; // } - return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // wreturn intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } public boolean debouncedIntakeLeftSensor() { // if (intakeLeftSensorDebouncer.calculate(intakeLeftSensor.isPressed())) { // return true; // } - return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // return intakeLeftSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } public boolean debouncedIntakeRightSensor() { // if (intakeRightSensorDebouncer.calculate(intakeRightSensor.isPressed())) { // return true; // } - return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + return false; + // return intakeRightSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } // override methods on shuffleboard @@ -317,7 +322,7 @@ public boolean getRejectOverride() { } public boolean isIntakeOn() { - return (intakeMotorFront.get() > 0 + return (intakeMotorLeft.get() > 0 || indexMotorUpper.get() > 0 || ingestMotor.get() > 0 || feederMotor.get() > 0); @@ -399,8 +404,6 @@ public void initShuffleboard() { * returns true if all the motors are set to be not moving */ public boolean isIntakeRunning() { - return (intakeMotorFront.get() != 0 - && intakeMotorLeft.get() != 0 - && intakeMotorRight.get() != 0); + return (intakeMotorLeft.get() != 0 && intakeMotorRight.get() != 0); } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index f28bb5ce..e9b728c1 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -11,6 +11,7 @@ import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.units.BaseUnits; @@ -40,7 +41,7 @@ public class LauncherSubsystem extends SubsystemBase { private static final float PIVOT_DISABLE_OFFSET = 0.04f; private static final int PIVOT_OFFSET = 36; // ANGLE VALUES - public static final int AMP_AIM_ANGLE = 288 + PIVOT_OFFSET; + public static final int AMP_AIM_ANGLE = 285 + PIVOT_OFFSET; public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; @@ -57,6 +58,7 @@ public class LauncherSubsystem extends SubsystemBase { // RPM public static final int SPEAKER_SHOOT_SPEED_RPM = 4500; public static final int TRAP_SHOOT_SPEED_RPM = 4500; + public static final int LOBBING_RPM = 4700; public static final double ANGLE_MAX_SPEED = 1; public static final double MAX_SET_ANGLE_OFFSET = 20; // 3392 RPM = 50% Speed @@ -88,6 +90,7 @@ public class LauncherSubsystem extends SubsystemBase { private double rpmSetpoint; private double angleSetpoint; private double manualAngleSetpoint; + private boolean ignoreLimits; private GenericEntry setLauncherSpeedEntry; @@ -278,7 +281,22 @@ public double getAngleSpeed() { return launcherAngleEncoder.getVelocity(); } - public void setAngleManual(double joystickInput) { + public void restoreLimits() { + this.ignoreLimits = false; + } + + public void setAngleManual(double joystickInput, boolean powerControl, boolean ignoreLimits) { + this.ignoreLimits = ignoreLimits; + if (powerControl || ignoreLimits) { + launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.1 : joystickInput); + manualAngleSetpoint = + MathUtil.clamp( + Units.degreesToRotations(getAngle()), + PIVOT_SOFTSTOP_BACKWARD, + PIVOT_SOFTSTOP_FORWARD); + return; + } + manualAngleSetpoint = MathUtil.clamp( manualAngleSetpoint + joystickInput * MANUAL_MODIFIER, @@ -413,10 +431,12 @@ public void periodic() { // sanity check the pivot encoder if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET || launcherAngleEncoder.getPosition() <= PIVOT_SOFTSTOP_BACKWARD - PIVOT_DISABLE_OFFSET) { - launcherAngleOneMotor.disable(); + if (!ignoreLimits) { + launcherAngleOneMotor.disable(); + } DriverStation.reportError( - "Launcher encoder angle is insane!!!! Reports angle of " + getAngle() + " degrees.", - true); + "Launcher encoder angle is insane!!!! Reports angle of " + getAngle() + " degrees. Is overridden: " + ignoreLimits, + false); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 02e247d9..3cfb87d4 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -370,6 +370,10 @@ public static BooleanSupplier hasNoNote() { return (INTAKE_ENABLED ? () -> !s.intakeSubsystem.isIntakeRunning() : () -> true); } + public static BooleanSupplier hasNote() { + return (INTAKE_ENABLED ? () -> s.intakeSubsystem.feederSensorHasNote() : () -> true); + } + // registered commands public static Command subwooferLaunch() { @@ -404,7 +408,7 @@ public static Command subwooferLaunch() { .until(untilFeederHasNoNote())) .andThen(new WaitCommand(0.4)), Commands.none(), - hasNoNote())) + hasNote())) : Commands.none()) .withName("Auto - SubwooferLaunchCommand"); } @@ -434,7 +438,7 @@ public static Command visionLaunch() { .until(untilFeederHasNoNote())) .andThen(new WaitCommand(0.4)), Commands.none(), - hasNoNote())) + hasNote())) : Commands.none()) .withName("Auto - VisionLaunchCommand"); } From 0f8f79682325381e798a2f23db12e88c8851c2af Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Fri, 5 Apr 2024 09:43:05 -0700 Subject: [PATCH 02/49] vision launch datapoint + auto adding? --- src/main/deploy/launcher_data.csv | 3 +- .../PresetAmpSideAutolineFar4Score2.auto | 61 ++++++++ .../pathplanner/paths/FarN3ToLowSpeaker.path | 2 +- .../paths/HighSpeakerCenterlineN2Return.path | 141 ++++++++++++++++++ .../paths/LowNoteToLowSpeaker.path | 2 +- .../paths/LowSpeakerMidNote2Return.path | 2 +- .../paths/LowSpeakerPassAutoLine.path | 2 +- .../paths/LowSpeakerQCenterLineN5.path | 2 +- .../paths/LowSpeakerToFarN4Return.path | 41 ++--- .../paths/LowSpeakerToHighNoteReturn.path | 2 +- .../paths/LowSpeakerToMiddleLowNote.path | 32 ++-- .../paths/MiddleLowNoteToLowSpeaker.path | 39 ++--- .../commands/launcher/ManualAngleCommand.java | 3 +- .../robot/subsystems/LauncherSubsystem.java | 6 +- 14 files changed, 269 insertions(+), 69 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto create mode 100644 src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data.csv index 52de7dab..345afa40 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data.csv @@ -14,7 +14,8 @@ # On Autoline N2 2.81, 223, 3600 - +#chain +3.84, 211, 3600 diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto new file mode 100644 index 00000000..50b0e905 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetAmpSideAutolineFar4Score2.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerHighNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterLineNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerCenterlineN2Return" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path index 40507852..25c54e97 100644 --- a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path @@ -52,7 +52,7 @@ "y": 4.47 }, "prevControl": { - "x": 1.6972725837343565, + "x": 1.6972725837343567, "y": 3.0877308601464057 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path b/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path new file mode 100644 index 00000000..34780f55 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HighSpeakerCenterlineN2Return.path @@ -0,0 +1,141 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": null, + "nextControl": { + "x": 4.048666786792263, + "y": 6.7241388044029655 + }, + "isLocked": false, + "linkedName": "TopSpeaker" + }, + { + "anchor": { + "x": 8.293644849397962, + "y": 5.800300603450209 + }, + "prevControl": { + "x": 9.030376579271676, + "y": 5.613194132371172 + }, + "nextControl": { + "x": 7.9360527619599806, + "y": 5.8911176415296955 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.73, + "y": 6.62 + }, + "prevControl": { + "x": 1.2057927218565005, + "y": 7.331049617052808 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopSpeaker" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.05, + "rotationDegrees": -130.0, + "rotateFast": false + }, + { + "waypointRelativePos": 0.35, + "rotationDegrees": 170.2291447191198, + "rotateFast": true + }, + { + "waypointRelativePos": 0.85, + "rotationDegrees": 171.51986105015604, + "rotateFast": false + }, + { + "waypointRelativePos": 1.5, + "rotationDegrees": -173.34649448732242, + "rotateFast": false + }, + { + "waypointRelativePos": 1.85, + "rotationDegrees": -130.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.15, + "maxWaypointRelativePos": 1.05, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.8, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -120.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 466702ce..4dbd009a 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -20,7 +20,7 @@ "y": 4.47 }, "prevControl": { - "x": 1.1980586123334407, + "x": 1.198058612333441, "y": 3.974467068033235 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index b6baab31..46740492 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 1.689169744135214, - "y": 3.8609042538750162 + "y": 3.860904253875016 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index 21e23e74..fe9bbbef 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.2211813497261212, + "x": 1.2211813497261215, "y": 3.0616743218759597 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path index 634af2cb..ac8e0fa9 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.2649659988419504, + "x": 1.2649659988419506, "y": 3.9350340011580496 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path index 6bda5110..bdbd8a5a 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path @@ -8,7 +8,7 @@ }, "prevControl": null, "nextControl": { - "x": 1.148516485067158, + "x": 1.1485164850671583, "y": 3.4029989427500227 }, "isLocked": false, @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 3.78, - "y": 1.8944530196752711 + "x": 3.8, + "y": 1.9 }, "prevControl": { - "x": 2.6615614666991383, - "y": 2.126231436115029 + "x": 2.092354686019913, + "y": 1.9233883088848789 }, "nextControl": { - "x": 5.394077866540532, - "y": 1.559961314696977 + "x": 4.572240225750198, + "y": 1.8894232175819474 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 2.4960698570045796 }, "prevControl": { - "x": 8.02042238010854, - "y": 2.264505651461778 + "x": 7.976085063097927, + "y": 2.378058890871132 }, "nextControl": { - "x": 8.834401077526127, - "y": 2.7075036084231523 + "x": 9.030376579271676, + "y": 2.642878903991426 }, "isLocked": false, "linkedName": null @@ -49,15 +49,15 @@ { "anchor": { "x": 3.78, - "y": 1.7744559954678114 + "y": 1.8827588652328318 }, "prevControl": { "x": 5.181058023487222, - "y": 1.549460899884071 + "y": 1.6577637696490914 }, "nextControl": { "x": 2.3229040282308606, - "y": 2.0084501926723575 + "y": 2.116753062437377 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 4.47 }, "prevControl": { - "x": 1.5429233708823726, - "y": 2.778681091803989 + "x": 1.4993411183403562, + "y": 3.3094457072105032 }, "nextControl": null, "isLocked": false, @@ -84,12 +84,12 @@ }, { "waypointRelativePos": 1.0, - "rotationDegrees": -171.68, + "rotationDegrees": -171.13373619481098, "rotateFast": false }, { "waypointRelativePos": 1.75, - "rotationDegrees": -158.14409893935215, + "rotationDegrees": -167.33444776423735, "rotateFast": false }, { @@ -101,6 +101,11 @@ "waypointRelativePos": 2.6, "rotationDegrees": -164.3286881943862, "rotateFast": false + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 159.12457737382582, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index 2c532257..855a9b7b 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 1.6794199859183583, - "y": 3.80240570457388 + "y": 3.8024057045738795 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index a1bdcba2..ce9bac8b 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -9,23 +9,23 @@ "prevControl": null, "nextControl": { "x": 2.0485713043498155, - "y": 1.848940634818207 + "y": 1.8489406348182067 }, "isLocked": false, "linkedName": "LowSpeaker" }, { "anchor": { - "x": 6.97, + "x": 6.4, "y": 0.8 }, "prevControl": { - "x": 4.923673991492127, - "y": 0.785799769797738 + "x": 4.100008967931012, + "y": 0.8064228032996081 }, "nextControl": { - "x": 7.0774886027857535, - "y": 0.8007459040726312 + "x": 6.509999571098952, + "y": 0.7996928224508884 }, "isLocked": false, "linkedName": null @@ -36,7 +36,7 @@ "y": 0.77 }, "prevControl": { - "x": 7.848466239624848, + "x": 7.8505863938223595, "y": 0.7700000000000005 }, "nextControl": null, @@ -47,23 +47,23 @@ "rotationTargets": [ { "waypointRelativePos": 0.05, - "rotationDegrees": 120.0, - "rotateFast": false - }, - { - "waypointRelativePos": 0.25, - "rotationDegrees": 150.98859902992157, + "rotationDegrees": 130.0, "rotateFast": false }, { "waypointRelativePos": 0.5, "rotationDegrees": 175.80321002729156, - "rotateFast": false + "rotateFast": true }, { "waypointRelativePos": 0.65, "rotationDegrees": 180.0, "rotateFast": true + }, + { + "waypointRelativePos": 0.3, + "rotationDegrees": 174.13621568699799, + "rotateFast": false } ], "constraintZones": [], @@ -87,7 +87,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -103,5 +103,5 @@ "rotation": 120.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index 0102b3da..e6398465 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -8,36 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.01058639382236, - "y": 0.7700000000000005 + "x": 5.182999767708936, + "y": 1.5436283864020728 }, "isLocked": false, "linkedName": "CenterLineN5" }, - { - "anchor": { - "x": 3.0808084473311808, - "y": 1.363329583802025 - }, - "prevControl": { - "x": 3.9310570049711533, - "y": 0.7769512681882514 - }, - "nextControl": { - "x": 1.8459892684582195, - "y": 2.2149290175075143 - }, - "isLocked": false, - "linkedName": null - }, { "anchor": { "x": 0.73, "y": 4.47 }, "prevControl": { - "x": 1.4899471997783693, - "y": 3.081925867364013 + "x": 1.382399573915957, + "y": 2.140030262966509 }, "nextControl": null, "isLocked": false, @@ -46,21 +30,26 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.2, - "rotationDegrees": 180.0, + "waypointRelativePos": 0.25, + "rotationDegrees": 175.66006175864302, "rotateFast": false }, { - "waypointRelativePos": 1.9, - "rotationDegrees": 120.0, + "waypointRelativePos": 0.6, + "rotationDegrees": 117.19405608697582, "rotateFast": false + }, + { + "waypointRelativePos": 0.95, + "rotationDegrees": 120.0, + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "rev launcher", - "waypointRelativePos": 1.0, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index ff4da61b..a58e8485 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -26,7 +26,8 @@ public ManualAngleCommand( @Override public void execute() { - launcherSubsystem.setAngleManual(launcherAngle.getAsDouble(), powerControl.getAsBoolean(), ignoreLimits.getAsBoolean()); + launcherSubsystem.setAngleManual( + launcherAngle.getAsDouble(), powerControl.getAsBoolean(), ignoreLimits.getAsBoolean()); } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index e9b728c1..9076a691 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -11,7 +11,6 @@ import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.units.BaseUnits; @@ -435,7 +434,10 @@ public void periodic() { launcherAngleOneMotor.disable(); } DriverStation.reportError( - "Launcher encoder angle is insane!!!! Reports angle of " + getAngle() + " degrees. Is overridden: " + ignoreLimits, + "Launcher encoder angle is insane!!!! Reports angle of " + + getAngle() + + " degrees. Is overridden: " + + ignoreLimits, false); } } From 0de22da1f860f74f7b6e59184949ae905e77cd83 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Fri, 5 Apr 2024 14:30:53 -0700 Subject: [PATCH 03/49] launcher data points + auto tweaking + lower translational apriltags standard deviation --- src/main/deploy/launcher_data.csv | 11 +-- .../PresetAmpSide1ScorePassAutoline.auto | 6 ++ .../autos/PresetMidAutoline3Score2.auto | 61 +++++++++++++ .../autos/VisionAmpSide5Score.auto | 2 +- .../autos/VisionAmpSideAutoline5Score.auto | 6 ++ .../autos/VisionAmpSideAutoline6Score.auto | 79 +++++++++++++++++ .../HighNoteLaunch2ToFarHighNoteToLaunch.path | 16 ++-- .../paths/HighSpeakerPassAutoline2.path | 13 ++- .../paths/launchCenterN2LaunchMidNote.path | 87 +++++++++++++++++++ .../launchSecondNoteCenterLineLaunch.path | 12 +-- .../java/frc/team2412/robot/Controls.java | 5 +- .../robot/sensors/AprilTagsProcessor.java | 2 +- .../robot/subsystems/LauncherSubsystem.java | 2 + .../team2412/robot/util/auto/AutoLogic.java | 1 + 14 files changed, 275 insertions(+), 28 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto create mode 100644 src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto create mode 100644 src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data.csv index 345afa40..2c678c09 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data.csv @@ -5,19 +5,20 @@ # These RPMs are target RPMs, actual was much lower # Front subwoofer -1.30, 254, 3500 +1.30, 257, 3500 # Front bumper at autoline -2.13, 234, 3500 +2.13, 236, 3500 # dcmps # On Autoline N2 -2.81, 223, 3600 +2.81, 225, 3600 #chain -3.84, 211, 3600 - +3.84, 213, 3600 +# wing line amp side +5.93, 207, 4000 # bad values? diff --git a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto index 7b1d693d..260b9e9c 100644 --- a/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto +++ b/src/main/deploy/pathplanner/autos/PresetAmpSide1ScorePassAutoline.auto @@ -11,6 +11,12 @@ "type": "sequential", "data": { "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto b/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto new file mode 100644 index 00000000..4269fdc8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PresetMidAutoline3Score2.auto @@ -0,0 +1,61 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.33, + "y": 5.55 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToMidNoteReturn" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "SpeakerToLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "LowNoteToSpeaker" + } + }, + { + "type": "named", + "data": { + "name": "Feed" + } + }, + { + "type": "path", + "data": { + "pathName": "MidSpeakerPassAutoLine" + } + } + ] + } + }, + "folder": "Preset Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto index c19c5ecd..6e2e3634 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSide5Score.auto @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "SpeakerToMidNote" + "pathName": "launchSecondNoteLaunch2" } } ] diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto index 7afeebe1..f23c834d 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto @@ -41,6 +41,12 @@ "name": "VisionLaunch" } }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto new file mode 100644 index 00000000..eff9c4f8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline6Score.auto @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 6.62 + }, + "rotation": -120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighSpeakerToHighNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteCenterLineLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "launchSecondNoteLaunch2" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "MidNoteToBottomNote" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index e0f6ecab..1d176d6a 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -20,24 +20,24 @@ "y": 7.395762539272827 }, "prevControl": { - "x": 8.405377986562273, - "y": 7.426708307431903 + "x": 8.901740880404835, + "y": 7.4608705342766815 }, "nextControl": { - "x": 7.630253917365975, - "y": 7.332753874802049 + "x": 7.628402463912032, + "y": 7.350576110310523 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.0824060026540545, - "y": 7.552922493229885 + "x": 5.413643259718952, + "y": 7.183134366268733 }, "prevControl": { - "x": 4.562069053099387, - "y": 7.653478859199186 + "x": 5.893306310164284, + "y": 7.283690732238034 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path index 91c897e3..c392d2dd 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerPassAutoline2.path @@ -20,12 +20,12 @@ "y": 7.68155593454044 }, "prevControl": { - "x": 1.96668964766501, - "y": 7.7108748503211295 + "x": 1.9554131415955136, + "y": 7.636282850913281 }, "nextControl": { - "x": 3.734277569039197, - "y": 7.657311579976456 + "x": 3.7337899746634324, + "y": 7.718539229435451 }, "isLocked": false, "linkedName": null @@ -54,6 +54,11 @@ "waypointRelativePos": 0.05, "rotationDegrees": -120.0, "rotateFast": false + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": 180.0, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path b/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path new file mode 100644 index 00000000..3943b529 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/launchCenterN2LaunchMidNote.path @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.41, + "y": 6.347545266519105 + }, + "prevControl": null, + "nextControl": { + "x": 4.942163397840309, + "y": 6.1662170171187185 + }, + "isLocked": false, + "linkedName": "LaunchCenterlineN2" + }, + { + "anchor": { + "x": 2.9, + "y": 5.55 + }, + "prevControl": { + "x": 3.6201708439452913, + "y": 5.583800717150984 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": -170.62, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path index c9224c11..08d8d18f 100644 --- a/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchSecondNoteCenterLineLaunch.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.0824060026540545, - "y": 7.552922493229885 + "x": 5.413643259718952, + "y": 7.183134366268733 }, "prevControl": null, "nextControl": { - "x": 6.0634581056947585, - "y": 7.21302167675837 + "x": 7.394695362759656, + "y": 6.843233549797217 }, "isLocked": false, "linkedName": "LaunchCenterlineN1" @@ -46,8 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, - "rotationDegrees": -148.64916364014846, + "waypointRelativePos": 0.65, + "rotationDegrees": -148.65, "rotateFast": false } ], diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 70ed6385..14ca6441 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -30,7 +30,6 @@ import frc.team2412.robot.commands.launcher.SetAngleLaunchCommand; import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; -import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -241,8 +240,8 @@ private void bindLauncherControls() { s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - launcherTrapPresetButton.onTrue( - TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); + // launcherTrapPresetButton.onTrue( + // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); codriveController.b().whileTrue(s.launcherSubsystem.run(s.launcherSubsystem::stopLauncher)); diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index ac3fdc60..c1a17850 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -51,7 +51,7 @@ private static class FilterInfo {} // TODO Measure these private static final Vector STANDARD_DEVS = - VecBuilder.fill(0.7, 0.7, Units.degreesToRadians(30)); + VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(30)); private static final double MAX_POSE_AMBIGUITY = 0.1; diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 9076a691..3d435b09 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -411,6 +411,8 @@ private void initShuffleboard() { launch(setLauncherSpeedEntry.getDouble(SPEAKER_SHOOT_SPEED_RPM)); }) .withName("Full Manual")); + + Shuffleboard.getTab("Launcher").addBoolean("Ignoring Limits", () -> ignoreLimits); } public void updateDistanceEntry(double distance) { diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 3cfb87d4..6de792da 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -113,6 +113,7 @@ public static enum StartPosition { // presets new AutoPath("Autoline N1 Centerline N1", "PresetAmpSideAutoline3Score"), new AutoPath("Autoline N2 N1", "PresetMidAutoline3Score"), + new AutoPath("Autoline N2 N3", "PresetMidAutoline3Score2"), new AutoPath("Centerline N5 N4", "PresetSourceSideCenterline3Score2"), new AutoPath("Centerline N5 N3", "PresetSourceSideCenterline3Score2"), // vision From fa43a8d0e5a5145ead7892a730a86d936a7a500f Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Fri, 5 Apr 2024 16:50:51 -0700 Subject: [PATCH 04/49] launcher data update --- src/main/deploy/launcher_data.csv | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data.csv index 2c678c09..e32b221e 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data.csv @@ -15,11 +15,15 @@ 2.81, 225, 3600 #chain -3.84, 213, 3600 +# 3.84, 213, 3600 # wing line amp side 5.93, 207, 4000 +3.82, 217, 4000 +4.32, 213, 4000 + + # bad values? # Near podium From 0c647ae58b9923d009428629dad04bbeee023a73 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Fri, 5 Apr 2024 17:13:32 -0700 Subject: [PATCH 05/49] encoder sanity check add on (not implemented) --- .../SyncPivotRelativeEncoderCommand.java | 49 +++++++++++++++++++ .../robot/subsystems/LauncherSubsystem.java | 49 +++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java new file mode 100644 index 00000000..76ebc44f --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -0,0 +1,49 @@ +package frc.team2412.robot.commands.launcher; + +import edu.wpi.first.math.filter.MedianFilter; +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.subsystems.LauncherSubsystem; + +public class SyncPivotRelativeEncoderCommand extends Command { + + private int MAX_SAMPLES = 300; // 6 seconds, 50 per second + private final MedianFilter filter; + + private final LauncherSubsystem launcherSubsystem;; + + private int samples; + + public SyncPivotRelativeEncoderCommand( + LauncherSubsystem launcherSubsystem, boolean ignorePrevOffset) { + this.launcherSubsystem = launcherSubsystem; + filter = new MedianFilter(300); + } + + public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { + this(launcherSubsystem, false); + } + + @Override + public void execute() { + + // if the arm is moving, reset the syncing because now inaccurate. + if (Math.abs(launcherSubsystem.getAngleSpeed()) <= 0.1) { + filter.reset(); + samples = 0; + return; + } + + filter.calculate(launcherSubsystem.getAngle()); + samples++; + } + + @Override + public void end(boolean interrupted) { + launcherSubsystem.zeroRelativeEncoder(filter.lastValue()); + } + + @Override + public boolean isFinished() { + return samples > MAX_SAMPLES; + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 3d435b09..b224670e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -29,6 +29,7 @@ import frc.team2412.robot.Robot; import frc.team2412.robot.util.SparkPIDWidget; import java.util.Map; +import java.util.Optional; public class LauncherSubsystem extends SubsystemBase { // CONSTANTS @@ -39,6 +40,12 @@ public class LauncherSubsystem extends SubsystemBase { private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; private static final int PIVOT_OFFSET = 36; + + // offset stuff + private static double COMPARE_PIVOT_MOTOR_OFFSET = 0; + private static final double ENCODER_DIFFERENCE_TOLERANCE = 0.01; + private static final double OFFSET_SYNCING_TOLERANCE = 0.06; + // ANGLE VALUES public static final int AMP_AIM_ANGLE = 285 + PIVOT_OFFSET; public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; @@ -91,6 +98,8 @@ public class LauncherSubsystem extends SubsystemBase { private double manualAngleSetpoint; private boolean ignoreLimits; + private Optional relativeEncoderStartPosition; + private GenericEntry setLauncherSpeedEntry; private GenericEntry setLauncherAngleEntry; @@ -419,6 +428,31 @@ public void updateDistanceEntry(double distance) { speakerDistanceEntry.setDouble(distance); } + public double getAngleOneMotorPosition() { + return (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) + * PIVOT_GEARING_RATIO + - relativeEncoderStartPosition.orElse(0.0); + } + + public void zeroRelativeEncoder(double pivotAngle) { + + double currentRelativePosition = + (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) + * PIVOT_GEARING_RATIO; + double offset = pivotAngle - currentRelativePosition; + + // + if (relativeEncoderStartPosition.isPresent() + || Math.abs(relativeEncoderStartPosition.get() - offset) > OFFSET_SYNCING_TOLERANCE) { + relativeEncoderStartPosition = Optional.of(offset); + } + } + // public void getAngleOneMotorData() { + // double offset = (getAngleOneMotorPosition() % 1) - getAngle(); + // relativeEncoderStartPosition = MedianFilter.calculate(getAngle()); + + // } + @Override public void periodic() { launcherAngleEntry.setDouble(getAngle()); @@ -429,6 +463,21 @@ public void periodic() { angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); + // other sanity check + + if (relativeEncoderStartPosition.isPresent() && Math.abs(launcherAngleEncoder.getPosition() - getAngleOneMotorPosition()) + <= ENCODER_DIFFERENCE_TOLERANCE) { + if (!ignoreLimits) { + launcherAngleOneMotor.disable(); + } + DriverStation.reportError( + "Launcher encoder angle is insane!!!! Reports angle of " + + getAngle() + + " degrees. Is overridden: " + + ignoreLimits, + false); + } + // sanity check the pivot encoder if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET || launcherAngleEncoder.getPosition() <= PIVOT_SOFTSTOP_BACKWARD - PIVOT_DISABLE_OFFSET) { From eaafbe10d92e9ee3f7883b29f14f0b685c903826 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Fri, 5 Apr 2024 17:55:51 -0700 Subject: [PATCH 06/49] implementation (probably not working) --- src/main/deploy/launcher_data.csv | 2 +- src/main/java/frc/team2412/robot/Robot.java | 3 +++ .../SyncPivotRelativeEncoderCommand.java | 3 ++- .../robot/subsystems/LauncherSubsystem.java | 20 ++++++++++++++++--- 4 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data.csv index e32b221e..5c09ec9c 100644 --- a/src/main/deploy/launcher_data.csv +++ b/src/main/deploy/launcher_data.csv @@ -22,7 +22,7 @@ 3.82, 217, 4000 4.32, 213, 4000 - +4.10, 212, 4000 # bad values? diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index df649ad1..ed0ce59f 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -19,6 +19,7 @@ import frc.team2412.robot.Subsystems.SubsystemConstants; import frc.team2412.robot.commands.diagnostic.IntakeDiagnosticCommand; import frc.team2412.robot.commands.diagnostic.LauncherDiagnosticCommand; +import frc.team2412.robot.commands.launcher.SyncPivotRelativeEncoderCommand; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.MatchDashboard; import frc.team2412.robot.util.auto.AutoLogic; @@ -121,6 +122,8 @@ public void robotInit() { dashboard = new MatchDashboard(subsystems); RobotController.setBrownoutVoltage(5.75); + + (new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem)).schedule(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java index 76ebc44f..ab6578cb 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -9,7 +9,8 @@ public class SyncPivotRelativeEncoderCommand extends Command { private int MAX_SAMPLES = 300; // 6 seconds, 50 per second private final MedianFilter filter; - private final LauncherSubsystem launcherSubsystem;; + private final LauncherSubsystem launcherSubsystem; + ; private int samples; diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index b224670e..211b9c56 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -150,6 +150,8 @@ public LauncherSubsystem() { // launcherAngleTwoPIDController = launcherAngleTwoMotor.getPIDController(); // launcherAngleTwoPIDController.setFeedbackDevice(launcherAngleEncoder); + relativeEncoderStartPosition = Optional.empty(); + configMotors(); initShuffleboard(); } @@ -342,6 +344,17 @@ private void initShuffleboard() { .addDouble("Top FlyWheel Temp", () -> launcherTopMotor.getMotorTemperature()); } + Shuffleboard.getTab("Launcher") + .addDouble("relative encoder offset", () -> relativeEncoderStartPosition.orElse(0.0)); + + Shuffleboard.getTab("Launcher.add") + .addDouble( + "relative encoder", + () -> + (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) + * PIVOT_GEARING_RATIO); + Shuffleboard.getTab("Launcher") + .addDouble("relative encoder (w/ offset)", this::getAngleOneMotorPosition); launcherIsAtSpeed = Shuffleboard.getTab("Match") .add("flywheels at target speed", false) @@ -441,7 +454,7 @@ public void zeroRelativeEncoder(double pivotAngle) { * PIVOT_GEARING_RATIO; double offset = pivotAngle - currentRelativePosition; - // + // if (relativeEncoderStartPosition.isPresent() || Math.abs(relativeEncoderStartPosition.get() - offset) > OFFSET_SYNCING_TOLERANCE) { relativeEncoderStartPosition = Optional.of(offset); @@ -465,8 +478,9 @@ public void periodic() { // other sanity check - if (relativeEncoderStartPosition.isPresent() && Math.abs(launcherAngleEncoder.getPosition() - getAngleOneMotorPosition()) - <= ENCODER_DIFFERENCE_TOLERANCE) { + if (relativeEncoderStartPosition.isPresent() + && Math.abs(launcherAngleEncoder.getPosition() - getAngleOneMotorPosition()) + <= ENCODER_DIFFERENCE_TOLERANCE) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); } From 76a5882305fcc1d7be6454df19b10c3fa57387fe Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Fri, 5 Apr 2024 22:01:27 -0700 Subject: [PATCH 07/49] run encoder syncing command needs a ignore disable --- src/main/java/frc/team2412/robot/Robot.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index ed0ce59f..0978e992 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -123,7 +123,8 @@ public void robotInit() { RobotController.setBrownoutVoltage(5.75); - (new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem)).schedule(); + CommandScheduler.getInstance() + .schedule(new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem).ignoringDisable(true)); } @Override From da7afcfe6163e94360b260eeb21f91dafafd0c14 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Fri, 5 Apr 2024 23:36:10 -0700 Subject: [PATCH 08/49] address comments --- src/main/java/frc/team2412/robot/Robot.java | 5 ++- .../SyncPivotRelativeEncoderCommand.java | 16 +++++++--- .../robot/subsystems/LauncherSubsystem.java | 32 +++++++------------ 3 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 0978e992..7442637a 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -124,7 +124,10 @@ public void robotInit() { RobotController.setBrownoutVoltage(5.75); CommandScheduler.getInstance() - .schedule(new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem).ignoringDisable(true)); + .schedule( + new SyncPivotRelativeEncoderCommand(subsystems.launcherSubsystem) + .ignoringDisable(true) + .withName("Sync Relative Encoder")); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java index ab6578cb..d99e7cf2 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -6,7 +6,8 @@ public class SyncPivotRelativeEncoderCommand extends Command { - private int MAX_SAMPLES = 300; // 6 seconds, 50 per second + private int MAX_SAMPLES = 50; // 6 seconds, 50 per second + private final double ANGLE_SPEED_TOLERANCE = 0.1; private final MedianFilter filter; private final LauncherSubsystem launcherSubsystem; @@ -17,7 +18,7 @@ public class SyncPivotRelativeEncoderCommand extends Command { public SyncPivotRelativeEncoderCommand( LauncherSubsystem launcherSubsystem, boolean ignorePrevOffset) { this.launcherSubsystem = launcherSubsystem; - filter = new MedianFilter(300); + filter = new MedianFilter(MAX_SAMPLES); } public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { @@ -27,24 +28,29 @@ public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { @Override public void execute() { - // if the arm is moving, reset the syncing because now inaccurate. - if (Math.abs(launcherSubsystem.getAngleSpeed()) <= 0.1) { + // if the pivot is moving, reset the syncing because now inaccurate. + if (Math.abs(launcherSubsystem.getAngleSpeed()) >= ANGLE_SPEED_TOLERANCE) { filter.reset(); samples = 0; return; } + // add current value to median filter filter.calculate(launcherSubsystem.getAngle()); samples++; } @Override public void end(boolean interrupted) { - launcherSubsystem.zeroRelativeEncoder(filter.lastValue()); + + if (samples > MAX_SAMPLES) { + launcherSubsystem.zeroRelativeEncoder(filter.lastValue()); + } } @Override public boolean isFinished() { + // stop when finished sampling for avg return samples > MAX_SAMPLES; } } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 211b9c56..763c4cdd 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -42,7 +42,6 @@ public class LauncherSubsystem extends SubsystemBase { private static final int PIVOT_OFFSET = 36; // offset stuff - private static double COMPARE_PIVOT_MOTOR_OFFSET = 0; private static final double ENCODER_DIFFERENCE_TOLERANCE = 0.01; private static final double OFFSET_SYNCING_TOLERANCE = 0.06; @@ -350,9 +349,7 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher.add") .addDouble( "relative encoder", - () -> - (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) - * PIVOT_GEARING_RATIO); + () -> launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO); Shuffleboard.getTab("Launcher") .addDouble("relative encoder (w/ offset)", this::getAngleOneMotorPosition); launcherIsAtSpeed = @@ -442,29 +439,22 @@ public void updateDistanceEntry(double distance) { } public double getAngleOneMotorPosition() { - return (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) - * PIVOT_GEARING_RATIO - - relativeEncoderStartPosition.orElse(0.0); + return Units.rotationsToDegrees( + launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO + - relativeEncoderStartPosition.orElse(0.0)); } public void zeroRelativeEncoder(double pivotAngle) { double currentRelativePosition = - (launcherAngleOneMotor.getEncoder().getPosition() + COMPARE_PIVOT_MOTOR_OFFSET) - * PIVOT_GEARING_RATIO; + launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO; double offset = pivotAngle - currentRelativePosition; - // if (relativeEncoderStartPosition.isPresent() - || Math.abs(relativeEncoderStartPosition.get() - offset) > OFFSET_SYNCING_TOLERANCE) { + || Math.abs(relativeEncoderStartPosition.get() + offset) > OFFSET_SYNCING_TOLERANCE) { relativeEncoderStartPosition = Optional.of(offset); } } - // public void getAngleOneMotorData() { - // double offset = (getAngleOneMotorPosition() % 1) - getAngle(); - // relativeEncoderStartPosition = MedianFilter.calculate(getAngle()); - - // } @Override public void periodic() { @@ -478,16 +468,18 @@ public void periodic() { // other sanity check - if (relativeEncoderStartPosition.isPresent() + if (relativeEncoderStartPosition.isEmpty() && Math.abs(launcherAngleEncoder.getPosition() - getAngleOneMotorPosition()) <= ENCODER_DIFFERENCE_TOLERANCE) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); } DriverStation.reportError( - "Launcher encoder angle is insane!!!! Reports angle of " - + getAngle() - + " degrees. Is overridden: " + "Pivot encoder deviated too far from encoder values ..... Reported pivot angle of " + + launcherAngleEncoder + + " and motor angle of " + + getAngleOneMotorPosition() + + ". Is overidden: " + ignoreLimits, false); } From c2852d6c0b8cf0185bb976650712c6897323332e Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Fri, 5 Apr 2024 23:44:21 -0700 Subject: [PATCH 09/49] more comment addressing --- .../robot/subsystems/LauncherSubsystem.java | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 763c4cdd..73ef946a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -349,9 +349,11 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher.add") .addDouble( "relative encoder", - () -> launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO); + () -> + Units.rotationsToDegrees( + launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO)); Shuffleboard.getTab("Launcher") - .addDouble("relative encoder (w/ offset)", this::getAngleOneMotorPosition); + .addDouble("relative encoder (w/ offset)", () -> getAngleOneMotorAngle()); launcherIsAtSpeed = Shuffleboard.getTab("Match") .add("flywheels at target speed", false) @@ -438,7 +440,7 @@ public void updateDistanceEntry(double distance) { speakerDistanceEntry.setDouble(distance); } - public double getAngleOneMotorPosition() { + public double getAngleOneMotorAngle() { return Units.rotationsToDegrees( launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO - relativeEncoderStartPosition.orElse(0.0)); @@ -447,7 +449,8 @@ public double getAngleOneMotorPosition() { public void zeroRelativeEncoder(double pivotAngle) { double currentRelativePosition = - launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO; + Units.rotationsToDegrees( + launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO); double offset = pivotAngle - currentRelativePosition; if (relativeEncoderStartPosition.isPresent() @@ -466,25 +469,24 @@ public void periodic() { angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); - // other sanity check - + // PIVOT ENCODER SANITY CHECKS + // compares the relative encoder angle vs the absolute encoder angle if (relativeEncoderStartPosition.isEmpty() - && Math.abs(launcherAngleEncoder.getPosition() - getAngleOneMotorPosition()) - <= ENCODER_DIFFERENCE_TOLERANCE) { + && Math.abs(getAngle() - getAngleOneMotorAngle()) <= ENCODER_DIFFERENCE_TOLERANCE) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); } DriverStation.reportError( - "Pivot encoder deviated too far from encoder values ..... Reported pivot angle of " + "Pivot encoder deviated too far from motor encoder angle ... .. Reported pivot angle of " + launcherAngleEncoder + " and motor angle of " - + getAngleOneMotorPosition() + + getAngleOneMotorAngle() + ". Is overidden: " + ignoreLimits, false); } - // sanity check the pivot encoder + // if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET || launcherAngleEncoder.getPosition() <= PIVOT_SOFTSTOP_BACKWARD - PIVOT_DISABLE_OFFSET) { if (!ignoreLimits) { From 9009b7b4b8a50c3d8f06c01ad48e4caf8ac33af5 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sat, 6 Apr 2024 00:08:03 -0700 Subject: [PATCH 10/49] auto changes --- .../autos/VisionAmpSideAutoline5Score.auto | 14 +- .../VisionSourceSide4ScoreUnderStage.auto | 67 ++++++++++ ...owLaunchNoteThreeCenterLineUnderStage.path | 126 ++++++++++++++++++ .../paths/MidNoteToCenterMidNodeLaunch.path | 40 +++--- .../paths/MiddleLowNoteToLaunch.path | 11 +- .../launchPosSecondCenterLineNoteLaunch.path | 40 ++---- .../paths/midUnderStageCenterNoteLaunch.path | 34 ++--- 7 files changed, 251 insertions(+), 81 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto create mode 100644 src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto index f23c834d..36b47fef 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideAutoline5Score.auto @@ -50,7 +50,7 @@ { "type": "path", "data": { - "pathName": "launchSecondNoteLaunch" + "pathName": "launchSecondNoteLaunch2" } }, { @@ -59,18 +59,6 @@ "name": "VisionLaunch" } }, - { - "type": "path", - "data": { - "pathName": "MidNoteToBottomNote" - } - }, - { - "type": "path", - "data": { - "pathName": "BottomNoteLaunch" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto b/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto new file mode 100644 index 00000000..f6a052ba --- /dev/null +++ b/src/main/deploy/pathplanner/autos/VisionSourceSide4ScoreUnderStage.auto @@ -0,0 +1,67 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.73, + "y": 4.47 + }, + "rotation": 120.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SubwooferLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowSpeakerToMiddleLowNote" + } + }, + { + "type": "path", + "data": { + "pathName": "MiddleLowNoteToLaunch" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteFourCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + }, + { + "type": "path", + "data": { + "pathName": "LowLaunchNoteThreeCenterLineUnderStage" + } + }, + { + "type": "named", + "data": { + "name": "VisionLaunch" + } + } + ] + } + }, + "folder": "Vision Autos", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path new file mode 100644 index 00000000..1734d8c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteThreeCenterLineUnderStage.path @@ -0,0 +1,126 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": null, + "nextControl": { + "x": 5.260103863424924, + "y": 4.362135811002534 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.349955083898307, + "y": 4.1046482092964185 + }, + "prevControl": { + "x": 7.631641521812732, + "y": 4.1046482092964185 + }, + "nextControl": { + "x": 8.44804559883397, + "y": 4.1046482092964185 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.9896948365325304, + "y": 5.321383214195 + }, + "prevControl": { + "x": 5.266234520608404, + "y": 4.165954781131208 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.0, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "intake", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "rev launcher", + "waypointRelativePos": 1.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RevLauncher" + } + } + ] + } + } + }, + { + "name": "retract", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RetractPivot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 176.97, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 176.97, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index 40c7b046..2fabe7ae 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 3.9067566423421134, - "y": 5.445130169539619 + "x": 3.907019136132327, + "y": 5.033384050796496 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.059835262527669, - "y": 4.265902246090207 + "x": 5.589292620026682, + "y": 4.487068849914097 }, "prevControl": { - "x": 4.106065843242836, - "y": 4.566441586006087 + "x": 4.615676309458881, + "y": 4.715260172703426 }, "nextControl": { - "x": 6.137894028707098, - "y": 3.926198473430942 + "x": 6.5216705628659755, + "y": 4.2685427695611375 }, "isLocked": false, "linkedName": null @@ -36,28 +36,28 @@ "y": 4.104648209296419 }, "prevControl": { - "x": 8.202270809898764, - "y": 4.4595947557966555 + "x": 8.110859453742629, + "y": 4.264045296066871 }, "nextControl": { - "x": 8.397729190101238, - "y": 3.749701662796183 + "x": 8.5815171861562, + "y": 3.867401250687913 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.589292620026682, - "y": 4.265902246090207 + "x": 5.799601695571071, + "y": 4.45 }, "prevControl": { - "x": 6.35984666440237, - "y": 3.9957190898956214 + "x": 6.602796124886758, + "y": 4.302924570674619 }, "nextControl": { - "x": 4.757269334358488, - "y": 4.55763866482688 + "x": 5.008746918902289, + "y": 4.59481587716151 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 5.78 }, "prevControl": { - "x": 4.4337047232653015, - "y": 5.001384417183249 + "x": 4.628646173526595, + "y": 5.067161387102611 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path index e9d716d7..ac23a341 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLaunch.path @@ -30,9 +30,14 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.35, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 0.75, + "rotationDegrees": 147.9081562884914, + "rotateFast": true } ], "constraintZones": [], @@ -56,7 +61,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -72,5 +77,5 @@ "rotation": 180.0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path index 594f0ab9..d9eb246c 100644 --- a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path @@ -8,40 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 4.572375270876667, - "y": 6.35111108060054 + "x": 5.885430896139777, + "y": 6.6979161979080075 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.02379366920896, - "y": 6.434313409167359 - }, - "prevControl": { - "x": 5.422887962893042, - "y": 6.434313409167359 - }, - "nextControl": { - "x": 6.67043999635063, - "y": 6.434313409167359 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.270256540513081, - "y": 5.78 + "x": 8.399000341366138, + "y": 5.67 }, "prevControl": { - "x": 8.239036732334611, - "y": 6.2371314046481 + "x": 8.121465209600638, + "y": 6.028469660415459 }, "nextControl": { - "x": 8.299575456293768, - "y": 5.350702129975688 + "x": 8.595181371237466, + "y": 5.41660884656091 }, "isLocked": false, "linkedName": null @@ -62,8 +46,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.7, - "rotationDegrees": -132.10466146576135, + "waypointRelativePos": 0.85, + "rotationDegrees": 163.154898401479, "rotateFast": false } ], @@ -71,7 +55,7 @@ "eventMarkers": [ { "name": "intake", - "waypointRelativePos": 1.2000000000000002, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -88,7 +72,7 @@ }, { "name": "rev launcher", - "waypointRelativePos": 2.55, + "waypointRelativePos": 1.5499999999999998, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path b/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path index f4f6c87c..e060d9ad 100644 --- a/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/midUnderStageCenterNoteLaunch.path @@ -20,28 +20,28 @@ "y": 4.890447979094157 }, "prevControl": { - "x": 2.3609477505216754, - "y": 4.877599864575197 + "x": 2.360805106514846, + "y": 4.897019030363906 }, "nextControl": { - "x": 4.017693080431205, - "y": 4.927426791790521 + "x": 3.9143033388107593, + "y": 4.873131591870992 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.886695178795763, + "x": 5.334722861104995, "y": 4.317276382300513 }, "prevControl": { - "x": 3.9622248613866597, - "y": 5.121565558446433 + "x": 4.4485644760238685, + "y": 4.49361633451813 }, "nextControl": { - "x": 5.326316485345794, - "y": 3.934805845601985 + "x": 6.1287009530540795, + "y": 4.159279729384658 }, "isLocked": false, "linkedName": null @@ -64,16 +64,16 @@ }, { "anchor": { - "x": 5.136302164496221, - "y": 4.317276382300513 + "x": 5.334722861104995, + "y": 4.450647836521938 }, "prevControl": { - "x": 5.727963167638047, - "y": 3.9752223648591443 + "x": 5.80717681627009, + "y": 4.269540487041985 }, "nextControl": { - "x": 4.5376988828435225, - "y": 4.663343904505979 + "x": 4.897670700399076, + "y": 4.618184498125873 }, "isLocked": false, "linkedName": null @@ -84,8 +84,8 @@ "y": 5.775826408795675 }, "prevControl": { - "x": 4.370800467689787, - "y": 5.2187723096530965 + "x": 4.577165782548069, + "y": 5.215489117757295 }, "nextControl": null, "isLocked": false, From 0213bef68025aca85bd9dc6318774d7d86531231 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Sat, 6 Apr 2024 10:58:05 -0700 Subject: [PATCH 11/49] lower pivot output --- .../launcher/SyncPivotRelativeEncoderCommand.java | 2 +- .../robot/subsystems/LauncherSubsystem.java | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java index d99e7cf2..9b7a39ac 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -11,7 +11,6 @@ public class SyncPivotRelativeEncoderCommand extends Command { private final MedianFilter filter; private final LauncherSubsystem launcherSubsystem; - ; private int samples; @@ -19,6 +18,7 @@ public SyncPivotRelativeEncoderCommand( LauncherSubsystem launcherSubsystem, boolean ignorePrevOffset) { this.launcherSubsystem = launcherSubsystem; filter = new MedianFilter(MAX_SAMPLES); + samples = 0; } public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 73ef946a..55f8b6b4 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -64,7 +64,7 @@ public class LauncherSubsystem extends SubsystemBase { public static final int SPEAKER_SHOOT_SPEED_RPM = 4500; public static final int TRAP_SHOOT_SPEED_RPM = 4500; public static final int LOBBING_RPM = 4700; - public static final double ANGLE_MAX_SPEED = 1; + public static final double ANGLE_MAX_SPEED = 0.5; // percent output public static final double MAX_SET_ANGLE_OFFSET = 20; // 3392 RPM = 50% Speed // 1356 RPM = 20% Speed @@ -297,7 +297,7 @@ public void restoreLimits() { public void setAngleManual(double joystickInput, boolean powerControl, boolean ignoreLimits) { this.ignoreLimits = ignoreLimits; if (powerControl || ignoreLimits) { - launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.1 : joystickInput); + launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); manualAngleSetpoint = MathUtil.clamp( Units.degreesToRotations(getAngle()), @@ -353,7 +353,7 @@ private void initShuffleboard() { Units.rotationsToDegrees( launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO)); Shuffleboard.getTab("Launcher") - .addDouble("relative encoder (w/ offset)", () -> getAngleOneMotorAngle()); + .addDouble("relative encoder (with offset)", () -> getAngleOneMotorAngle()); launcherIsAtSpeed = Shuffleboard.getTab("Match") .add("flywheels at target speed", false) @@ -442,8 +442,8 @@ public void updateDistanceEntry(double distance) { public double getAngleOneMotorAngle() { return Units.rotationsToDegrees( - launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO - - relativeEncoderStartPosition.orElse(0.0)); + launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO) + + relativeEncoderStartPosition.orElse(0.0); } public void zeroRelativeEncoder(double pivotAngle) { @@ -453,8 +453,8 @@ public void zeroRelativeEncoder(double pivotAngle) { launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO); double offset = pivotAngle - currentRelativePosition; - if (relativeEncoderStartPosition.isPresent() - || Math.abs(relativeEncoderStartPosition.get() + offset) > OFFSET_SYNCING_TOLERANCE) { + if (relativeEncoderStartPosition.isEmpty() + || Math.abs(relativeEncoderStartPosition.orElse(0.0) + offset) > OFFSET_SYNCING_TOLERANCE) { relativeEncoderStartPosition = Optional.of(offset); } } From 2fa700f47970efbd19d2c5270a4aa4b6d786694b Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Sat, 6 Apr 2024 13:02:52 -0700 Subject: [PATCH 12/49] yohohoho merry christmas filter works --- .../robot/subsystems/LauncherSubsystem.java | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 55f8b6b4..179bb256 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -64,8 +64,8 @@ public class LauncherSubsystem extends SubsystemBase { public static final int SPEAKER_SHOOT_SPEED_RPM = 4500; public static final int TRAP_SHOOT_SPEED_RPM = 4500; public static final int LOBBING_RPM = 4700; - public static final double ANGLE_MAX_SPEED = 0.5; // percent output - public static final double MAX_SET_ANGLE_OFFSET = 20; + public static final double ANGLE_MAX_SPEED = 0.3; // percent output + public static final double MAX_SET_ANGLE_OFFSET = 15; // 3392 RPM = 50% Speed // 1356 RPM = 20% Speed // 1017 RPM = 15% Speed @@ -204,6 +204,9 @@ public void configMotors() { launcherAngleTwoMotor.getEncoder().setPosition(launcherAngleEncoder.getPosition()); launcherAngleTwoMotor.getEncoder().setPositionConversionFactor(PIVOT_GEARING_RATIO); + // launcherAngleOneMotor.setIdleMode(IdleMode.kCoast); + // launcherAngleTwoMotor.setIdleMode(IdleMode.kCoast); + launcherTopMotor.burnFlash(); launcherBottomMotor.burnFlash(); launcherAngleOneMotor.burnFlash(); @@ -343,17 +346,22 @@ private void initShuffleboard() { .addDouble("Top FlyWheel Temp", () -> launcherTopMotor.getMotorTemperature()); } + Shuffleboard.getTab("Launcher") + .addDouble( + "relative encoder", () -> launcherAngleOneMotor.getEncoder().getPosition() * 360); Shuffleboard.getTab("Launcher") .addDouble("relative encoder offset", () -> relativeEncoderStartPosition.orElse(0.0)); Shuffleboard.getTab("Launcher.add") .addDouble( "relative encoder", - () -> - Units.rotationsToDegrees( - launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO)); + () -> Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition())); Shuffleboard.getTab("Launcher") - .addDouble("relative encoder (with offset)", () -> getAngleOneMotorAngle()); + .addDouble( + "relative encoder (with offset)", + () -> + (launcherAngleOneMotor.getEncoder().getPosition() * 360 + + relativeEncoderStartPosition.orElse(0.0))); launcherIsAtSpeed = Shuffleboard.getTab("Match") .add("flywheels at target speed", false) @@ -441,16 +449,14 @@ public void updateDistanceEntry(double distance) { } public double getAngleOneMotorAngle() { - return Units.rotationsToDegrees( - launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO) + return Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition()) + relativeEncoderStartPosition.orElse(0.0); } public void zeroRelativeEncoder(double pivotAngle) { double currentRelativePosition = - Units.rotationsToDegrees( - launcherAngleOneMotor.getEncoder().getPosition() * PIVOT_GEARING_RATIO); + Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition()); double offset = pivotAngle - currentRelativePosition; if (relativeEncoderStartPosition.isEmpty() @@ -471,7 +477,7 @@ public void periodic() { // PIVOT ENCODER SANITY CHECKS // compares the relative encoder angle vs the absolute encoder angle - if (relativeEncoderStartPosition.isEmpty() + if (relativeEncoderStartPosition.isPresent() && Math.abs(getAngle() - getAngleOneMotorAngle()) <= ENCODER_DIFFERENCE_TOLERANCE) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); From d5c97db8f62630f14a91b269e392a89b34a99ff4 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sun, 7 Apr 2024 14:38:50 -0700 Subject: [PATCH 13/49] auto feeder in command is bad --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 6de792da..bae12a88 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -479,7 +479,7 @@ public static Command feederIn() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) .andThen(Commands.waitSeconds(FEEDER_DELAY)) - .andThen(new FeederInCommand(s.intakeSubsystem)) + .andThen(new FeederInCommand(s.intakeSubsystem).until(untilNoNote()) .andThen(Commands.waitSeconds(0.1)) : Commands.none()) .withName("Auto - FeedCommand"); From 86e180bae7fea5560f38f7af808afb583077ed2f Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Sun, 7 Apr 2024 18:18:55 -0700 Subject: [PATCH 14/49] Update driver b button launch to 6500 --- src/main/java/frc/team2412/robot/Controls.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 14ca6441..f905bfa5 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -251,7 +251,7 @@ private void bindLauncherControls() { // s.launcherSubsystem.runEnd( // s.launcherSubsystem::launch, s.launcherSubsystem::stopLauncher)); - driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(4000))); + driveController.b().onTrue(new InstantCommand(() -> s.launcherSubsystem.launch(6500))); } private void bindSysIdControls() { From daaa9c37007de9c9ec700441e588243b40f5c2e5 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Sun, 7 Apr 2024 21:08:07 -0700 Subject: [PATCH 15/49] Increase drive motor current limits --- src/main/deploy/swerve/modules/physicalproperties.json | 2 +- src/main/java/swervelib/motors/TalonFXSwerve.java | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index d2bb71ac..ff4a1f2d 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,7 +2,7 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 60, + "drive": 140, "angle": 24 }, "conversionFactor": { diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 113eaaa7..0b50d11b 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -408,7 +408,9 @@ public void setCurrentLimit(int currentLimit) { cfg.refresh(configuration.CurrentLimits); cfg.apply( configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) - .withStatorCurrentLimitEnable(true)); + .withStatorCurrentLimitEnable(true) + .withSupplyCurrentLimit(80) + .withSupplyCurrentLimitEnable(true)); } /** From 653d3b3119f384ad2792a613690a1bb76e087c27 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Sun, 7 Apr 2024 21:58:28 -0700 Subject: [PATCH 16/49] auto adjustments --- .../pathplanner/paths/BottomNoteLaunch.path | 8 ++-- .../paths/LaunchHighNoteCenterSteal.path | 41 ++++++++++++++----- .../pathplanner/paths/LowLaunchToMidNote.path | 8 ++-- 3 files changed, 39 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path index 8e484700..839185f3 100644 --- a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6, - "y": 4.79 + "x": 2.551735285288752, + "y": 4.207264414528829 }, "prevControl": { - "x": 2.5, - "y": 4.79 + "x": 2.451735285288752, + "y": 4.207264414528829 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path b/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path index 5ae1d3d7..f12d1959 100644 --- a/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/LaunchHighNoteCenterSteal.path @@ -48,28 +48,28 @@ }, { "anchor": { - "x": 6.726391110439878, - "y": 3.4297848775877737 + "x": 6.525781051562102, + "y": 3.1158011406931907 }, "prevControl": { - "x": 7.484456770715343, - "y": 2.838123874445947 + "x": 7.057519569584594, + "y": 2.378830212205879 }, "nextControl": { - "x": 5.7597547108329605, - "y": 4.1842327992321975 + "x": 5.878621542262904, + "y": 4.012741513230675 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.405970613743029, - "y": 5.306459621928253 + "x": 4.212252060867758, + "y": 5.158796499411181 }, "prevControl": { - "x": 4.858961069273489, - "y": 3.9197541458145984 + "x": 5.2664003509825195, + "y": 4.3751818412727745 }, "nextControl": null, "isLocked": false, @@ -86,6 +86,16 @@ "waypointRelativePos": 1.2, "rotationDegrees": 169.04986019290004, "rotateFast": false + }, + { + "waypointRelativePos": 2.1, + "rotationDegrees": 173.2949618109403, + "rotateFast": false + }, + { + "waypointRelativePos": 2.7, + "rotationDegrees": 127.35178648180019, + "rotateFast": false } ], "constraintZones": [ @@ -99,6 +109,17 @@ "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 } + }, + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 2.3, + "maxWaypointRelativePos": 4.0, + "constraints": { + "maxVelocity": 4.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } } ], "eventMarkers": [ diff --git a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path index f354569c..5da87bc2 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.6, - "y": 4.79 + "x": 2.551735285288752, + "y": 4.207264414528829 }, "prevControl": null, "nextControl": { - "x": 2.770693659609494, - "y": 5.1295050435105765 + "x": 2.722428944898246, + "y": 4.546769458039406 }, "isLocked": false, "linkedName": "PodiumLaunch" From d21e1533b907859306eef0696954c4f52cb7e4e6 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 8 Apr 2024 14:04:17 -0700 Subject: [PATCH 17/49] debug a --- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index bae12a88..6b0e6ac2 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -479,8 +479,10 @@ public static Command feederIn() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) .andThen(Commands.waitSeconds(FEEDER_DELAY)) - .andThen(new FeederInCommand(s.intakeSubsystem).until(untilNoNote()) - .andThen(Commands.waitSeconds(0.1)) + .andThen( + new FeederInCommand(s.intakeSubsystem) + .until(untilFeederHasNoNote()) + .andThen(Commands.waitSeconds(0.1))) : Commands.none()) .withName("Auto - FeedCommand"); } From 717a8e3b1079dda8d0c383130c7d6b40ce3e0980 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 8 Apr 2024 14:08:38 -0700 Subject: [PATCH 18/49] address pr comments --- .../commands/launcher/SyncPivotRelativeEncoderCommand.java | 7 +------ .../frc/team2412/robot/subsystems/LauncherSubsystem.java | 4 ---- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java index 9b7a39ac..d14669bb 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -14,17 +14,12 @@ public class SyncPivotRelativeEncoderCommand extends Command { private int samples; - public SyncPivotRelativeEncoderCommand( - LauncherSubsystem launcherSubsystem, boolean ignorePrevOffset) { + public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { this.launcherSubsystem = launcherSubsystem; filter = new MedianFilter(MAX_SAMPLES); samples = 0; } - public SyncPivotRelativeEncoderCommand(LauncherSubsystem launcherSubsystem) { - this(launcherSubsystem, false); - } - @Override public void execute() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 179bb256..dd77016a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -352,10 +352,6 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher") .addDouble("relative encoder offset", () -> relativeEncoderStartPosition.orElse(0.0)); - Shuffleboard.getTab("Launcher.add") - .addDouble( - "relative encoder", - () -> Units.rotationsToDegrees(launcherAngleOneMotor.getEncoder().getPosition())); Shuffleboard.getTab("Launcher") .addDouble( "relative encoder (with offset)", From 6fcafe59fe716965110cca925435db59e47a3a9c Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 14:31:29 -0700 Subject: [PATCH 19/49] Sysid work --- .../swerve/modules/physicalproperties.json | 2 +- src/main/java/frc/team2412/robot/Controls.java | 9 +++++---- .../robot/subsystems/DrivebaseSubsystem.java | 17 +++++++++++++++++ .../frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 4 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index ff4a1f2d..f7cc5d79 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,7 +2,7 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 140, + "drive": 120, "angle": 24 }, "conversionFactor": { diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index f905bfa5..d11d2ba0 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -270,10 +270,11 @@ private void bindSysIdControls() { // .rightTrigger() // .whileTrue(s.launcherSubsystem.flywheelSysIdDynamic(Direction.kReverse)); // switch these between angle and drive tests in code when tuning - driveController.x().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kForward)); - driveController.y().whileTrue(s.drivebaseSubsystem.angleSysIdQuasistatic(Direction.kReverse)); - driveController.a().whileTrue(s.drivebaseSubsystem.angleSysIdDynamic(Direction.kForward)); - driveController.b().whileTrue(s.drivebaseSubsystem.angleSysIdDynamic(Direction.kReverse)); + driveController.x().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kForward)); + driveController.y().whileTrue(s.drivebaseSubsystem.driveSysIdQuasistatic(Direction.kReverse)); + driveController.a().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kForward)); + driveController.b().whileTrue(s.drivebaseSubsystem.driveSysIdDynamic(Direction.kReverse)); + driveController.back().whileTrue(s.drivebaseSubsystem.debugDriveFullPower()); } public void vibrateDriveController(double vibration) { diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 3e6fc02a..a972f78c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -437,6 +437,23 @@ private SysIdRoutine getAngleSysIdRoutine() { this)); } + public Command debugDriveFullPower() { + return this.runEnd( + () -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getDriveMotor().set(1.0); + module.setAngle(0); + } + }, + () -> { + for (SwerveModule module : swerveDrive.getModules()) { + module.getDriveMotor().set(0.0); + module.setAngle(0); + } + }) + .withName("DriveFullPower"); + } + public Command driveSysIdQuasistatic(SysIdRoutine.Direction direction) { return getDriveSysIdRoutine().quasistatic(direction); } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index bae12a88..6de792da 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -479,7 +479,7 @@ public static Command feederIn() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) .andThen(Commands.waitSeconds(FEEDER_DELAY)) - .andThen(new FeederInCommand(s.intakeSubsystem).until(untilNoNote()) + .andThen(new FeederInCommand(s.intakeSubsystem)) .andThen(Commands.waitSeconds(0.1)) : Commands.none()) .withName("Auto - FeedCommand"); From 5a86db5f236426ba6ee55d17272725f1452b9793 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 15:09:14 -0700 Subject: [PATCH 20/49] tune drivebase with higher current limits --- src/main/deploy/swerve/modules/backleft.json | 8 ++++---- src/main/deploy/swerve/modules/backright.json | 8 ++++---- src/main/deploy/swerve/modules/frontleft.json | 8 ++++---- src/main/deploy/swerve/modules/frontright.json | 8 ++++---- src/main/deploy/swerve/modules/physicalproperties.json | 2 +- .../frc/team2412/robot/subsystems/DrivebaseSubsystem.java | 2 +- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/swerve/modules/backleft.json b/src/main/deploy/swerve/modules/backleft.json index c2735a21..1d0d6978 100644 --- a/src/main/deploy/swerve/modules/backleft.json +++ b/src/main/deploy/swerve/modules/backleft.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.0036753, + "p": 0.61249, "i": 0, "d": 0, - "kS": 0.18532, - "kV": 1.6909, - "kA": 0.28447 + "kS": 0.30249, + "kV": 1.7619, + "kA": 0.058255 }, "angleTuning": { "p": 68.706, diff --git a/src/main/deploy/swerve/modules/backright.json b/src/main/deploy/swerve/modules/backright.json index 37bf4195..cb7fec7b 100644 --- a/src/main/deploy/swerve/modules/backright.json +++ b/src/main/deploy/swerve/modules/backright.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.055241, + "p": 1.0788, "i": 0, "d": 0, - "kS": 0.15508, - "kV": 1.7507, - "kA": 0.26534 + "kS": 0.14653, + "kV": 1.7027, + "kA": 0.28842 }, "angleTuning": { "p": 68.04, diff --git a/src/main/deploy/swerve/modules/frontleft.json b/src/main/deploy/swerve/modules/frontleft.json index e83987de..8cf926a3 100644 --- a/src/main/deploy/swerve/modules/frontleft.json +++ b/src/main/deploy/swerve/modules/frontleft.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.087084, + "p": 0.080223, "i": 0, "d": 0, - "kS": 0.026162, - "kV": 1.8747, - "kA": 0.39611 + "kS": 0.20631, + "kV": 1.8319, + "kA": 0.19741 }, "angleTuning": { "p": 69.289, diff --git a/src/main/deploy/swerve/modules/frontright.json b/src/main/deploy/swerve/modules/frontright.json index fff7a39b..33a1b835 100644 --- a/src/main/deploy/swerve/modules/frontright.json +++ b/src/main/deploy/swerve/modules/frontright.json @@ -24,12 +24,12 @@ "angle": false }, "driveTuning": { - "p": 0.76249, + "p": 0.00097084, "i": 0, "d": 0, - "kS": 0.12121, - "kV": 1.7955, - "kA": 0.33215 + "kS": 0.2071, + "kV": 1.8453, + "kA": 0.13646 }, "angleTuning": { "p": 67.31, diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index ff4a1f2d..f7cc5d79 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -2,7 +2,7 @@ "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, "currentLimit": { - "drive": 140, + "drive": 120, "angle": 24 }, "conversionFactor": { diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 3e6fc02a..2a6e8f2e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -49,7 +49,7 @@ public class DrivebaseSubsystem extends SubsystemBase { Robot.getInstance().getRobotType() == RobotType.BONK ? 3.0 : Robot.getInstance().getRobotType() == RobotType.COMPETITION - ? 4.7 + ? 6.0 : Robot.getInstance().getRobotType() == RobotType.CRANE ? 3.0 : 1.0; // Auto align stuff, dw abt it From f5f78acf04cc0fe677ecb510baa598f5d83c38f4 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 8 Apr 2024 15:25:05 -0700 Subject: [PATCH 21/49] add logging --- .../commands/launcher/SyncPivotRelativeEncoderCommand.java | 2 +- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java index d14669bb..8b965412 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SyncPivotRelativeEncoderCommand.java @@ -6,7 +6,7 @@ public class SyncPivotRelativeEncoderCommand extends Command { - private int MAX_SAMPLES = 50; // 6 seconds, 50 per second + private int MAX_SAMPLES = 50; // 50 samples/second private final double ANGLE_SPEED_TOLERANCE = 0.1; private final MedianFilter filter; diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index dd77016a..d6ed9b59 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -438,6 +438,9 @@ private void initShuffleboard() { .withName("Full Manual")); Shuffleboard.getTab("Launcher").addBoolean("Ignoring Limits", () -> ignoreLimits); + + Shuffleboard.getTab("Launcher").addBoolean("Angle Insane", () -> (relativeEncoderStartPosition.isPresent() + && Math.abs(getAngle() - getAngleOneMotorAngle()) <= ENCODER_DIFFERENCE_TOLERANCE)); } public void updateDistanceEntry(double distance) { From 6990ee92515b3a2da7773839ebc087b584ce83d3 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 15:29:02 -0700 Subject: [PATCH 22/49] modify yagsl json to allow stator and supply setting --- .../bonkswerve/modules/physicalproperties.json | 2 +- .../bonkswerve/modules/pidfproperties.json | 4 ++-- .../craneswerve/modules/physicalproperties.json | 2 +- .../craneswerve/modules/pidfproperties.json | 4 ++-- .../swerve/modules/physicalproperties.json | 8 ++++++-- src/main/java/swervelib/SwerveModule.java | 4 ++-- .../java/swervelib/motors/SparkFlexSwerve.java | 7 ++++--- .../motors/SparkMaxBrushedMotorSwerve.java | 7 ++++--- .../java/swervelib/motors/SparkMaxSwerve.java | 7 ++++--- src/main/java/swervelib/motors/SwerveMotor.java | 5 +++-- .../java/swervelib/motors/TalonFXSwerve.java | 10 ++++++---- .../java/swervelib/motors/TalonSRXSwerve.java | 9 +++++---- .../SwerveModulePhysicalCharacteristics.java | 17 +++++++++++------ .../parser/json/PhysicalPropertiesJson.java | 9 ++++++--- 14 files changed, 57 insertions(+), 38 deletions(-) diff --git a/src/main/deploy/bonkswerve/modules/physicalproperties.json b/src/main/deploy/bonkswerve/modules/physicalproperties.json index a4e443a1..c3f4500f 100644 --- a/src/main/deploy/bonkswerve/modules/physicalproperties.json +++ b/src/main/deploy/bonkswerve/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { + "statorLimit": { "drive": 40, "angle": 20 }, diff --git a/src/main/deploy/bonkswerve/modules/pidfproperties.json b/src/main/deploy/bonkswerve/modules/pidfproperties.json index 4d615439..42df985c 100644 --- a/src/main/deploy/bonkswerve/modules/pidfproperties.json +++ b/src/main/deploy/bonkswerve/modules/pidfproperties.json @@ -3,14 +3,14 @@ "p": 20.0, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 }, "angle": { "p": 35.0, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 } } \ No newline at end of file diff --git a/src/main/deploy/craneswerve/modules/physicalproperties.json b/src/main/deploy/craneswerve/modules/physicalproperties.json index 9be10f3d..d4f63e09 100644 --- a/src/main/deploy/craneswerve/modules/physicalproperties.json +++ b/src/main/deploy/craneswerve/modules/physicalproperties.json @@ -1,7 +1,7 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { + "statorLimit": { "drive": 40, "angle": 20 }, diff --git a/src/main/deploy/craneswerve/modules/pidfproperties.json b/src/main/deploy/craneswerve/modules/pidfproperties.json index 48abf219..0bbe6b00 100644 --- a/src/main/deploy/craneswerve/modules/pidfproperties.json +++ b/src/main/deploy/craneswerve/modules/pidfproperties.json @@ -3,14 +3,14 @@ "p": 0.1, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 }, "angle": { "p": 0.01, "i": 0, "d": 0, - "f": 0, + "kV": 0, "iz": 0 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index f7cc5d79..8b88ec01 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -1,9 +1,13 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, - "currentLimit": { + "statorLimit": { "drive": 120, - "angle": 24 + "angle": 50 + }, + "supplyLimit": { + "drive": 80, + "angle": 30 }, "conversionFactor": { "angle": 19.2, diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 7d5004a5..1d9d61c7 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -78,8 +78,8 @@ public SwerveModule( // Configure voltage comp, current limit, and ramp rate. angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit); - driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit); + angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorStatorLimit, configuration.physicalCharacteristics.angleMotorSupplyLimit); + driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorStatorLimit, configuration.physicalCharacteristics.driveMotorSupplyLimit); angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 741c7aae..13121657 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -110,11 +110,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkFlex(() -> motor.setSmartCurrentLimit(statorLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 2582134d..0ba1750d 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -157,11 +157,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(statorLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index d52b34bd..496590c1 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -91,11 +91,12 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit)); + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configureSparkMax(() -> motor.setSmartCurrentLimit(statorLimit)); } /** diff --git a/src/main/java/swervelib/motors/SwerveMotor.java b/src/main/java/swervelib/motors/SwerveMotor.java index 3e19d4da..4d2505e5 100644 --- a/src/main/java/swervelib/motors/SwerveMotor.java +++ b/src/main/java/swervelib/motors/SwerveMotor.java @@ -147,9 +147,10 @@ public abstract class SwerveMotor { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit; */ - public abstract void setCurrentLimit(int currentLimit); + public abstract void setCurrentLimit(int statorLimit, int supplyLimit); /** * Set the maximum rate the open/closed loop output can change by. diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 0b50d11b..6337b069 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -400,17 +400,19 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { + public void setCurrentLimit(int statorLimit, int supplyLimit) { TalonFXConfigurator cfg = motor.getConfigurator(); cfg.refresh(configuration.CurrentLimits); cfg.apply( - configuration.CurrentLimits.withStatorCurrentLimit(currentLimit) + configuration.CurrentLimits.withStatorCurrentLimit(statorLimit) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(80) + .withSupplyCurrentLimit(supplyLimit) .withSupplyCurrentLimitEnable(true)); + System.out.println("Configured drivebase motor " + motor.getDeviceID() + " with stator limit of " + statorLimit + " amps and supply limit of " + supplyLimit + " amps."); } /** diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 060680b9..36c6320e 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -360,12 +360,13 @@ public void setVoltageCompensation(double nominalVoltage) { * Set the current limit for the swerve drive motor, remember this may cause jumping if used in * conjunction with voltage compensation. This is useful to protect the motor from current spikes. * - * @param currentLimit Current limit in AMPS at free speed. + * @param statorLimit Current limit in AMPS at free speed. + * @param supplyLimit */ @Override - public void setCurrentLimit(int currentLimit) { - configuration.continuousCurrentLimit = currentLimit; - configuration.peakCurrentLimit = currentLimit; + public void setCurrentLimit(int statorLimit, int supplyLimit) { + configuration.continuousCurrentLimit = statorLimit; + configuration.peakCurrentLimit = statorLimit; configChanged = true; } diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 11439164..11d0d749 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -6,7 +6,8 @@ public class SwerveModulePhysicalCharacteristics { /** Current limits for the Swerve Module. */ - public final int driveMotorCurrentLimit, angleMotorCurrentLimit; + public final int driveMotorStatorLimit, angleMotorStatorLimit; + public final int driveMotorSupplyLimit, angleMotorSupplyLimit; /** The time it takes for the motor to go from 0 to full throttle in seconds. */ public final double driveMotorRampRate, angleMotorRampRate; /** Wheel grip tape coefficient of friction on carpet, as described by the vendor. */ @@ -40,8 +41,10 @@ public SwerveModulePhysicalCharacteristics( MotorConfigDouble conversionFactors, double wheelGripCoefficientOfFriction, double optimalVoltage, - int driveMotorCurrentLimit, - int angleMotorCurrentLimit, + int driveMotorStatorLimit, + int angleMotorStatorLimit, + int driveMotorSupplyLimit, + int angleMotorSupplyLimit, double driveMotorRampRate, double angleMotorRampRate) { this.wheelGripCoefficientOfFriction = wheelGripCoefficientOfFriction; @@ -55,8 +58,10 @@ public SwerveModulePhysicalCharacteristics( } } - this.driveMotorCurrentLimit = driveMotorCurrentLimit; - this.angleMotorCurrentLimit = angleMotorCurrentLimit; + this.driveMotorStatorLimit = driveMotorStatorLimit; + this.angleMotorStatorLimit = angleMotorStatorLimit; + this.driveMotorSupplyLimit = driveMotorSupplyLimit; + this.angleMotorSupplyLimit = angleMotorSupplyLimit; this.driveMotorRampRate = driveMotorRampRate; this.angleMotorRampRate = angleMotorRampRate; } @@ -76,6 +81,6 @@ public SwerveModulePhysicalCharacteristics( */ public SwerveModulePhysicalCharacteristics( MotorConfigDouble conversionFactors, double driveMotorRampRate, double angleMotorRampRate) { - this(conversionFactors, 1.19, 12, 40, 20, driveMotorRampRate, angleMotorRampRate); + this(conversionFactors, 1.19, 12, 40, 20, 40, 20, driveMotorRampRate, angleMotorRampRate); } } diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index 0d6877b6..f13e3b17 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -16,7 +16,8 @@ public class PhysicalPropertiesJson { */ public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** The current limit in AMPs to apply to the motors. */ - public MotorConfigInt currentLimit = new MotorConfigInt(40, 20); + public MotorConfigInt statorLimit = new MotorConfigInt(40, 20); + public MotorConfigInt supplyLimit = new MotorConfigInt(40, 20); /** The minimum number of seconds to take for the motor to go from 0 to full throttle. */ public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); /** @@ -37,8 +38,10 @@ public SwerveModulePhysicalCharacteristics createPhysicalProperties() { conversionFactor, wheelGripCoefficientOfFriction, optimalVoltage, - currentLimit.drive, - currentLimit.angle, + statorLimit.drive, + statorLimit.angle, + supplyLimit.drive, + supplyLimit.angle, rampRate.drive, rampRate.angle); } From bd47852157394c84166b9d7acb1aad6e10435c52 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 16:21:57 -0700 Subject: [PATCH 23/49] improve match dashboard --- src/main/java/frc/team2412/robot/Robot.java | 2 +- .../robot/subsystems/IntakeSubsystem.java | 16 ++++++++++++++++ .../robot/subsystems/LauncherSubsystem.java | 10 ---------- .../frc/team2412/robot/util/MatchDashboard.java | 8 +++++++- .../frc/team2412/robot/util/auto/AutoLogic.java | 10 +++++----- 5 files changed, 29 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 7442637a..85a86716 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -42,7 +42,7 @@ public static Robot getInstance() { // increases logging private static final boolean debugMode = true; // Really dangerous to keep this enabled as it disables all other controls, use with caution - private static final boolean sysIdMode = false; + private static final boolean sysIdMode = true; // Turns on an off auto logic private static final boolean autoEnabled = true; diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 5bef335c..69a907be 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -328,6 +328,22 @@ public boolean isIntakeOn() { || feederMotor.get() > 0); } + public boolean isLeftIntakeRunning() { + return intakeMotorLeft.getEncoder().getVelocity() > 1.0; + } + + public boolean isFrontIntakeRunning() { + return intakeMotorFront.getEncoder().getVelocity() > 1.0; + } + + public boolean isRightIntakeRunning() { + return intakeMotorRight.getEncoder().getVelocity() > 1.0; + } + + public boolean isIndexRunning() { + return indexMotorUpper.getEncoder().getVelocity() > 1.0; + } + // logging public void initShuffleboard() { if (Robot.isDebugMode()) { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index d6ed9b59..b0764d03 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -109,8 +109,6 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherAngleSpeedEntry; - private GenericEntry launcherIsAtSpeed; - private GenericEntry launcherAngleManual; private GenericEntry speakerDistanceEntry; @@ -358,13 +356,6 @@ private void initShuffleboard() { () -> (launcherAngleOneMotor.getEncoder().getPosition() * 360 + relativeEncoderStartPosition.orElse(0.0))); - launcherIsAtSpeed = - Shuffleboard.getTab("Match") - .add("flywheels at target speed", false) - .withSize(1, 1) - .withWidget(BuiltInWidgets.kBooleanBox) - .withPosition(0, 2) - .getEntry(); launcherAngleSpeedEntry = Shuffleboard.getTab("Launcher") .add("Launcher angle Speed", 0) @@ -469,7 +460,6 @@ public void periodic() { launcherAngleEntry.setDouble(getAngle()); launcherSpeedEntry.setDouble(getLauncherSpeed()); launcherAngleSpeedEntry.setDouble(getAngleSpeed()); - launcherIsAtSpeed.setBoolean(isAtSpeed()); launcherAngleManual.setDouble(manualAngleSetpoint); angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index d8290074..3b7ab696 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -20,6 +20,12 @@ public MatchDashboard(Subsystems s) { tab.add(new FMSWidget()).withPosition(0, 0).withSize(4, 1); tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); - AutonomousField.configureShuffleboardTab(tab, 7, 0, "Available Auto Variants", r::addPeriodic); + AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); + tab.add("Left Intake Running", s.intakeSubsystem.isLeftIntakeRunning()).withPosition(0, 4).withSize(2, 1); + tab.add("Front Intake Running", s.intakeSubsystem.isFrontIntakeRunning()).withPosition(2, 4).withSize(2, 1); + tab.add("Right Intake Running", s.intakeSubsystem.isRightIntakeRunning()).withPosition(4, 4).withSize(2, 1); + tab.add("Index Running", s.intakeSubsystem.isIndexRunning()).withPosition(6, 4).withSize(2, 1); + tab.add("Has Note", s.intakeSubsystem.indexSensorHasNote() || s.intakeSubsystem.feederSensorHasNote()).withPosition(8, 4).withSize(2, 1); + tab.add("Flywheels At Speed", s.launcherSubsystem.isAtSpeed(400)).withPosition(10, 4).withSize(2, 1); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 6b0e6ac2..c4cd97b6 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -260,11 +260,11 @@ public static void initShuffleBoard() { gameObjects.addOption(String.valueOf(i), i); } - tab.add("Starting Position", startPositionChooser).withPosition(5, 0).withSize(2, 1); - tab.add("Launch Type", isVision).withPosition(5, 1); - tab.add("Game Objects", gameObjects).withPosition(6, 1); - tab.add("Available Auto Variants", availableAutos).withPosition(5, 2).withSize(2, 1); - autoDelayEntry = tab.add("Auto Delay", 0).withPosition(5, 3).withSize(1, 1).getEntry(); + tab.add("Starting Position", startPositionChooser).withPosition(4, 0).withSize(2, 1); + tab.add("Launch Type", isVision).withPosition(4, 1); + tab.add("Game Objects", gameObjects).withPosition(5, 1); + tab.add("Available Auto Variants", availableAutos).withPosition(4, 2).withSize(2, 1); + autoDelayEntry = tab.add("Auto Delay", 0).withPosition(4, 3).withSize(1, 1).getEntry(); isVision.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); startPositionChooser.onChange((dummyVar) -> AutoLogic.filterAutos(gameObjects.getSelected())); From e40ff753ca8de80390a42415ac9c3da39f39fa90 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 16:23:35 -0700 Subject: [PATCH 24/49] Add intake motor enabled constants --- .../robot/subsystems/IntakeSubsystem.java | 93 ++++++++++++++----- 1 file changed, 68 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 5bef335c..0c84d3a6 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -37,6 +37,15 @@ public class IntakeSubsystem extends SubsystemBase { private static final boolean enableFrontAndSideIntakes = true; + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean FRONT_MOTOR_ENABLED = enableFrontAndSideIntakes && false; + + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean LEFT_MOTOR_ENABLED = enableFrontAndSideIntakes && true; + + @SuppressWarnings("ComplexBooleanConstant") + private static final boolean RIGHT_MOTOR_ENABLED = enableFrontAndSideIntakes && true; + // Motors private final CANSparkMax intakeMotorFront; private final CANSparkMax intakeMotorLeft; @@ -84,9 +93,12 @@ public class IntakeSubsystem extends SubsystemBase { public IntakeSubsystem() { - intakeMotorFront = new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless); - intakeMotorLeft = new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless); - intakeMotorRight = new CANSparkMax(INTAKE_MOTOR_RIGHT, MotorType.kBrushless); + intakeMotorFront = + FRONT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_FRONT, MotorType.kBrushless) : null; + intakeMotorLeft = + LEFT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_LEFT, MotorType.kBrushless) : null; + intakeMotorRight = + RIGHT_MOTOR_ENABLED ? new CANSparkMax(INTAKE_MOTOR_RIGHT, MotorType.kBrushless) : null; ingestMotor = new CANSparkFlex(INGEST_MOTOR, MotorType.kBrushless); indexMotorUpper = new CANSparkFlex(INDEX_MOTOR_UPPER, MotorType.kBrushless); @@ -101,8 +113,14 @@ public IntakeSubsystem() { // intakeMotorFront.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); // intakeBackSensor = // intakeMotorBack.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeLeftSensor = intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - intakeRightSensor = intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + intakeLeftSensor = + LEFT_MOTOR_ENABLED + ? intakeMotorLeft.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen) + : null; + intakeRightSensor = + RIGHT_MOTOR_ENABLED + ? intakeMotorRight.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen) + : null; // sensor must be true for 0.1 seconds before being actually true intakeFrontSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); @@ -138,9 +156,15 @@ private void configureMotor(CANSparkBase motor, boolean invert) { } private void resetMotors() { - configureMotor(intakeMotorFront, 25, true); - configureMotor(intakeMotorLeft, 25, true); - configureMotor(intakeMotorRight, 25, true); + if (FRONT_MOTOR_ENABLED) { + configureMotor(intakeMotorFront, 25, true); + } + if (LEFT_MOTOR_ENABLED) { + configureMotor(intakeMotorLeft, 25, true); + } + if (RIGHT_MOTOR_ENABLED) { + configureMotor(intakeMotorRight, 25, true); + } configureMotor(ingestMotor, false); configureMotor(indexMotorUpper, 40, false); @@ -152,9 +176,15 @@ private void resetMotors() { public void intakeSet(double speed) { if (enableFrontAndSideIntakes) { - intakeMotorFront.set(speed); - intakeMotorLeft.set(speed); - intakeMotorRight.set(speed); + if (FRONT_MOTOR_ENABLED) { + intakeMotorFront.set(speed); + } + if (LEFT_MOTOR_ENABLED) { + intakeMotorLeft.set(speed); + } + if (RIGHT_MOTOR_ENABLED) { + intakeMotorRight.set(speed); + } } ingestMotor.set(speed); } @@ -171,9 +201,15 @@ public void intakeReverse() { public void intakeSteal() { if (enableFrontAndSideIntakes) { - intakeMotorLeft.set(INTAKE_IN_SPEED); - intakeMotorRight.set(INTAKE_IN_SPEED); - intakeMotorFront.set(INTAKE_REJECT_SPEED); + if (LEFT_MOTOR_ENABLED) { + intakeMotorLeft.set(INTAKE_IN_SPEED); + } + if (RIGHT_MOTOR_ENABLED) { + intakeMotorRight.set(INTAKE_IN_SPEED); + } + if (FRONT_MOTOR_ENABLED) { + intakeMotorFront.set(INTAKE_REJECT_SPEED); + } } ingestMotor.set(INTAKE_REJECT_SPEED); indexMotorUpper.set(INTAKE_IN_SPEED); @@ -185,19 +221,19 @@ public void intakeStop() { } public void intakeFrontStop() { - if (enableFrontAndSideIntakes) { + if (FRONT_MOTOR_ENABLED) { intakeMotorFront.set(0); } } public void intakeLeftStop() { - if (enableFrontAndSideIntakes) { + if (LEFT_MOTOR_ENABLED) { intakeMotorLeft.set(0); } } public void intakeRightStop() { - if (enableFrontAndSideIntakes) { + if (RIGHT_MOTOR_ENABLED) { intakeMotorRight.set(0); } } @@ -208,19 +244,19 @@ public void intakeReject() { } public void intakeFrontReject() { - if (enableFrontAndSideIntakes) { + if (FRONT_MOTOR_ENABLED) { intakeMotorFront.set(INTAKE_REJECT_SPEED); } } public void intakeLeftReject() { - if (enableFrontAndSideIntakes) { + if (LEFT_MOTOR_ENABLED) { intakeMotorLeft.set(INTAKE_REJECT_SPEED); } } public void intakeRightReject() { - if (enableFrontAndSideIntakes) { + if (RIGHT_MOTOR_ENABLED) { intakeMotorRight.set(INTAKE_REJECT_SPEED); } } @@ -322,7 +358,7 @@ public boolean getRejectOverride() { } public boolean isIntakeOn() { - return (intakeMotorLeft.get() > 0 + return ((LEFT_MOTOR_ENABLED && intakeMotorLeft.get() > 0) || indexMotorUpper.get() > 0 || ingestMotor.get() > 0 || feederMotor.get() > 0); @@ -332,15 +368,21 @@ public boolean isIntakeOn() { public void initShuffleboard() { if (Robot.isDebugMode()) { shuffleboardTab - .addDouble("Front Intake Motor Temp", () -> intakeMotorFront.getMotorTemperature()) + .addDouble( + "Front Intake Motor Temp", + () -> FRONT_MOTOR_ENABLED ? intakeMotorFront.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab - .addDouble("Left Intake Motor Temp", () -> intakeMotorLeft.getMotorTemperature()) + .addDouble( + "Left Intake Motor Temp", + () -> LEFT_MOTOR_ENABLED ? intakeMotorLeft.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab - .addDouble("Right Intake Motor Temp", () -> intakeMotorRight.getMotorTemperature()) + .addDouble( + "Right Intake Motor Temp", + () -> RIGHT_MOTOR_ENABLED ? intakeMotorRight.getMotorTemperature() : -1) .withSize(1, 1) .withWidget(BuiltInWidgets.kTextView); shuffleboardTab @@ -404,6 +446,7 @@ public void initShuffleboard() { * returns true if all the motors are set to be not moving */ public boolean isIntakeRunning() { - return (intakeMotorLeft.get() != 0 && intakeMotorRight.get() != 0); + return (!LEFT_MOTOR_ENABLED || intakeMotorLeft.get() != 0) + && (!RIGHT_MOTOR_ENABLED || intakeMotorRight.get() != 0); } } From d08ec3bf5c12c346b83dd113b086160c05179089 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 16:41:29 -0700 Subject: [PATCH 25/49] get match dashboard working and start adding more haptic --- .../java/frc/team2412/robot/Hardware.java | 4 +-- src/main/java/frc/team2412/robot/Robot.java | 2 +- .../robot/commands/intake/AllInCommand.java | 8 ++++-- ...va => RumbleCoDriveControllerCommand.java} | 4 +-- .../intake/RumbleDriveControllerCommand.java | 28 +++++++++++++++++++ .../robot/subsystems/IntakeSubsystem.java | 4 --- .../robot/subsystems/LauncherSubsystem.java | 9 ++++-- .../team2412/robot/util/MatchDashboard.java | 23 +++++++++++---- 8 files changed, 63 insertions(+), 19 deletions(-) rename src/main/java/frc/team2412/robot/commands/intake/{RumbleCommand.java => RumbleCoDriveControllerCommand.java} (81%) create mode 100644 src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java diff --git a/src/main/java/frc/team2412/robot/Hardware.java b/src/main/java/frc/team2412/robot/Hardware.java index 1088f17a..1ffca78a 100644 --- a/src/main/java/frc/team2412/robot/Hardware.java +++ b/src/main/java/frc/team2412/robot/Hardware.java @@ -21,9 +21,9 @@ public class Hardware { public static final int LAUNCHER_PIVOT_TWO_MOTOR_ID = 32; // intake [40 - 49] - public static final int INTAKE_MOTOR_FRONT = 40; + public static final int INTAKE_MOTOR_FRONT = 41; // public static final int INTAKE_MOTOR_BACK = 42; - public static final int INTAKE_MOTOR_LEFT = 41; + public static final int INTAKE_MOTOR_LEFT = 40; public static final int INTAKE_MOTOR_RIGHT = 43; public static final int INGEST_MOTOR = 44; diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index bfba0ce8..789ec924 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -43,7 +43,7 @@ public static Robot getInstance() { // increases logging private static final boolean debugMode = true; // Really dangerous to keep this enabled as it disables all other controls, use with caution - private static final boolean sysIdMode = true; + private static final boolean sysIdMode = false; // Turns on an off auto logic private static final boolean autoEnabled = true; diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 32e841b3..65e2ca64 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -39,7 +39,7 @@ public void execute() { } if (controls != null && !rumbledIndex) { - Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(3)).schedule(); rumbledIndex = true; } } @@ -56,8 +56,12 @@ public void end(boolean interrupted) { intakeSubsystem.indexStop(); intakeSubsystem.feederStop(); + if (interrupted) { + return; + } + if (controls != null) { - Commands.race(new RumbleCommand(controls), new WaitCommand(3)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(1)).schedule(); } // rumbledIntakeFront = false; diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java similarity index 81% rename from src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java rename to src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java index b0b0d842..151d222d 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/RumbleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleCoDriveControllerCommand.java @@ -4,10 +4,10 @@ import frc.team2412.robot.Controls; import frc.team2412.robot.Controls.ControlConstants; -public class RumbleCommand extends Command { +public class RumbleCoDriveControllerCommand extends Command { private final Controls controls; - public RumbleCommand(Controls controls) { + public RumbleCoDriveControllerCommand(Controls controls) { this.controls = controls; } diff --git a/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java b/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java new file mode 100644 index 00000000..291da267 --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/intake/RumbleDriveControllerCommand.java @@ -0,0 +1,28 @@ +package frc.team2412.robot.commands.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.team2412.robot.Controls; +import frc.team2412.robot.Controls.ControlConstants; + +public class RumbleDriveControllerCommand extends Command { + private final Controls controls; + + public RumbleDriveControllerCommand(Controls controls) { + this.controls = controls; + } + + @Override + public void initialize() { + controls.vibrateDriveController(ControlConstants.RUMBLE_VIBRATION); + } + // exist + @Override + public void end(boolean interrupted) { + controls.vibrateDriveController(0.0); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index a8639278..69a3f292 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -368,10 +368,6 @@ public boolean isLeftIntakeRunning() { return intakeMotorLeft.getEncoder().getVelocity() > 1.0; } - public boolean isFrontIntakeRunning() { - return intakeMotorFront.getEncoder().getVelocity() > 1.0; - } - public boolean isRightIntakeRunning() { return intakeMotorRight.getEncoder().getVelocity() > 1.0; } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index b0764d03..5403d19c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -430,8 +430,13 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher").addBoolean("Ignoring Limits", () -> ignoreLimits); - Shuffleboard.getTab("Launcher").addBoolean("Angle Insane", () -> (relativeEncoderStartPosition.isPresent() - && Math.abs(getAngle() - getAngleOneMotorAngle()) <= ENCODER_DIFFERENCE_TOLERANCE)); + Shuffleboard.getTab("Launcher") + .addBoolean( + "Angle Insane", + () -> + (relativeEncoderStartPosition.isPresent() + && Math.abs(getAngle() - getAngleOneMotorAngle()) + <= ENCODER_DIFFERENCE_TOLERANCE)); } public void updateDistanceEntry(double distance) { diff --git a/src/main/java/frc/team2412/robot/util/MatchDashboard.java b/src/main/java/frc/team2412/robot/util/MatchDashboard.java index 3b7ab696..56e39318 100644 --- a/src/main/java/frc/team2412/robot/util/MatchDashboard.java +++ b/src/main/java/frc/team2412/robot/util/MatchDashboard.java @@ -21,11 +21,22 @@ public MatchDashboard(Subsystems s) { tab.add(field).withPosition(0, 1).withSize(4, 3); Robot r = Robot.getInstance(); AutonomousField.configureShuffleboardTab(tab, 6, 0, "Available Auto Variants", r::addPeriodic); - tab.add("Left Intake Running", s.intakeSubsystem.isLeftIntakeRunning()).withPosition(0, 4).withSize(2, 1); - tab.add("Front Intake Running", s.intakeSubsystem.isFrontIntakeRunning()).withPosition(2, 4).withSize(2, 1); - tab.add("Right Intake Running", s.intakeSubsystem.isRightIntakeRunning()).withPosition(4, 4).withSize(2, 1); - tab.add("Index Running", s.intakeSubsystem.isIndexRunning()).withPosition(6, 4).withSize(2, 1); - tab.add("Has Note", s.intakeSubsystem.indexSensorHasNote() || s.intakeSubsystem.feederSensorHasNote()).withPosition(8, 4).withSize(2, 1); - tab.add("Flywheels At Speed", s.launcherSubsystem.isAtSpeed(400)).withPosition(10, 4).withSize(2, 1); + tab.addBoolean("Left Intake Running", s.intakeSubsystem::isLeftIntakeRunning) + .withPosition(0, 4) + .withSize(2, 1); + tab.addBoolean("Right Intake Running", s.intakeSubsystem::isRightIntakeRunning) + .withPosition(2, 4) + .withSize(2, 1); + tab.addBoolean("Index Running", s.intakeSubsystem::isIndexRunning) + .withPosition(4, 4) + .withSize(2, 1); + tab.addBoolean( + "Has Note", + () -> s.intakeSubsystem.indexSensorHasNote() || s.intakeSubsystem.feederSensorHasNote()) + .withPosition(6, 4) + .withSize(2, 1); + tab.addBoolean("Flywheels At Speed", () -> s.launcherSubsystem.isAtSpeed(400)) + .withPosition(8, 4) + .withSize(2, 1); } } From 21bbb45f3ed0a39890b62f3583d87b35a30dbd09 Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 16:42:36 -0700 Subject: [PATCH 26/49] add rumble for drive controller on note pickup --- .../java/frc/team2412/robot/commands/intake/AllInCommand.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 65e2ca64..456c7fe6 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -62,6 +62,7 @@ public void end(boolean interrupted) { if (controls != null) { Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleDriveControllerCommand(controls), new WaitCommand(1)).schedule(); } // rumbledIntakeFront = false; From feaf67f3e08251eaaffda28a1758e616509d00ce Mon Sep 17 00:00:00 2001 From: kirby Date: Mon, 8 Apr 2024 16:54:27 -0700 Subject: [PATCH 27/49] add a stragler --- .../robot/commands/launcher/SetAngleLaunchCommand.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java index feca4ddd..e355294d 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/SetAngleLaunchCommand.java @@ -6,7 +6,7 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.team2412.robot.Controls; import frc.team2412.robot.Robot; -import frc.team2412.robot.commands.intake.RumbleCommand; +import frc.team2412.robot.commands.intake.RumbleCoDriveControllerCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; // this command can be used as a preset in controls, allowing the user to input a speed and angle // value when they keybind it multiple times. @@ -39,6 +39,6 @@ public boolean isFinished() { public void end(boolean interrupted) { Robot robot = Robot.getInstance(); Controls controls = robot.controls; - Commands.race(new RumbleCommand(controls), new WaitCommand(1)).schedule(); + Commands.race(new RumbleCoDriveControllerCommand(controls), new WaitCommand(1)).schedule(); } } From 0fa02b0f4fcb5f1327611dca99b05415890a2208 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:03:32 -0700 Subject: [PATCH 28/49] led change --- src/main/java/frc/team2412/robot/Hardware.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Hardware.java b/src/main/java/frc/team2412/robot/Hardware.java index 5101e2b6..9d77d32b 100644 --- a/src/main/java/frc/team2412/robot/Hardware.java +++ b/src/main/java/frc/team2412/robot/Hardware.java @@ -31,8 +31,8 @@ public class Hardware { public static final int INDEX_MOTOR_UPPER = 45; public static final int FEEDER_MOTOR = 46; - // LED strip is PWM port 2 - public static final int BLINKIN_LED = 2; + // LED strip is PWM port 3 + public static final int BLINKIN_LED = 3; // intake sensors (Digital IO) public static final int INDEX_SENSOR = 1; From b96d6c044317f9a4409003929171b1ceeb09d3b7 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:04:41 -0700 Subject: [PATCH 29/49] Reenable vision orientation correction --- .../frc/team2412/robot/sensors/AprilTagsProcessor.java | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index c1a17850..819a5bd9 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -40,8 +40,6 @@ *

2D field poses are just the projection of the 3D pose onto the XY plane. */ public class AprilTagsProcessor { - private static class FilterInfo {} - public static final Transform3d ROBOT_TO_CAM = new Transform3d( Units.inchesToMeters(27.0 / 2.0 - 0.94996), @@ -82,7 +80,6 @@ private static boolean resultIsValid(PhotonPipelineResult result) { private final PhotonPoseEstimator photonPoseEstimator; private final DrivebaseWrapper aprilTagsHelper; private final FieldObject2d rawVisionFieldObject; - private final FieldObject2d adjustedFieldObject; // These are always set with every pipeline result private double lastRawTimestampSeconds = 0; @@ -102,7 +99,6 @@ private static boolean resultIsValid(PhotonPipelineResult result) { public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { this.aprilTagsHelper = aprilTagsHelper; rawVisionFieldObject = aprilTagsHelper.getField().getObject("RawVision"); - adjustedFieldObject = aprilTagsHelper.getField().getObject("AdjustedVision"); var networkTables = NetworkTableInstance.getDefault(); // if (Robot.isSimulation()) { // networkTables.stopServer(); @@ -160,11 +156,8 @@ public void update() { if (latestPose.isPresent()) { lastValidTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); - var oldPose = aprilTagsHelper.getEstimatedPosition(); - var adjustedPose = new Pose2d(lastFieldPose.getTranslation(), oldPose.getRotation()); rawVisionFieldObject.setPose(lastFieldPose); - adjustedFieldObject.setPose(adjustedPose); - aprilTagsHelper.addVisionMeasurement(adjustedPose, lastValidTimestampSeconds, STANDARD_DEVS); + aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastValidTimestampSeconds, STANDARD_DEVS); var estimatedPose = aprilTagsHelper.getEstimatedPosition(); aprilTagsHelper.getField().setRobotPose(estimatedPose); photonPoseEstimator.setLastPose(estimatedPose); From a4997e52794aa881d2a2dccd0299cc0ef8a54d37 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:17:31 -0700 Subject: [PATCH 30/49] invert ignest, fix is running checks --- .../frc/team2412/robot/subsystems/IntakeSubsystem.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index 69a3f292..c36252e8 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -166,7 +166,7 @@ private void resetMotors() { configureMotor(intakeMotorRight, 25, true); } - configureMotor(ingestMotor, false); + configureMotor(ingestMotor, true); configureMotor(indexMotorUpper, 40, false); configureMotor(feederMotor, 40, true); @@ -365,15 +365,15 @@ public boolean isIntakeOn() { } public boolean isLeftIntakeRunning() { - return intakeMotorLeft.getEncoder().getVelocity() > 1.0; + return Math.abs(intakeMotorLeft.getEncoder().getVelocity()) > 1.0; } public boolean isRightIntakeRunning() { - return intakeMotorRight.getEncoder().getVelocity() > 1.0; + return Math.abs(intakeMotorRight.getEncoder().getVelocity()) > 1.0; } public boolean isIndexRunning() { - return indexMotorUpper.getEncoder().getVelocity() > 1.0; + return Math.abs(indexMotorUpper.getEncoder().getVelocity()) > 1.0; } // logging From ffca80669df29653632c173a5e2d90dcc33c36e5 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:27:04 -0700 Subject: [PATCH 31/49] faster feeder and sensors --- .../team2412/robot/subsystems/IntakeSubsystem.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index c36252e8..cc208ed5 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -25,15 +25,15 @@ public class IntakeSubsystem extends SubsystemBase { public static final double INTAKE_REJECT_SPEED = -0.4; public static final double INDEX_UPPER_IN_SPEED = 1.0; - public static final double INDEX_UPPER_REVERSE_SPEED = -0.3; + public static final double INDEX_UPPER_REVERSE_SPEED = -1.0; public static final double INGEST_LOWER_IN_SPEED = 0.8; - public static final double INGEST_LOWER_REVERSE_SPEED = -0.3; + public static final double INGEST_LOWER_REVERSE_SPEED = -1.0; public static final double FEEDER_SHOOT_SPEED = 1.0; - public static final double FEEDER_IN_SPEED = 0.65; - public static final double FEEDER_REVERSE_SPEED = -0.3; + public static final double FEEDER_IN_SPEED = 1.0; + public static final double FEEDER_REVERSE_SPEED = -1.0; private static final boolean enableFrontAndSideIntakes = true; @@ -127,8 +127,8 @@ public IntakeSubsystem() { intakeRightSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); intakeLeftSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); - feederSensorDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); - feederSensorIRDebouncer = new Debouncer(0.1, Debouncer.DebounceType.kBoth); + feederSensorDebouncer = new Debouncer(0.01, Debouncer.DebounceType.kBoth); + feederSensorIRDebouncer = new Debouncer(0.01, Debouncer.DebounceType.kBoth); resetMotors(); From 2de15b774b28db251911fa549b82929a5b64e5cb Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 17:33:07 -0700 Subject: [PATCH 32/49] merge more sysid --- src/main/deploy/swerve/modules/physicalproperties.json | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index f3e4a183..8b88ec01 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -1,11 +1,6 @@ { "optimalVoltage": 12, "wheelGripCoefficientOfFriction": 1.19, -<<<<<<< HEAD - "currentLimit": { - "drive": 120, - "angle": 24 -======= "statorLimit": { "drive": 120, "angle": 50 @@ -13,7 +8,6 @@ "supplyLimit": { "drive": 80, "angle": 30 ->>>>>>> 6990ee92515b3a2da7773839ebc087b584ce83d3 }, "conversionFactor": { "angle": 19.2, From 2e10198d0896a8a1bfe0b1a1d37f0667158198bd Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 18:21:20 -0700 Subject: [PATCH 33/49] tune amp align --- .../java/frc/team2412/robot/Controls.java | 3 +- .../frc/team2412/robot/util/AmpAlign.java | 42 +++++++++++++++---- 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index 6e3be020..ebc18e62 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -30,7 +30,6 @@ import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.AmpAlign; -import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -231,7 +230,7 @@ private void bindLauncherControls() { // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); launcherAmpAlignPresetButton.onTrue( Commands.either( - AmpAlign.ampPreset(s.drivebaseSubsystem), + AmpAlign.ampPreset(s.drivebaseSubsystem, s.launcherSubsystem), Commands.none(), () -> s.drivebaseSubsystem.getPose().getY() > 5.0)); diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index 654cb808..1e8bbbe6 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -10,19 +10,25 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.subsystems.DrivebaseSubsystem; +import frc.team2412.robot.subsystems.LauncherSubsystem; import java.util.List; public class AmpAlign { private static final Pose2d BLUE_AMP_POSES = // amp on the blue side - new Pose2d(new Translation2d(1.8, 7.3), Rotation2d.fromDegrees(270)); + new Pose2d(new Translation2d(1.82, 7.65), Rotation2d.fromDegrees(270)); private static final Pose2d RED_AMP_POSES = // amp on the red side - new Pose2d(new Translation2d(14.5, 7.3), Rotation2d.fromDegrees(270)); + new Pose2d(new Translation2d(14.61, 7.65), Rotation2d.fromDegrees(270)); - private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) { + private static final double LAUNCHER_AMP_PRESET_DISTANCE = 2; + + private static Command ampAlign( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { Pose2d robotPose = drivebaseSubsystem.getPose(); boolean isBlue; if (!DriverStation.getAlliance().isEmpty()) { @@ -50,25 +56,43 @@ private static Command ampAlign(DrivebaseSubsystem drivebaseSubsystem) { path = path.flipPath(); } - return AutoBuilder.followPath(path); + return AutoBuilder.followPath(path) + .deadlineWith( + Commands.waitUntil( + () -> + drivebaseSubsystem + .getPose() + .getTranslation() + .minus(ampPose.getTranslation()) + .getNorm() + < 2) + .andThen( + new SetAngleAmpLaunchCommand( + launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.AMP_AIM_ANGLE))); } - public static Command ampPreset(DrivebaseSubsystem drivebaseSubsystem) { - return new AlignCommand(drivebaseSubsystem); + public static Command ampPreset( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { + return new AlignCommand(drivebaseSubsystem, launcherSubsystem); } private static class AlignCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; + private final LauncherSubsystem launcherSubsystem; private Command ampCommand = null; - public AlignCommand(DrivebaseSubsystem drivebaseSubsystem) { + public AlignCommand( + DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { this.drivebaseSubsystem = drivebaseSubsystem; - addRequirements(drivebaseSubsystem); + this.launcherSubsystem = launcherSubsystem; + addRequirements(drivebaseSubsystem, launcherSubsystem); } @Override public void initialize() { - ampCommand = ampAlign(drivebaseSubsystem); + ampCommand = ampAlign(drivebaseSubsystem, launcherSubsystem); ampCommand.initialize(); // launcherSubsystem.setAngle(LauncherSubsystem.TRAP_AIM_ANGLE); // launcherSubsystem.launch(LauncherSubsystem.TRAP_SHOOT_SPEED_RPM); From a11a2226e495b8def310d639c72891e0ca883416 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 19:22:37 -0700 Subject: [PATCH 34/49] Spotless + use launcher amp preset distance constant --- src/main/java/frc/team2412/robot/util/AmpAlign.java | 2 +- src/main/java/swervelib/SwerveModule.java | 8 ++++++-- src/main/java/swervelib/motors/TalonFXSwerve.java | 9 ++++++++- .../parser/SwerveModulePhysicalCharacteristics.java | 1 + .../swervelib/parser/json/PhysicalPropertiesJson.java | 1 + 5 files changed, 17 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index 1e8bbbe6..39898b4b 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -65,7 +65,7 @@ private static Command ampAlign( .getTranslation() .minus(ampPose.getTranslation()) .getNorm() - < 2) + < LAUNCHER_AMP_PRESET_DISTANCE) .andThen( new SetAngleAmpLaunchCommand( launcherSubsystem, diff --git a/src/main/java/swervelib/SwerveModule.java b/src/main/java/swervelib/SwerveModule.java index 1d9d61c7..0475ff03 100644 --- a/src/main/java/swervelib/SwerveModule.java +++ b/src/main/java/swervelib/SwerveModule.java @@ -78,8 +78,12 @@ public SwerveModule( // Configure voltage comp, current limit, and ramp rate. angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage); - angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorStatorLimit, configuration.physicalCharacteristics.angleMotorSupplyLimit); - driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorStatorLimit, configuration.physicalCharacteristics.driveMotorSupplyLimit); + angleMotor.setCurrentLimit( + configuration.physicalCharacteristics.angleMotorStatorLimit, + configuration.physicalCharacteristics.angleMotorSupplyLimit); + driveMotor.setCurrentLimit( + configuration.physicalCharacteristics.driveMotorStatorLimit, + configuration.physicalCharacteristics.driveMotorSupplyLimit); angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate); driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate); diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 6337b069..019fc863 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -412,7 +412,14 @@ public void setCurrentLimit(int statorLimit, int supplyLimit) { .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(supplyLimit) .withSupplyCurrentLimitEnable(true)); - System.out.println("Configured drivebase motor " + motor.getDeviceID() + " with stator limit of " + statorLimit + " amps and supply limit of " + supplyLimit + " amps."); + System.out.println( + "Configured drivebase motor " + + motor.getDeviceID() + + " with stator limit of " + + statorLimit + + " amps and supply limit of " + + supplyLimit + + " amps."); } /** diff --git a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java index 11d0d749..35d35bbe 100644 --- a/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java +++ b/src/main/java/swervelib/parser/SwerveModulePhysicalCharacteristics.java @@ -7,6 +7,7 @@ public class SwerveModulePhysicalCharacteristics { /** Current limits for the Swerve Module. */ public final int driveMotorStatorLimit, angleMotorStatorLimit; + public final int driveMotorSupplyLimit, angleMotorSupplyLimit; /** The time it takes for the motor to go from 0 to full throttle in seconds. */ public final double driveMotorRampRate, angleMotorRampRate; diff --git a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java index f13e3b17..af8b5e62 100644 --- a/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java +++ b/src/main/java/swervelib/parser/json/PhysicalPropertiesJson.java @@ -17,6 +17,7 @@ public class PhysicalPropertiesJson { public MotorConfigDouble conversionFactor = new MotorConfigDouble(0, 0); /** The current limit in AMPs to apply to the motors. */ public MotorConfigInt statorLimit = new MotorConfigInt(40, 20); + public MotorConfigInt supplyLimit = new MotorConfigInt(40, 20); /** The minimum number of seconds to take for the motor to go from 0 to full throttle. */ public MotorConfigDouble rampRate = new MotorConfigDouble(0.25, 0.25); From 68181e8eea7e9eb7e5ff2830b3eb95c6eb577b52 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Mon, 8 Apr 2024 21:06:13 -0700 Subject: [PATCH 35/49] auto tuning --- .../pathplanner/autos/MasterPIDTest.auto | 25 ++++++ .../pathplanner/paths/BottomNoteLaunch.path | 8 +- .../pathplanner/paths/FarN3ToLowSpeaker.path | 8 +- .../HighSpeakerCenterLineNoteReturn.path | 6 +- .../pathplanner/paths/LowLaunchToMidNote.path | 8 +- .../paths/LowNoteToLowSpeaker.path | 8 +- .../paths/LowSpeakerCenterLine.path | 8 +- .../paths/LowSpeakerCenterSteal.path | 8 +- .../paths/LowSpeakerMidNote2Return.path | 16 ++-- .../paths/LowSpeakerPassAutoLine.path | 8 +- .../paths/LowSpeakerQCenterLineN5.path | 8 +- .../pathplanner/paths/LowSpeakerToFarN3.path | 8 +- .../paths/LowSpeakerToFarN4Return.path | 16 ++-- .../paths/LowSpeakerToHighNoteReturn.path | 16 ++-- .../paths/LowSpeakerToLowNote.path | 8 +- .../paths/LowSpeakerToMiddleLowNote.path | 8 +- .../pathplanner/paths/MasterPIDTest.path | 79 +++++++++++++++++++ .../paths/MiddleLowNoteToLowSpeaker.path | 8 +- .../paths/SpeakerToHighNoteReturn.path | 2 +- .../pathplanner/paths/SpeakerToLowNote.path | 8 +- .../robot/sensors/AprilTagsProcessor.java | 2 +- .../robot/subsystems/DrivebaseSubsystem.java | 4 +- .../team2412/robot/util/auto/AutoLogic.java | 1 + 23 files changed, 188 insertions(+), 83 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MasterPIDTest.auto create mode 100644 src/main/deploy/pathplanner/paths/MasterPIDTest.path diff --git a/src/main/deploy/pathplanner/autos/MasterPIDTest.auto b/src/main/deploy/pathplanner/autos/MasterPIDTest.auto new file mode 100644 index 00000000..42cad18d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MasterPIDTest.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2.3, + "y": 5.496252587946771 + }, + "rotation": 180.0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MasterPIDTest" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path index 839185f3..8e484700 100644 --- a/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/BottomNoteLaunch.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.551735285288752, - "y": 4.207264414528829 + "x": 2.6, + "y": 4.79 }, "prevControl": { - "x": 2.451735285288752, - "y": 4.207264414528829 + "x": 2.5, + "y": 4.79 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path index 25c54e97..3b078f13 100644 --- a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 1.6972725837343567, - "y": 3.0877308601464057 + "x": 3.261816204160629, + "y": 4.113983448093177 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path index 3409018e..6ffdc1b6 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path @@ -47,7 +47,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": -172.64, + "rotationDegrees": -172.0, "rotateFast": false }, { @@ -61,8 +61,8 @@ "rotateFast": false }, { - "waypointRelativePos": 0.6, - "rotationDegrees": 180.0, + "waypointRelativePos": 0.35, + "rotationDegrees": -161.01300595815806, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path index 5da87bc2..f354569c 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.551735285288752, - "y": 4.207264414528829 + "x": 2.6, + "y": 4.79 }, "prevControl": null, "nextControl": { - "x": 2.722428944898246, - "y": 4.546769458039406 + "x": 2.770693659609494, + "y": 5.1295050435105765 }, "isLocked": false, "linkedName": "PodiumLaunch" diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 4dbd009a..0891c3a8 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 1.198058612333441, - "y": 3.974467068033235 + "x": 2.762602232759713, + "y": 5.0007196559800065 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path index 15be5d2c..6aeabb17 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.689169744135214, - "y": 2.3304424556117516 + "x": 3.253713364561486, + "y": 3.356695043558523 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path index ab8d22c8..ed52640d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 2.288933586876183, - "y": 2.1355264332150297 + "x": 3.853477207302455, + "y": 3.1617790211618013 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index 46740492..e99c0b58 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.689169744135214, - "y": 3.860904253875016 + "x": 3.253713364561486, + "y": 4.8871568418217874 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 1.6794199859183583, - "y": 3.948902319609865 + "x": 3.2439636063446304, + "y": 4.975154907556637 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index fe9bbbef..b8fe82cd 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.2211813497261215, - "y": 3.0616743218759597 + "x": 2.7857249701523936, + "y": 4.087926909822731 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path index ac8e0fa9..2d475edf 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.2649659988419506, - "y": 3.9350340011580496 + "x": 2.8295096192682228, + "y": 4.961286589104821 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path index c2920a61..9c981485 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.7933352100034745, - "y": 2.218463140171392 + "x": 3.3578788304297467, + "y": 3.2447157281181638 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path index bdbd8a5a..1726b8a8 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.1485164850671583, - "y": 3.4029989427500227 + "x": 2.7130601054934305, + "y": 4.429251530696795 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 1.4993411183403562, - "y": 3.3094457072105032 + "x": 3.0638847387666286, + "y": 4.3356982951572745 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index 855a9b7b..518a7f10 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.6794199859183583, - "y": 3.8024057045738795 + "x": 3.2439636063446304, + "y": 4.828658292520651 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 2.079160072809459, - "y": 3.3149177937310745 + "x": 3.643703693235731, + "y": 4.341170381677846 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index ebc7459b..cadfd3d0 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 1.6986020361229914, - "y": 4.097669396600055 + "x": 3.2631456565492636, + "y": 5.1239219845468265 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index ce9bac8b..5c681db9 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": null, "nextControl": { - "x": 2.0485713043498155, - "y": 1.8489406348182067 + "x": 3.6131149247760876, + "y": 2.8751932227649784 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/MasterPIDTest.path b/src/main/deploy/pathplanner/paths/MasterPIDTest.path new file mode 100644 index 00000000..149fc7b0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MasterPIDTest.path @@ -0,0 +1,79 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.3, + "y": 5.496252587946771 + }, + "prevControl": null, + "nextControl": { + "x": 3.7734634597474326, + "y": 7.180210827658124 + }, + "isLocked": false, + "linkedName": "LowSpeaker" + }, + { + "anchor": { + "x": 6.223779513086091, + "y": 7.063269283233724 + }, + "prevControl": { + "x": 5.346717929903097, + "y": 7.215293290985443 + }, + "nextControl": { + "x": 7.617215478650247, + "y": 6.821740382535936 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4522649102278256, + "y": 6.0692661556263285 + }, + "prevControl": { + "x": 4.762010207781099, + "y": 6.3616200166873265 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.05, + "rotationDegrees": 34.76, + "rotateFast": false + }, + { + "waypointRelativePos": 0.1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 1.332219853869633, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 180.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index e6398465..176ded21 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.73, - "y": 4.47 + "x": 2.294543620426272, + "y": 5.496252587946771 }, "prevControl": { - "x": 1.382399573915957, - "y": 2.140030262966509 + "x": 2.946943194342229, + "y": 3.1662828509132805 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path index 3a93eb82..75aa19ac 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToHighNoteReturn.path @@ -48,7 +48,7 @@ { "waypointRelativePos": 0.65, "rotationDegrees": -131.3418666211863, - "rotateFast": false + "rotateFast": true }, { "waypointRelativePos": 1.0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path index b2630521..95b72437 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToLowNote.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.6148626582967456, - "y": 4.529772488116405 + "x": 1.382399573915957, + "y": 4.7595208580730555 }, "isLocked": false, "linkedName": "MidSpeaker" @@ -20,8 +20,8 @@ "y": 4.07539893464585 }, "prevControl": { - "x": 1.9520301897746657, - "y": 4.089988751406074 + "x": 1.3005404928188775, + "y": 4.1046482092964185 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 819a5bd9..3aa74044 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -49,7 +49,7 @@ public class AprilTagsProcessor { // TODO Measure these private static final Vector STANDARD_DEVS = - VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(30)); + VecBuilder.fill(0.2, 0.2, Units.degreesToRadians(30)); private static final double MAX_POSE_AMBIGUITY = 0.1; diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 32e36aa1..3c0f7605 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -71,13 +71,13 @@ public class DrivebaseSubsystem extends SubsystemBase { private static final PIDConstants AUTO_TRANSLATION_PID = Robot.getInstance().getRobotType() == RobotType.COMPETITION - ? new PIDConstants(5, 0, 0.5) // practice + ? new PIDConstants(5.3, 0, 0.6) // practice : Robot.getInstance().getRobotType() == RobotType.BONK ? new PIDConstants(6, 0, 0.1) // bonk : Robot.getInstance().getRobotType() == RobotType.CRANE ? new PIDConstants(3.9, 0, 0.2) // crane : new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune - private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(4.0, 0, 0.2); + private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 0); private static final double MAX_AUTO_SPEED = 500.0; // this seems to only affect rotation for some reason diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 66d7f833..21a74a6f 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -86,6 +86,7 @@ public static enum StartPosition { // presets new AutoPath("Test Path Rotate", "5mForwardRotate180"), new AutoPath("Test Path", "DiameterTest"), + new AutoPath("Master PID Test", "MasterPIDTest"), new AutoPath("Tune Translational PID", "TuneTranslationalPID"), new AutoPath("Tune Rotational PID", "TuneRotationalPID"), new AutoPath("Stand Still", "PresetSourceSide1Score"), From 388e5f7aca96146e41561fa61060eb27d188a765 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Apr 2024 14:50:12 -0700 Subject: [PATCH 36/49] switch to throughbore encoder --- .../robot/subsystems/LauncherSubsystem.java | 167 +++++++++++++----- 1 file changed, 119 insertions(+), 48 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 5403d19c..e36fb8c7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -34,15 +34,20 @@ public class LauncherSubsystem extends SubsystemBase { // CONSTANTS + private static final boolean USE_THROUGHBORE = true; + // HARDWARE private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0; + private static final double PIVOT_TO_ENCODER_GEARING_RATIO = 1.0 / 2.0; private static final float PIVOT_SOFTSTOP_FORWARD = 0.93f; private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f; + private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.93f; + private static final float PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE = 0.38f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; private static final int PIVOT_OFFSET = 36; // offset stuff - private static final double ENCODER_DIFFERENCE_TOLERANCE = 0.01; + private static final double ENCODER_DIFFERENCE_TOLERANCE = 15; private static final double OFFSET_SYNCING_TOLERANCE = 0.06; // ANGLE VALUES @@ -78,7 +83,9 @@ public class LauncherSubsystem extends SubsystemBase { private final RelativeEncoder launcherTopEncoder; private final RelativeEncoder launcherBottomEncoder; private final SparkAbsoluteEncoder launcherAngleEncoder; + private final SparkAbsoluteEncoder launcherAngleThroughboreEncoder; private final SparkPIDController launcherAngleOnePIDController; + private final SparkPIDController launcherAngleTwoPIDController; // private final SparkPIDController launcherAngleTwoPIDController; private final ArmFeedforward launcherPivotFF = new ArmFeedforward(0.40434, 0.096771, 0.0056403); // arm FF values: @@ -119,6 +126,8 @@ public class LauncherSubsystem extends SubsystemBase { private GenericEntry launcherFlywheelSetpointEntry; + private GenericEntry launcherDisabledEntry; + // Constructors public LauncherSubsystem() { @@ -134,6 +143,7 @@ public LauncherSubsystem() { launcherTopEncoder = launcherTopMotor.getEncoder(); launcherBottomEncoder = launcherBottomMotor.getEncoder(); launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle); + launcherAngleThroughboreEncoder = launcherAngleTwoMotor.getAbsoluteEncoder(Type.kDutyCycle); manualAngleSetpoint = launcherAngleEncoder.getPosition(); // PID controllers @@ -144,8 +154,8 @@ public LauncherSubsystem() { launcherBottomPIDController.setFeedbackDevice(launcherBottomEncoder); launcherAngleOnePIDController = launcherAngleOneMotor.getPIDController(); launcherAngleOnePIDController.setFeedbackDevice(launcherAngleEncoder); - // launcherAngleTwoPIDController = launcherAngleTwoMotor.getPIDController(); - // launcherAngleTwoPIDController.setFeedbackDevice(launcherAngleEncoder); + launcherAngleTwoPIDController = launcherAngleTwoMotor.getPIDController(); + launcherAngleTwoPIDController.setFeedbackDevice(launcherAngleThroughboreEncoder); relativeEncoderStartPosition = Optional.empty(); @@ -173,20 +183,38 @@ public void configMotors() { launcherAngleOneMotor.setSmartCurrentLimit(100); launcherAngleTwoMotor.setSmartCurrentLimit(100); - launcherAngleOneMotor.setSoftLimit( - CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD); - launcherAngleOneMotor.setSoftLimit( - CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD); - launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); - launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + if (!USE_THROUGHBORE) { + launcherAngleOneMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD); + launcherAngleOneMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD); + launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + launcherAngleOneMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); - launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); + launcherAngleTwoMotor.follow(launcherAngleOneMotor, true); + + launcherAngleOnePIDController.setP(5.2); + launcherAngleOnePIDController.setI(0); + launcherAngleOnePIDController.setD(0.066248); + launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); + } else { + launcherAngleTwoMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kForward, PIVOT_SOFTSTOP_FORWARD_THROUGHBORE); + launcherAngleTwoMotor.setSoftLimit( + CANSparkBase.SoftLimitDirection.kReverse, PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE); + launcherAngleTwoMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kForward, true); + launcherAngleTwoMotor.enableSoftLimit(CANSparkBase.SoftLimitDirection.kReverse, true); + + launcherAngleTwoMotor.setInverted(true); + launcherAngleOneMotor.follow(launcherAngleTwoMotor, true); + + launcherAngleTwoPIDController.setP(5.2); + launcherAngleTwoPIDController.setI(0); + launcherAngleTwoPIDController.setD(0.066248); + launcherAngleTwoPIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); + } // PID - launcherAngleOnePIDController.setP(5.2); - launcherAngleOnePIDController.setI(0); - launcherAngleOnePIDController.setD(0.066248); - launcherAngleOnePIDController.setOutputRange(-ANGLE_MAX_SPEED, ANGLE_MAX_SPEED); launcherTopPIDController.setP(0.002); // 7.7633E-05); launcherTopPIDController.setI(0); launcherTopPIDController.setD(0.001); @@ -240,14 +268,27 @@ public void ampLaunch(double speed) { public double getLauncherSpeed() { return launcherTopEncoder.getVelocity(); } + + public double convertEncoderRotationsToPivotRotations(double encoderRotations) { + return 1 - PIVOT_TO_ENCODER_GEARING_RATIO * (1 - encoderRotations); + } + + public double convertPivotRotationsToEncoderRotations(double pivotRotations) { + return 1 - (1 - pivotRotations) / PIVOT_TO_ENCODER_GEARING_RATIO; + } + // returns the degrees of the angle of the launcher public double getAngle() { // get position returns a double in the form of rotations - return Units.rotationsToDegrees(launcherAngleEncoder.getPosition()); + if (!USE_THROUGHBORE) { + return Units.rotationsToDegrees(launcherAngleEncoder.getPosition()); + } + return Units.rotationsToDegrees( + convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition())); } /** - * Sets the launcher angle, taking the offset into account. + * s Sets the launcher angle, taking the offset into account. * * @param launcherAngle Launcher angle. PIVOT_OFFSET will be added to this. */ @@ -261,18 +302,26 @@ public void setAngle(double launcherAngle) { } else { angleSetpoint = launcherAngle; } - launcherAngleOnePIDController.setReference( - Units.degreesToRotations(angleSetpoint), - ControlType.kPosition, - 0, - launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + if (!USE_THROUGHBORE) { + launcherAngleOnePIDController.setReference( + Units.degreesToRotations(angleSetpoint), + ControlType.kPosition, + 0, + launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + } else { + launcherAngleTwoPIDController.setReference( + convertPivotRotationsToEncoderRotations(Units.degreesToRotations(angleSetpoint)), + ControlType.kPosition, + 0, + launcherPivotFF.calculate(Units.degreesToRadians(launcherAngle - FF_PIVOT_OFFSET), 0)); + } manualAngleSetpoint = Units.degreesToRotations(launcherAngle); // launcherAngleTwoPIDController.setReference( // Units.degreesToRotations(angleSetpoint), ControlType.kPosition); } public boolean isAtAngle(double tolerance) { - return MathUtil.isNear(angleSetpoint, launcherAngleEncoder.getPosition(), tolerance); + return MathUtil.isNear(angleSetpoint, getAngle(), tolerance); } public boolean isAtAngle() { @@ -298,7 +347,11 @@ public void restoreLimits() { public void setAngleManual(double joystickInput, boolean powerControl, boolean ignoreLimits) { this.ignoreLimits = ignoreLimits; if (powerControl || ignoreLimits) { - launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); + if (!USE_THROUGHBORE) { + launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); + } else { + launcherAngleTwoMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); + } manualAngleSetpoint = MathUtil.clamp( Units.degreesToRotations(getAngle()), @@ -315,22 +368,39 @@ public void setAngleManual(double joystickInput, boolean powerControl, boolean i if (Units.degreesToRotations(getAngle()) > PIVOT_SOFTSTOP_BACKWARD && Units.degreesToRotations(getAngle()) < PIVOT_SOFTSTOP_FORWARD) { - launcherAngleOnePIDController.setReference( - manualAngleSetpoint, - ControlType.kPosition, - 0, - launcherPivotFF.calculate( - Units.degreesToRadians( - Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), - 0)); + if (!USE_THROUGHBORE) { + launcherAngleOnePIDController.setReference( + manualAngleSetpoint, + ControlType.kPosition, + 0, + launcherPivotFF.calculate( + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); + } else { + launcherAngleTwoPIDController.setReference( + convertPivotRotationsToEncoderRotations(manualAngleSetpoint), + ControlType.kPosition, + 0, + launcherPivotFF.calculate( + Units.degreesToRadians( + Units.rotationsToDegrees(manualAngleSetpoint) - FF_PIVOT_OFFSET), + 0)); + } } } private void initShuffleboard() { if (Robot.isDebugMode()) { - Shuffleboard.getTab("Launcher") - .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) - .withPosition(2, 0); + if (!USE_THROUGHBORE) { + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherAngleOnePIDController, "launcherAnglePID")) + .withPosition(2, 0); + } else { + Shuffleboard.getTab("Launcher") + .add(new SparkPIDWidget(launcherAngleTwoPIDController, "launcherAngleThroughborePID")) + .withPosition(2, 0); + } Shuffleboard.getTab("Launcher") .add(new SparkPIDWidget(launcherTopPIDController, "launcherTopPID")) .withPosition(0, 0); @@ -386,8 +456,7 @@ private void initShuffleboard() { .withPosition(5, 0) .getEntry(); - setLauncherAngleEntry = - Shuffleboard.getTab("Launcher").add("Launcher Angle Setpoint", getAngle()).getEntry(); + Shuffleboard.getTab("Launcher").addDouble("Launcher Angle Setpoint", () -> manualAngleSetpoint); launcherAngleManual = Shuffleboard.getTab("Launcher") @@ -430,13 +499,7 @@ private void initShuffleboard() { Shuffleboard.getTab("Launcher").addBoolean("Ignoring Limits", () -> ignoreLimits); - Shuffleboard.getTab("Launcher") - .addBoolean( - "Angle Insane", - () -> - (relativeEncoderStartPosition.isPresent() - && Math.abs(getAngle() - getAngleOneMotorAngle()) - <= ENCODER_DIFFERENCE_TOLERANCE)); + launcherDisabledEntry = Shuffleboard.getTab("Launcher").add("Angle Insane", false).getEntry(); } public void updateDistanceEntry(double distance) { @@ -468,17 +531,20 @@ public void periodic() { launcherAngleManual.setDouble(manualAngleSetpoint); angleSetpointEntry.setDouble(angleSetpoint); launcherFlywheelSetpointEntry.setDouble(rpmSetpoint); + launcherDisabledEntry.setBoolean(false); // PIVOT ENCODER SANITY CHECKS // compares the relative encoder angle vs the absolute encoder angle if (relativeEncoderStartPosition.isPresent() - && Math.abs(getAngle() - getAngleOneMotorAngle()) <= ENCODER_DIFFERENCE_TOLERANCE) { + && Math.abs(getAngle() - getAngleOneMotorAngle()) >= ENCODER_DIFFERENCE_TOLERANCE) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); + launcherAngleTwoMotor.disable(); + launcherDisabledEntry.setBoolean(true); } DriverStation.reportError( "Pivot encoder deviated too far from motor encoder angle ... .. Reported pivot angle of " - + launcherAngleEncoder + + getAngle() + " and motor angle of " + getAngleOneMotorAngle() + ". Is overidden: " @@ -486,11 +552,16 @@ public void periodic() { false); } - // - if (launcherAngleEncoder.getPosition() >= PIVOT_SOFTSTOP_FORWARD + PIVOT_DISABLE_OFFSET - || launcherAngleEncoder.getPosition() <= PIVOT_SOFTSTOP_BACKWARD - PIVOT_DISABLE_OFFSET) { + if (launcherAngleEncoder.getPosition() + >= (USE_THROUGHBORE ? PIVOT_SOFTSTOP_FORWARD_THROUGHBORE : PIVOT_SOFTSTOP_FORWARD) + + PIVOT_DISABLE_OFFSET + || launcherAngleEncoder.getPosition() + <= (USE_THROUGHBORE ? PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE : PIVOT_SOFTSTOP_BACKWARD) + - PIVOT_DISABLE_OFFSET) { if (!ignoreLimits) { launcherAngleOneMotor.disable(); + launcherAngleTwoMotor.disable(); + launcherDisabledEntry.setBoolean(true); } DriverStation.reportError( "Launcher encoder angle is insane!!!! Reports angle of " From 19c39f1cd8ea02d10dc6c3912422c2fca83bf9c8 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Tue, 9 Apr 2024 14:50:55 -0700 Subject: [PATCH 37/49] Driver turbo rotation --- src/main/java/frc/team2412/robot/Controls.java | 3 ++- .../robot/subsystems/DrivebaseSubsystem.java | 12 +++++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index ebc18e62..b468e8bb 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -154,7 +154,8 @@ private void bindDrivebaseControls() { s.drivebaseSubsystem.driveJoystick( driveController::getLeftY, driveController::getLeftX, - () -> Rotation2d.fromRotations(driveController.getRightX()))); + () -> Rotation2d.fromRotations(driveController.getRightX()), + driveController.rightTrigger())); driveController.rightStick().onTrue(new InstantCommand(s.drivebaseSubsystem::toggleXWheels)); driveController .start() diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 3c0f7605..59c907bb 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -32,6 +32,7 @@ import java.io.File; import java.util.EnumSet; import java.util.Map; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; import swervelib.SwerveDrive; @@ -91,6 +92,7 @@ public class DrivebaseSubsystem extends SubsystemBase { private GenericEntry headingCorrectionEntry; private GenericEntry translationSpeedEntry; private GenericEntry rotationSpeedEntry; + private GenericEntry turboRotationMultiplierEntry; private GenericEntry xWheelsEntry; private GenericEntry flipTranslationEntry; @@ -188,13 +190,14 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO * @param rotation Rotation2d value of robot rotation. CW is positive TODO: is this true? */ public Command driveJoystick( - DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation) { + DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation, BooleanSupplier turboRotation) { return this.run( () -> { Rotation2d constrainedRotation = Rotation2d.fromRotations( SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND) * MAX_SPEED + * (turboRotation.getAsBoolean() ? turboRotationMultiplierEntry.getDouble(1.0) : 1) * rotationSpeedEntry.getDouble(1.0) * -1); Translation2d constrainedTranslation = @@ -322,6 +325,13 @@ private void initShuffleboard() { .withSize(2, 1) .withProperties(Map.of("Min", 0.0)) .getEntry(); + turboRotationMultiplierEntry = + drivebaseTab + .addPersistent("Turbo rotation multiplier", 1.0) + .withWidget(BuiltInWidgets.kNumberSlider) + .withSize(2, 1) + .withProperties(Map.of("Min", 0.5, "Max", 5.0)) + .getEntry(); xWheelsEntry = drivebaseTab .addPersistent("X Wheels", xWheelsEnabled) From 528d10cbd2deb50f7a42dcfe01ffb7646b2f4959 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Apr 2024 16:10:46 -0700 Subject: [PATCH 38/49] launcher data points w/ throughbore --- ...launcher_data.csv => launcher_data_lamprey.csv} | 0 src/main/deploy/launcher_data_throughbore.csv | 8 ++++++++ src/main/deploy/swerve/controllerproperties.json | 4 ++-- .../robot/commands/launcher/FullTargetCommand.java | 14 ++++++++++++-- .../commands/launcher/ManualAngleCommand.java | 5 +++++ .../robot/subsystems/DrivebaseSubsystem.java | 13 +++++++++++-- .../robot/subsystems/LauncherSubsystem.java | 8 ++++++-- .../frc/team2412/robot/util/auto/AutoLogic.java | 1 + 8 files changed, 45 insertions(+), 8 deletions(-) rename src/main/deploy/{launcher_data.csv => launcher_data_lamprey.csv} (100%) create mode 100644 src/main/deploy/launcher_data_throughbore.csv diff --git a/src/main/deploy/launcher_data.csv b/src/main/deploy/launcher_data_lamprey.csv similarity index 100% rename from src/main/deploy/launcher_data.csv rename to src/main/deploy/launcher_data_lamprey.csv diff --git a/src/main/deploy/launcher_data_throughbore.csv b/src/main/deploy/launcher_data_throughbore.csv new file mode 100644 index 00000000..7e3c8e0e --- /dev/null +++ b/src/main/deploy/launcher_data_throughbore.csv @@ -0,0 +1,8 @@ +# DISTANCE (meters), ANGLE (degrees without PIVOT_OFFSET), RPM (velocity) + +1.30, 254, 3500 +2.38, 237, 3500 +3.00, 232, 3750 +4.00, 228, 4000 +5.00, 224.5, 4500 +6.00, 220.5, 5500 \ No newline at end of file diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index 901c1a2b..ffb1caf4 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.5, + "p": 0.4, "i": 0, - "d": 0.05 + "d": 0.15 } } \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index 76be812e..fa26125a 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -23,7 +23,11 @@ public class FullTargetCommand extends Command { private static final InterpolatingTreeMap LAUNCHER_DATA = LauncherDataLoader.fromCSV( FileSystems.getDefault() - .getPath(Filesystem.getDeployDirectory().getPath(), "launcher_data.csv")); + .getPath( + Filesystem.getDeployDirectory().getPath(), + LauncherSubsystem.USE_THROUGHBORE + ? "launcher_data_throughbore.csv" + : "launcher_data_lamprey.csv")); private static final double YAW_TARGET_VIBRATION_TOLERANCE = 10; // degrees private Translation2d SPEAKER_POSITION; @@ -59,7 +63,13 @@ public void initialize() { @Override public void execute() { - Translation2d robotPosition = drivebaseSubsystem.getPose().getTranslation(); + // look ahead half a second into the future + var fieldSpeed = drivebaseSubsystem.getFieldSpeeds().times(0.5); + Translation2d robotPosition = + drivebaseSubsystem + .getPose() + .getTranslation() + .plus(new Translation2d(fieldSpeed.vxMetersPerSecond, fieldSpeed.vyMetersPerSecond)); Translation2d robotToSpeaker = SPEAKER_POSITION.minus(robotPosition); yawTarget = robotToSpeaker.getAngle(); double distance = robotToSpeaker.getNorm(); diff --git a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java index a58e8485..daf62b08 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/ManualAngleCommand.java @@ -24,6 +24,11 @@ public ManualAngleCommand( addRequirements(launcherSubsystem); } + @Override + public void initialize() { + launcherSubsystem.resetManualAngleSetpoint(); + } + @Override public void execute() { launcherSubsystem.setAngleManual( diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 59c907bb..dde3a25e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -190,14 +190,19 @@ public void drive(Translation2d translation, Rotation2d rotation, boolean fieldO * @param rotation Rotation2d value of robot rotation. CW is positive TODO: is this true? */ public Command driveJoystick( - DoubleSupplier forward, DoubleSupplier strafe, Supplier rotation, BooleanSupplier turboRotation) { + DoubleSupplier forward, + DoubleSupplier strafe, + Supplier rotation, + BooleanSupplier turboRotation) { return this.run( () -> { Rotation2d constrainedRotation = Rotation2d.fromRotations( SwerveMath.applyDeadband(rotation.get().getRotations(), true, JOYSTICK_DEADBAND) * MAX_SPEED - * (turboRotation.getAsBoolean() ? turboRotationMultiplierEntry.getDouble(1.0) : 1) + * (turboRotation.getAsBoolean() + ? turboRotationMultiplierEntry.getDouble(1.0) + : 1) * rotationSpeedEntry.getDouble(1.0) * -1); Translation2d constrainedTranslation = @@ -251,6 +256,10 @@ public ChassisSpeeds getRobotSpeeds() { return swerveDrive.getRobotVelocity(); } + public ChassisSpeeds getFieldSpeeds() { + return swerveDrive.getFieldVelocity(); + } + /** Set the robot's pose. TODO: does this change yaw too? does this affect field oriented? */ public void setPose(Pose2d pose) { swerveDrive.resetOdometry(pose); diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index e36fb8c7..fa02cdaf 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -34,7 +34,7 @@ public class LauncherSubsystem extends SubsystemBase { // CONSTANTS - private static final boolean USE_THROUGHBORE = true; + public static final boolean USE_THROUGHBORE = true; // HARDWARE private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0; @@ -44,7 +44,7 @@ public class LauncherSubsystem extends SubsystemBase { private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.93f; private static final float PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE = 0.38f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; - private static final int PIVOT_OFFSET = 36; + private static final int PIVOT_OFFSET = USE_THROUGHBORE ? 40 : 36; // offset stuff private static final double ENCODER_DIFFERENCE_TOLERANCE = 15; @@ -523,6 +523,10 @@ public void zeroRelativeEncoder(double pivotAngle) { } } + public void resetManualAngleSetpoint() { + manualAngleSetpoint = Units.degreesToRotations(getAngle()); + } + @Override public void periodic() { launcherAngleEntry.setDouble(getAngle()); diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 21a74a6f..eea883ff 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -434,6 +434,7 @@ public static Command visionLaunch() { Commands.either( new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) .until(isReadyToLaunch()) + .andThen(Commands.waitUntil(hasNote())) .andThen(new WaitCommand(FEEDER_DELAY)) .andThen( new FeederInCommand(s.intakeSubsystem) From f0c92a27aadf185a99b38d763e08a80c91e20e74 Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Apr 2024 16:18:07 -0700 Subject: [PATCH 39/49] tuning full target and amp align --- src/main/deploy/launcher_data_throughbore.csv | 2 +- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 2 +- src/main/java/frc/team2412/robot/util/AmpAlign.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/deploy/launcher_data_throughbore.csv b/src/main/deploy/launcher_data_throughbore.csv index 7e3c8e0e..774c1c2d 100644 --- a/src/main/deploy/launcher_data_throughbore.csv +++ b/src/main/deploy/launcher_data_throughbore.csv @@ -4,5 +4,5 @@ 2.38, 237, 3500 3.00, 232, 3750 4.00, 228, 4000 -5.00, 224.5, 4500 +5.00, 224.3, 4500 6.00, 220.5, 5500 \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index fa02cdaf..374e7ea9 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -51,7 +51,7 @@ public class LauncherSubsystem extends SubsystemBase { private static final double OFFSET_SYNCING_TOLERANCE = 0.06; // ANGLE VALUES - public static final int AMP_AIM_ANGLE = 285 + PIVOT_OFFSET; + public static final int AMP_AIM_ANGLE = 290 + PIVOT_OFFSET; public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index 39898b4b..b3add876 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -19,7 +19,7 @@ public class AmpAlign { private static final Pose2d BLUE_AMP_POSES = // amp on the blue side - new Pose2d(new Translation2d(1.82, 7.65), Rotation2d.fromDegrees(270)); + new Pose2d(new Translation2d(1.71, 7.65), Rotation2d.fromDegrees(270)); private static final Pose2d RED_AMP_POSES = // amp on the red side From e230f579c2a42daf3bb8f5c19ec0b533c57d45df Mon Sep 17 00:00:00 2001 From: DriverStation3 <58869582+jbko6@users.noreply.github.com> Date: Tue, 9 Apr 2024 17:32:56 -0700 Subject: [PATCH 40/49] auto tuning (vision works) --- .../pathplanner/paths/FarN3ToLowSpeaker.path | 8 +++---- .../HighNoteLaunch2ToFarHighNoteToLaunch.path | 14 ++++++------- .../paths/HighSpeakerLaunchHighNote.path | 16 ++++++++++++-- .../paths/HighSpeakerToHighNoteLaunch2.path | 17 +++++++++------ .../paths/LowLaunchNoteFourCenterLine.path | 2 +- .../paths/LowNoteToLowSpeaker.path | 8 +++---- .../paths/LowSpeakerCenterLine.path | 8 +++---- .../paths/LowSpeakerCenterSteal.path | 8 +++---- .../paths/LowSpeakerMidNote2Return.path | 16 +++++++------- .../paths/LowSpeakerPassAutoLine.path | 8 +++---- .../paths/LowSpeakerQCenterLineN5.path | 8 +++---- .../pathplanner/paths/LowSpeakerToFarN3.path | 8 +++---- .../paths/LowSpeakerToFarN4Return.path | 16 +++++++------- .../paths/LowSpeakerToHighNoteReturn.path | 16 +++++++------- .../paths/LowSpeakerToLowNote.path | 8 +++---- .../paths/LowSpeakerToMiddleLowNote.path | 8 +++---- .../pathplanner/paths/MasterPIDTest.path | 8 +++---- .../paths/MidNoteToCenterMidNodeLaunch.path | 19 ++++++++++++++++- .../paths/MidSpeakerToCenterNotetoLaunch.path | 2 +- .../paths/MiddleLowNoteToLowSpeaker.path | 8 +++---- .../team2412/robot/util/auto/AutoLogic.java | 21 +++++-------------- 21 files changed, 125 insertions(+), 102 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path index 3b078f13..3eb62dad 100644 --- a/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/FarN3ToLowSpeaker.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 3.261816204160629, - "y": 4.113983448093177 + "x": 1.6947995088736767, + "y": 3.084897857158463 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index 1d176d6a..869d7da3 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.459228700001152, - "y": 7.268235228706192 + "x": 6.48105091081977, + "y": 7.320540680967402 }, "isLocked": false, "linkedName": "LaunchHighNote" @@ -20,12 +20,12 @@ "y": 7.395762539272827 }, "prevControl": { - "x": 8.901740880404835, - "y": 7.4608705342766815 + "x": 9.311036285890234, + "y": 7.390705607622041 }, "nextControl": { - "x": 7.628402463912032, - "y": 7.350576110310523 + "x": 7.58026817713618, + "y": 7.398244514513777 }, "isLocked": false, "linkedName": null @@ -46,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, + "waypointRelativePos": 0.55, "rotationDegrees": -172.5750748946377, "rotateFast": false } diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path index 3c528ed9..6a28feb8 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.35, - "rotationDegrees": -166.19572126875954, + "rotationDegrees": -169.16565991676583, "rotateFast": false }, { @@ -40,7 +40,19 @@ "rotateFast": false } ], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.45, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [ { "name": "intake", diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path index 186bbffd..18cfec5e 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.3644632694670795, - "y": 6.924282677394184 + "x": 1.4057878828008366, + "y": 6.876162812154685 }, "isLocked": false, "linkedName": "TopSpeaker" @@ -31,13 +31,18 @@ "rotationTargets": [ { "waypointRelativePos": 0.4, - "rotationDegrees": -178.6167416994117, - "rotateFast": false + "rotationDegrees": -172.45241554385072, + "rotateFast": true }, { "waypointRelativePos": 0.1, "rotationDegrees": -126.93135629519831, "rotateFast": false + }, + { + "waypointRelativePos": 0.25, + "rotationDegrees": -148.60777368547065, + "rotateFast": false } ], "constraintZones": [ @@ -46,8 +51,8 @@ "minWaypointRelativePos": 0.0, "maxWaypointRelativePos": 0.3, "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.0, + "maxVelocity": 3.0, + "maxAcceleration": 2.4, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 } diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path index 455f25eb..c993acd6 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLine.path @@ -52,7 +52,7 @@ }, { "waypointRelativePos": 1.0, - "rotationDegrees": 162.59131699825656, + "rotationDegrees": 149.60543545481846, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path index 0891c3a8..43172183 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToLowSpeaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 2.762602232759713, - "y": 5.0007196559800065 + "x": 1.1955855374727609, + "y": 3.9716340650452926 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path index 6aeabb17..80cb7c5c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.253713364561486, - "y": 3.356695043558523 + "x": 1.686696669274534, + "y": 2.3276094526238094 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path index ed52640d..0ff0eb2d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerCenterSteal.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.853477207302455, - "y": 3.1617790211618013 + "x": 2.286460512015503, + "y": 2.1326934302270875 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path index e99c0b58..2cda12a4 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerMidNote2Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.253713364561486, - "y": 4.8871568418217874 + "x": 1.686696669274534, + "y": 3.8580712508870736 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 3.2439636063446304, - "y": 4.975154907556637 + "x": 1.6769469110576778, + "y": 3.946069316621923 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path index b8fe82cd..a5e7dc9d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerPassAutoLine.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 2.7857249701523936, - "y": 4.087926909822731 + "x": 1.2187082748654414, + "y": 3.0588413188880175 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path index 2d475edf..f56e0458 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerQCenterLineN5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 2.8295096192682228, - "y": 4.961286589104821 + "x": 1.2624929239812706, + "y": 3.9322009981701074 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path index 9c981485..d170d21c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.3578788304297467, - "y": 3.2447157281181638 + "x": 1.7908621351427945, + "y": 2.21563013718345 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path index 1726b8a8..868ed51c 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToFarN4Return.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 2.7130601054934305, - "y": 4.429251530696795 + "x": 1.1460434102064783, + "y": 3.400165939762081 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -64,12 +64,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 3.0638847387666286, - "y": 4.3356982951572745 + "x": 1.4968680434796764, + "y": 3.3066127042225606 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path index 518a7f10..0fd6c18d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToHighNoteReturn.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.2439636063446304, - "y": 4.828658292520651 + "x": 1.6769469110576782, + "y": 3.7995727015859373 }, "isLocked": false, "linkedName": "LowSpeaker" @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 3.643703693235731, - "y": 4.341170381677846 + "x": 2.0766869979487783, + "y": 3.312084790743132 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index cadfd3d0..945ef2f8 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.2631456565492636, - "y": 5.1239219845468265 + "x": 1.6961289612623114, + "y": 4.094836393612113 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path index 5c681db9..093abf34 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToMiddleLowNote.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.6131149247760876, - "y": 2.8751932227649784 + "x": 2.046098229489136, + "y": 1.8461076318302645 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/MasterPIDTest.path b/src/main/deploy/pathplanner/paths/MasterPIDTest.path index 149fc7b0..a5c08fd1 100644 --- a/src/main/deploy/pathplanner/paths/MasterPIDTest.path +++ b/src/main/deploy/pathplanner/paths/MasterPIDTest.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.3, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": null, "nextControl": { - "x": 3.7734634597474326, - "y": 7.180210827658124 + "x": 2.2009903848867527, + "y": 6.151125236723411 }, "isLocked": false, "linkedName": "LowSpeaker" diff --git a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index 2fabe7ae..d73b14f0 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -104,7 +104,7 @@ "eventMarkers": [ { "name": "intake", - "waypointRelativePos": 0, + "waypointRelativePos": 1.25, "command": { "type": "parallel", "data": { @@ -118,6 +118,23 @@ ] } } + }, + { + "name": "understage", + "waypointRelativePos": 0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "RetractPivot" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path index ca3f8eff..caa86ac2 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterNotetoLaunch.path @@ -108,7 +108,7 @@ "eventMarkers": [ { "name": "retract", - "waypointRelativePos": 0.2, + "waypointRelativePos": 0.25, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index 176ded21..b2803d18 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.294543620426272, - "y": 5.496252587946771 + "x": 0.7275269251393202, + "y": 4.4671669970120575 }, "prevControl": { - "x": 2.946943194342229, - "y": 3.1662828509132805 + "x": 1.3799264990552769, + "y": 2.1371972599785667 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index eea883ff..a308ac81 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -416,18 +416,6 @@ public static Command subwooferLaunch() { } public static Command visionLaunch() { - - // return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED - // ? stopFeeder() - // .andThen( - // Commands.either(Commands.none(), index(), s.intakeSubsystem::feederSensorHasNote)) - // .andThen( - // new FullTargetCommand(s.launcherSubsystem, s.drivebaseSubsystem, controls) - // .until(isReadyToLaunch()) - // .andThen(new WaitCommand(FEEDER_DELAY)) - // .andThen(new FeederInCommand(s.intakeSubsystem).until(untilNoNote()))) - // : Commands.none()); - return (LAUNCHER_ENABLED && INTAKE_ENABLED && APRILTAGS_ENABLED ? stopFeeder() .andThen( @@ -460,7 +448,7 @@ public static Command stopLaunching() { public static Command setAngleRetracted() { return (LAUNCHER_ENABLED && INTAKE_ENABLED - ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, LauncherSubsystem.RETRACTED_ANGLE) + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, 262) : Commands.none()) .withName("Auto - SetPivotRetractedCommand"); } @@ -480,9 +468,10 @@ public static Command setAngleSubwoofer() { public static Command feederIn() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) - .andThen(Commands.waitSeconds(FEEDER_DELAY)) - .andThen(new FeederInCommand(s.intakeSubsystem)) - .andThen(Commands.waitSeconds(0.1)) + .andThen(Commands.waitUntil(hasNote())) + .andThen(new WaitCommand(FEEDER_DELAY)) + .andThen(new FeederInCommand(s.intakeSubsystem).until(untilFeederHasNoNote())) + .andThen(new WaitCommand(0.4)) : Commands.none()) .withName("Auto - FeedCommand"); } From bae5eaa00b6922f60e8916a6e80ff752a72a5169 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Tue, 9 Apr 2024 23:07:04 -0700 Subject: [PATCH 41/49] pathplanner things --- .pathplanner/settings.json | 4 +- ...terMiddleNoteLaunchToSecondNoteLaunch.path | 9 ++++- .../HighNoteLaunch2ToFarHighNoteToLaunch.path | 6 +-- .../HighSpeakerCenterLineNoteReturn.path | 10 ++--- ...LowLaunchNoteFourCenterLineUnderStage.path | 39 +++++++++++++------ .../pathplanner/paths/LowLaunchToMidNote.path | 8 ++-- .../pathplanner/paths/LowNoteToSpeaker.path | 2 +- .../paths/LowSpeakerToLowNote.path | 4 +- .../paths/MidNoteToBottomNote.path | 12 ++++-- .../paths/MidNoteToCenterMidNodeLaunch.path | 26 +++++++++---- .../paths/MidSpeakerToCenterMiddleNode.path | 5 --- .../paths/MiddleLowNoteToLowSpeaker.path | 15 ++++--- .../launchPosSecondCenterLineNoteLaunch.path | 7 +++- .../team2412/robot/util/auto/AutoLogic.java | 4 +- 14 files changed, 96 insertions(+), 55 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index f2b442cd..e6660ec6 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.84, - "robotLength": 0.84, + "robotWidth": 0.8382, + "robotLength": 0.8382, "holonomicMode": true, "pathFolders": [ "Auto Logic Paths" diff --git a/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path b/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path index ddb02157..5d0cd8b8 100644 --- a/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/CenterMiddleNoteLaunchToSecondNoteLaunch.path @@ -46,8 +46,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7, - "rotationDegrees": 155.83202134477403, + "waypointRelativePos": 0.8, + "rotationDegrees": 158.64, + "rotateFast": false + }, + { + "waypointRelativePos": 0.9, + "rotationDegrees": 158.64, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path index 869d7da3..25b9a887 100644 --- a/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path +++ b/src/main/deploy/pathplanner/paths/HighNoteLaunch2ToFarHighNoteToLaunch.path @@ -17,15 +17,15 @@ { "anchor": { "x": 8.150075399249896, - "y": 7.395762539272827 + "y": 7.45 }, "prevControl": { "x": 9.311036285890234, - "y": 7.390705607622041 + "y": 7.444943068349215 }, "nextControl": { "x": 7.58026817713618, - "y": 7.398244514513777 + "y": 7.452481975240951 }, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path index 6ffdc1b6..a1a59c2f 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerCenterLineNoteReturn.path @@ -17,15 +17,15 @@ { "anchor": { "x": 8.37, - "y": 7.45 + "y": 7.46 }, "prevControl": { "x": 8.73873589093028, - "y": 7.43760330109946 + "y": 7.44760330109946 }, "nextControl": { "x": 8.00126410906971, - "y": 7.46239669890054 + "y": 7.47239669890054 }, "isLocked": false, "linkedName": null @@ -47,12 +47,12 @@ "rotationTargets": [ { "waypointRelativePos": 0.75, - "rotationDegrees": -172.0, + "rotationDegrees": -174.0, "rotateFast": false }, { "waypointRelativePos": 1.2, - "rotationDegrees": -172.0, + "rotationDegrees": -174.0, "rotateFast": false }, { diff --git a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path index 6afe0186..96e1878b 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchNoteFourCenterLineUnderStage.path @@ -20,12 +20,12 @@ "y": 2.4481294676875076 }, "prevControl": { - "x": 8.337533809215634, - "y": 1.7307971969497182 + "x": 8.057894836551087, + "y": 1.8070758038267916 }, "nextControl": { - "x": 8.240937624732393, - "y": 3.5769077252440225 + "x": 8.534598822328235, + "y": 3.0693076055203994 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 5.321383214195 }, "prevControl": { - "x": 5.059835262527669, - "y": 3.9580536303929756 + "x": 5.641519404680388, + "y": 4.3469619676204285 }, "nextControl": null, "isLocked": false, @@ -46,18 +46,33 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.75, - "rotationDegrees": 146.44882381880868, + "waypointRelativePos": 0.7, + "rotationDegrees": 137.84146867175556, "rotateFast": false }, { - "waypointRelativePos": 1.65, + "waypointRelativePos": 1.6, "rotationDegrees": 176.0, "rotateFast": false }, { "waypointRelativePos": 1.0, - "rotationDegrees": 162.59131699825656, + "rotationDegrees": 145.1820620122512, + "rotateFast": false + }, + { + "waypointRelativePos": 0.8, + "rotationDegrees": 135.96440825571213, + "rotateFast": false + }, + { + "waypointRelativePos": 1.3, + "rotationDegrees": 157.65879132111158, + "rotateFast": false + }, + { + "waypointRelativePos": 1.45, + "rotationDegrees": 164.3979661370285, "rotateFast": false } ], @@ -98,8 +113,8 @@ } }, { - "name": "retract", - "waypointRelativePos": 0.45, + "name": "under stage", + "waypointRelativePos": 1.5, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path index f354569c..b2e1e6b1 100644 --- a/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path +++ b/src/main/deploy/pathplanner/paths/LowLaunchToMidNote.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.770693659609494, - "y": 5.1295050435105765 + "x": 2.7776195372993056, + "y": 4.946521435183591 }, "isLocked": false, "linkedName": "PodiumLaunch" @@ -20,8 +20,8 @@ "y": 5.55 }, "prevControl": { - "x": 2.843883094194073, - "y": 5.224806376319017 + "x": 2.877482177096141, + "y": 5.287229265078679 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path b/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path index c8cd71d7..5a8a4831 100644 --- a/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path +++ b/src/main/deploy/pathplanner/paths/LowNoteToSpeaker.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.6785413734247925, - "y": 4.938025867025607 + "y": 4.9380258670256065 }, "isLocked": false, "linkedName": "Podium" diff --git a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path index 945ef2f8..dd0fac3d 100644 --- a/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path +++ b/src/main/deploy/pathplanner/paths/LowSpeakerToLowNote.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.77, + "x": 2.7713929062062417, "y": 4.07539893464585 }, "prevControl": { - "x": 2.4513806186170926, + "x": 2.452773524823334, "y": 4.070802440768179 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path b/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path index 83f8c4da..3fac1700 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToBottomNote.path @@ -20,15 +20,21 @@ "y": 4.07539893464585 }, "prevControl": { - "x": 1.8241337089851442, - "y": 4.2187176915536355 + "x": 1.8408648194756145, + "y": 4.097305368134887 }, "nextControl": null, "isLocked": false, "linkedName": "Podium" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 163.6034850413123, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { diff --git a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path index d73b14f0..10d0f373 100644 --- a/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path +++ b/src/main/deploy/pathplanner/paths/MidNoteToCenterMidNodeLaunch.path @@ -48,16 +48,16 @@ }, { "anchor": { - "x": 5.799601695571071, - "y": 4.45 + "x": 5.785884780858597, + "y": 4.3346788339914255 }, "prevControl": { - "x": 6.602796124886758, - "y": 4.302924570674619 + "x": 6.538462814530829, + "y": 4.0178527359044205 }, "nextControl": { - "x": 5.008746918902289, - "y": 4.59481587716151 + "x": 5.045760238335085, + "y": 4.646262165439392 }, "isLocked": false, "linkedName": null @@ -68,8 +68,8 @@ "y": 5.78 }, "prevControl": { - "x": 4.628646173526595, - "y": 5.067161387102611 + "x": 4.405272674669961, + "y": 5.16154399760424 }, "nextControl": null, "isLocked": false, @@ -86,6 +86,16 @@ "waypointRelativePos": 1.2, "rotationDegrees": 180.0, "rotateFast": false + }, + { + "waypointRelativePos": 2.4, + "rotationDegrees": 171.2669358966649, + "rotateFast": false + }, + { + "waypointRelativePos": 2.85, + "rotationDegrees": 180.0, + "rotateFast": false } ], "constraintZones": [ diff --git a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path index 498e7c2b..eb62451b 100644 --- a/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path +++ b/src/main/deploy/pathplanner/paths/MidSpeakerToCenterMiddleNode.path @@ -81,11 +81,6 @@ "waypointRelativePos": 2.75, "rotationDegrees": 180.0, "rotateFast": false - }, - { - "waypointRelativePos": 0.5, - "rotationDegrees": 0, - "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path index b2803d18..e40e7ec4 100644 --- a/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path +++ b/src/main/deploy/pathplanner/paths/MiddleLowNoteToLowSpeaker.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.182999767708936, - "y": 1.5436283864020728 + "x": 5.156891888026078, + "y": 1.4098255030081461 }, "isLocked": false, "linkedName": "CenterLineN5" @@ -20,8 +20,8 @@ "y": 4.4671669970120575 }, "prevControl": { - "x": 1.3799264990552769, - "y": 2.1371972599785667 + "x": 1.7057565421060124, + "y": 2.2909664423919924 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.25, - "rotationDegrees": 175.66006175864302, + "rotationDegrees": -173.50545807332855, "rotateFast": false }, { @@ -43,6 +43,11 @@ "waypointRelativePos": 0.95, "rotationDegrees": 120.0, "rotateFast": true + }, + { + "waypointRelativePos": 0.5, + "rotationDegrees": -161.29004592647703, + "rotateFast": false } ], "constraintZones": [], diff --git a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path index d9eb246c..dfff0d38 100644 --- a/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path +++ b/src/main/deploy/pathplanner/paths/launchPosSecondCenterLineNoteLaunch.path @@ -47,7 +47,12 @@ "rotationTargets": [ { "waypointRelativePos": 0.85, - "rotationDegrees": 163.154898401479, + "rotationDegrees": 156.2896135289697, + "rotateFast": false + }, + { + "waypointRelativePos": 0.2, + "rotationDegrees": 155.22824361973127, "rotateFast": false } ], diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index a308ac81..dde16dfb 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -49,7 +49,7 @@ public class AutoLogic { // rpm to rev up launcher before launching public static final double REV_RPM = 2500; - public static final double STAGE_ANGLE = 247; + public static final double STAGE_ANGLE = 262; public static enum StartPosition { AMP_SIDE_SUBWOOFER( @@ -448,7 +448,7 @@ public static Command stopLaunching() { public static Command setAngleRetracted() { return (LAUNCHER_ENABLED && INTAKE_ENABLED - ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, 262) + ? new SetAngleLaunchCommand(s.launcherSubsystem, 0, STAGE_ANGLE) : Commands.none()) .withName("Auto - SetPivotRetractedCommand"); } From 06301f3b346e64b6555229c77f514b95649afdc8 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Thu, 11 Apr 2024 10:17:02 -0700 Subject: [PATCH 42/49] Cleanup code --- src/main/java/frc/team2412/robot/Controls.java | 4 ++-- src/main/java/frc/team2412/robot/Subsystems.java | 2 -- .../commands/diagnostic/IntakeDiagnosticCommand.java | 2 -- .../diagnostic/LauncherDiagnosticCommand.java | 12 ++++-------- .../robot/commands/drivebase/DriveToNoteCommand.java | 4 ++-- .../team2412/robot/commands/intake/AllInCommand.java | 3 --- .../robot/commands/launcher/FullTargetCommand.java | 3 +++ .../robot/subsystems/DrivebaseSubsystem.java | 1 + .../team2412/robot/subsystems/IntakeSubsystem.java | 10 +++++++--- .../team2412/robot/subsystems/LauncherSubsystem.java | 7 ++++--- src/main/java/frc/team2412/robot/util/AmpAlign.java | 2 +- .../team2412/robot/util/DynamicSendableChooser.java | 3 ++- .../frc/team2412/robot/util/LauncherDataLoader.java | 4 +++- src/main/java/frc/team2412/robot/util/TrapAlign.java | 5 ++++- .../java/frc/team2412/robot/util/auto/AutoLogic.java | 6 ++++-- .../java/frc/team2412/robot/util/auto/AutoPath.java | 2 +- 16 files changed, 38 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index b468e8bb..86b4b02b 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -58,7 +58,7 @@ public static class ControlConstants { private final Trigger launcherSubwooferPresetButton; private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; - private final Trigger launcherTrapPresetButton; + // private final Trigger launcherTrapPresetButton; private final Trigger launcherAmpAlignPresetButton; private final Trigger launcherLaunchButton; @@ -76,7 +76,7 @@ public Controls(Subsystems s) { launcherSubwooferPresetButton = codriveController.a(); launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - launcherTrapPresetButton = codriveController.start(); + // launcherTrapPresetButton = codriveController.start(); launcherAmpAlignPresetButton = driveController.y(); launcherLaunchButton = codriveController.rightBumper(); // intake controls (confirmed with driveteam) diff --git a/src/main/java/frc/team2412/robot/Subsystems.java b/src/main/java/frc/team2412/robot/Subsystems.java index 077cc3fb..adaf6840 100644 --- a/src/main/java/frc/team2412/robot/Subsystems.java +++ b/src/main/java/frc/team2412/robot/Subsystems.java @@ -12,8 +12,6 @@ public class Subsystems { public static class SubsystemConstants { - - private static final boolean IS_COMP = Robot.getInstance().isCompetition(); public static final boolean APRILTAGS_ENABLED = true; public static final boolean LIMELIGHT_ENABLED = false; public static final boolean CLIMB_ENABLED = false; diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java index 0373645d..79ac0317 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/IntakeDiagnosticCommand.java @@ -14,10 +14,8 @@ import frc.team2412.robot.subsystems.IntakeSubsystem; public class IntakeDiagnosticCommand extends SequentialCommandGroup { - private final IntakeSubsystem intakeSubsystem; public IntakeDiagnosticCommand(IntakeSubsystem intakeSubsystem) { - this.intakeSubsystem = intakeSubsystem; addCommands( new FeederInCommand(intakeSubsystem).withTimeout(0.5), new WaitCommand(0.5), diff --git a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java index fe254e5f..99e896a1 100644 --- a/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java +++ b/src/main/java/frc/team2412/robot/commands/diagnostic/LauncherDiagnosticCommand.java @@ -7,18 +7,14 @@ import frc.team2412.robot.subsystems.LauncherSubsystem; public class LauncherDiagnosticCommand extends SequentialCommandGroup { - private final LauncherSubsystem launcherSubsystem; - private final double Angle; - public LauncherDiagnosticCommand(LauncherSubsystem launcherSubsystem) { - this.launcherSubsystem = launcherSubsystem; - this.Angle = launcherSubsystem.getAngle(); + double angle = launcherSubsystem.getAngle(); addCommands( - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 10), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle + 10), new WaitCommand(2), - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle + 20), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle + 20), new WaitCommand(1), - new SetAngleLaunchCommand(launcherSubsystem, 500, Angle), + new SetAngleLaunchCommand(launcherSubsystem, 500, angle), new WaitCommand(5), new StopLauncherCommand(launcherSubsystem)); } diff --git a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java index 933117c2..9f7b71f7 100644 --- a/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java +++ b/src/main/java/frc/team2412/robot/commands/drivebase/DriveToNoteCommand.java @@ -31,8 +31,8 @@ public void execute() { * Units.inchesToMeters(limelightSubsystem.getDistanceFromTargetInches()), 0.0); Rotation2d turn = - new Rotation2d() - .fromDegrees(2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); + Rotation2d.fromDegrees( + 2 * INVERT_DRIVE_DIRECTION * limelightSubsystem.getHorizontalOffset()); drivebaseSubsystem.drive(move, turn, false); } } diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 456c7fe6..9f9d20a6 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -9,9 +9,6 @@ public class AllInCommand extends Command { private final IntakeSubsystem intakeSubsystem; private final Controls controls; - private boolean rumbledIntakeFront = false; - private boolean rumbledIntakeLeft = false; - private boolean rumbledIntakeRight = false; private boolean rumbledIndex = false; public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index fa26125a..31467698 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -28,7 +28,10 @@ public class FullTargetCommand extends Command { LauncherSubsystem.USE_THROUGHBORE ? "launcher_data_throughbore.csv" : "launcher_data_lamprey.csv")); + + @SuppressWarnings("UnusedVariable") private static final double YAW_TARGET_VIBRATION_TOLERANCE = 10; // degrees + private Translation2d SPEAKER_POSITION; private DrivebaseSubsystem drivebaseSubsystem; diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index dde3a25e..9c54c813 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -96,6 +96,7 @@ public class DrivebaseSubsystem extends SubsystemBase { private GenericEntry xWheelsEntry; private GenericEntry flipTranslationEntry; + @SuppressWarnings("StaticAssignmentInConstructor") public DrivebaseSubsystem() { initShuffleboard(); diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index cc208ed5..263bbd2a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -63,9 +63,15 @@ public class IntakeSubsystem extends SubsystemBase { // private final SparkLimitSwitch intakeFrontSensor; // debounce ! ! + @SuppressWarnings("UnusedVariable") private final Debouncer intakeFrontSensorDebouncer; + + @SuppressWarnings("UnusedVariable") private final Debouncer intakeRightSensorDebouncer; + + @SuppressWarnings("UnusedVariable") private final Debouncer intakeLeftSensorDebouncer; + private final Debouncer feederSensorDebouncer; private final Debouncer feederSensorIRDebouncer; @@ -89,8 +95,6 @@ public class IntakeSubsystem extends SubsystemBase { // reject override private GenericEntry rejectOverride; - private boolean feederSensorSignal; - public IntakeSubsystem() { intakeMotorFront = @@ -329,7 +333,7 @@ public boolean debouncedIntakeFrontSensor() { // return true; // } return false; - // wreturn intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); + // return intakeFrontSensorDebouncer.calculate(intakeFrontSensor.isPressed()); } public boolean debouncedIntakeLeftSensor() { diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 374e7ea9..e4a77068 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -7,7 +7,6 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkAbsoluteEncoder.Type; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; @@ -142,8 +141,10 @@ public LauncherSubsystem() { // encoders launcherTopEncoder = launcherTopMotor.getEncoder(); launcherBottomEncoder = launcherBottomMotor.getEncoder(); - launcherAngleEncoder = launcherAngleOneMotor.getAbsoluteEncoder(Type.kDutyCycle); - launcherAngleThroughboreEncoder = launcherAngleTwoMotor.getAbsoluteEncoder(Type.kDutyCycle); + launcherAngleEncoder = + launcherAngleOneMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); + launcherAngleThroughboreEncoder = + launcherAngleTwoMotor.getAbsoluteEncoder(SparkAbsoluteEncoder.Type.kDutyCycle); manualAngleSetpoint = launcherAngleEncoder.getPosition(); // PID controllers diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index b3add876..e013722c 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -37,7 +37,7 @@ private static Command ampAlign( isBlue = false; } // figures out which amp to go to - Pose2d ampPose = (isBlue) ? BLUE_AMP_POSES : RED_AMP_POSES; + Pose2d ampPose = isBlue ? BLUE_AMP_POSES : RED_AMP_POSES; // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, ampPose); // this is flipped diff --git a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java index e0c2ef47..1c63382c 100644 --- a/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java +++ b/src/main/java/frc/team2412/robot/util/DynamicSendableChooser.java @@ -64,7 +64,7 @@ public void addOption(String name, T object) { /** * Removes the given object from the list of options. * - * @param object the option + * @param name the name of the option */ public void removeOption(String name) { m_map.remove(name); @@ -115,6 +115,7 @@ public T getSelected() { * * @param listener The function to call that accepts the new value */ + @SuppressWarnings("LockNotBeforeTry") // Assigning to m_listener shouldn't error public void onChange(Consumer listener) { requireNonNullParam(listener, "listener", "onChange"); m_mutex.lock(); diff --git a/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java b/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java index d35cc748..fc3a09a0 100644 --- a/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java +++ b/src/main/java/frc/team2412/robot/util/LauncherDataLoader.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.math.interpolation.InverseInterpolator; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import frc.team2412.robot.Robot; import java.io.BufferedReader; @@ -40,7 +41,8 @@ public static InterpolatingTreeMap fromCSV(Path path) debugWriter.append(debugPrefix + msg); debugWriter.newLine(); } catch (IOException e) { - e.printStackTrace(); + DriverStation.reportWarning( + "Error writing to CSV interpreter log: " + e.getMessage(), e.getStackTrace()); } }; diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index a8109423..eeaa138d 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -46,7 +46,7 @@ private static Command trapAlign(DrivebaseSubsystem drivebaseSubsystem) { isBlue = false; } // figures out which trap to go to - Pose2d trapPose = robotPose.nearest(List.of((isBlue) ? BLUE_TRAP_POSES : RED_TRAP_POSES)); + Pose2d trapPose = robotPose.nearest(List.of(isBlue ? BLUE_TRAP_POSES : RED_TRAP_POSES)); // sets the point for the path to go to List bezierPoints = PathPlannerPath.bezierFromPoses(robotPose, trapPose); // this is flipped @@ -74,7 +74,10 @@ public static Command trapPreset( private static class AlignCommand extends Command { private final DrivebaseSubsystem drivebaseSubsystem; + + @SuppressWarnings("UnusedVariable") private final LauncherSubsystem launcherSubsystem; + private Command trapCommand = null; public AlignCommand( diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index dde16dfb..49519287 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -51,6 +51,8 @@ public class AutoLogic { public static final double REV_RPM = 2500; public static final double STAGE_ANGLE = 262; + // ErrorProne doesn't know Pose2d's are immutable + @SuppressWarnings("ImmutableEnumChecker") public static enum StartPosition { AMP_SIDE_SUBWOOFER( "Amp Side Subwoofer", new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), @@ -237,7 +239,7 @@ public static void registerCommands() { /** * Takes a PathPlanner path and returns it as a command. * - * @param pathName + * @param pathName the PathPlanner name for the path * @return follow path command */ public static Command getAutoCommand(String pathName) { @@ -352,7 +354,7 @@ public static BooleanSupplier isReadyToLaunch() { // Should be able to launch if: // Launcher is at the correct angle and flywheel rpm // Note is finished indexing (intake all in command is finished) - return (INTAKE_ENABLED & LAUNCHER_ENABLED + return (INTAKE_ENABLED && LAUNCHER_ENABLED ? () -> (s.launcherSubsystem.isAtAngle() && s.launcherSubsystem.isAtSpeed() diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java index a739ed25..eabff37d 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoPath.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoPath.java @@ -71,7 +71,7 @@ public boolean isVision() { * Checks the x, y, and rotation of the auto's starting position and compares it with the expected * starting position, returning true if it is considered close enough to be the same. * - * @param expectedStartPosition + * @param expectedStartPosition The expected start position * @return if it is matching */ public boolean matchesStartPosition(StartPosition expectedStartPosition) { From b31783c18522cb435d65aaeead97daf4c8c7ba92 Mon Sep 17 00:00:00 2001 From: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com> Date: Fri, 12 Apr 2024 10:55:54 -0700 Subject: [PATCH 43/49] Remove YAW_TARGET_VIBRATION_TOLERANCE --- .../team2412/robot/commands/launcher/FullTargetCommand.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java index 31467698..11bcd66e 100644 --- a/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java +++ b/src/main/java/frc/team2412/robot/commands/launcher/FullTargetCommand.java @@ -29,9 +29,6 @@ public class FullTargetCommand extends Command { ? "launcher_data_throughbore.csv" : "launcher_data_lamprey.csv")); - @SuppressWarnings("UnusedVariable") - private static final double YAW_TARGET_VIBRATION_TOLERANCE = 10; // degrees - private Translation2d SPEAKER_POSITION; private DrivebaseSubsystem drivebaseSubsystem; From 7ca07156487f4847d180ff00c1ddf4d4b765e2b5 Mon Sep 17 00:00:00 2001 From: jbko6 <58869582+jbko6@users.noreply.github.com> Date: Sun, 14 Apr 2024 21:49:33 -0700 Subject: [PATCH 44/49] address comments --- src/main/java/swervelib/motors/SparkFlexSwerve.java | 2 +- .../java/swervelib/motors/SparkMaxBrushedMotorSwerve.java | 2 +- src/main/java/swervelib/motors/SparkMaxSwerve.java | 2 +- src/main/java/swervelib/motors/TalonFXSwerve.java | 8 -------- src/main/java/swervelib/motors/TalonSRXSwerve.java | 1 + 5 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/main/java/swervelib/motors/SparkFlexSwerve.java b/src/main/java/swervelib/motors/SparkFlexSwerve.java index 13121657..7bb212f1 100644 --- a/src/main/java/swervelib/motors/SparkFlexSwerve.java +++ b/src/main/java/swervelib/motors/SparkFlexSwerve.java @@ -115,7 +115,7 @@ public void setVoltageCompensation(double nominalVoltage) { */ @Override public void setCurrentLimit(int statorLimit, int supplyLimit) { - configureSparkFlex(() -> motor.setSmartCurrentLimit(statorLimit)); + configureSparkFlex(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 0ba1750d..0a8a0bcf 100644 --- a/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -162,7 +162,7 @@ public void setVoltageCompensation(double nominalVoltage) { */ @Override public void setCurrentLimit(int statorLimit, int supplyLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(statorLimit)); + configureSparkMax(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/SparkMaxSwerve.java b/src/main/java/swervelib/motors/SparkMaxSwerve.java index 496590c1..853a6b08 100644 --- a/src/main/java/swervelib/motors/SparkMaxSwerve.java +++ b/src/main/java/swervelib/motors/SparkMaxSwerve.java @@ -96,7 +96,7 @@ public void setVoltageCompensation(double nominalVoltage) { */ @Override public void setCurrentLimit(int statorLimit, int supplyLimit) { - configureSparkMax(() -> motor.setSmartCurrentLimit(statorLimit)); + configureSparkMax(() -> motor.setSmartCurrentLimit(supplyLimit)); } /** diff --git a/src/main/java/swervelib/motors/TalonFXSwerve.java b/src/main/java/swervelib/motors/TalonFXSwerve.java index 019fc863..dd052981 100644 --- a/src/main/java/swervelib/motors/TalonFXSwerve.java +++ b/src/main/java/swervelib/motors/TalonFXSwerve.java @@ -412,14 +412,6 @@ public void setCurrentLimit(int statorLimit, int supplyLimit) { .withStatorCurrentLimitEnable(true) .withSupplyCurrentLimit(supplyLimit) .withSupplyCurrentLimitEnable(true)); - System.out.println( - "Configured drivebase motor " - + motor.getDeviceID() - + " with stator limit of " - + statorLimit - + " amps and supply limit of " - + supplyLimit - + " amps."); } /** diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 36c6320e..03538f80 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -5,6 +5,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.Timer; From be717d29e95c6e82da70e37f5c7f51e9df9040d2 Mon Sep 17 00:00:00 2001 From: jbko6 <58869582+jbko6@users.noreply.github.com> Date: Sun, 14 Apr 2024 21:55:09 -0700 Subject: [PATCH 45/49] spotless --- src/main/java/swervelib/motors/TalonSRXSwerve.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/swervelib/motors/TalonSRXSwerve.java b/src/main/java/swervelib/motors/TalonSRXSwerve.java index 03538f80..36c6320e 100644 --- a/src/main/java/swervelib/motors/TalonSRXSwerve.java +++ b/src/main/java/swervelib/motors/TalonSRXSwerve.java @@ -5,7 +5,6 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; -import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.Timer; From 0e18e3b85a49408b43690b19801b1075fa43c370 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 15 Apr 2024 15:01:25 -0700 Subject: [PATCH 46/49] spotless after mergeconflict --- src/main/java/frc/team2412/robot/Controls.java | 4 +--- src/main/java/frc/team2412/robot/util/AmpAlign.java | 4 ---- 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index b44e6021..f5d5c2f8 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -30,8 +30,6 @@ import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.AmpAlign; -import frc.team2412.robot.util.TrapAlign; - public class Controls { public static class ControlConstants { @@ -229,7 +227,7 @@ private void bindLauncherControls() { s.launcherSubsystem, LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - + // launcherTrapPresetButton.onTrue( // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); launcherAmpAlignPresetButton.onTrue( diff --git a/src/main/java/frc/team2412/robot/util/AmpAlign.java b/src/main/java/frc/team2412/robot/util/AmpAlign.java index 86c99628..c97f9885 100644 --- a/src/main/java/frc/team2412/robot/util/AmpAlign.java +++ b/src/main/java/frc/team2412/robot/util/AmpAlign.java @@ -14,7 +14,6 @@ import frc.team2412.robot.commands.launcher.SetAngleAmpLaunchCommand; import frc.team2412.robot.subsystems.DrivebaseSubsystem; import frc.team2412.robot.subsystems.LauncherSubsystem; - import java.util.List; public class AmpAlign { @@ -59,7 +58,6 @@ private static Command ampAlign( path = path.flipPath(); } - return AutoBuilder.followPath(path) .deadlineWith( Commands.waitUntil( @@ -80,7 +78,6 @@ private static Command ampAlign( public static Command ampPreset( DrivebaseSubsystem drivebaseSubsystem, LauncherSubsystem launcherSubsystem) { return new AlignCommand(drivebaseSubsystem, launcherSubsystem); - } private static class AlignCommand extends Command { @@ -93,7 +90,6 @@ public AlignCommand( this.drivebaseSubsystem = drivebaseSubsystem; this.launcherSubsystem = launcherSubsystem; addRequirements(drivebaseSubsystem, launcherSubsystem); - } @Override From 55773452bc3bcf6265d03fc59e3038e10565cc1d Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 15 Apr 2024 15:12:09 -0700 Subject: [PATCH 47/49] address pr commments --- .../java/frc/team2412/robot/commands/intake/AllInCommand.java | 2 +- .../team2412/robot/commands/intake/AllInNoSensorCommand.java | 2 +- .../robot/commands/intake/AllInSensorOverrideCommand.java | 2 +- .../frc/team2412/robot/commands/intake/FeederInCommand.java | 2 +- .../java/frc/team2412/robot/subsystems/IntakeSubsystem.java | 2 +- .../java/frc/team2412/robot/subsystems/LauncherSubsystem.java | 4 ++-- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java index 456c7fe6..bfa81b51 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInCommand.java @@ -24,7 +24,7 @@ public AllInCommand(IntakeSubsystem intakeSubsystem, Controls controls) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java index 17e459e9..9486c837 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInNoSensorCommand.java @@ -15,7 +15,7 @@ public AllInNoSensorCommand(IntakeSubsystem intakeSubsystem) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java b/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java index d1de3762..3b172efb 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/AllInSensorOverrideCommand.java @@ -15,7 +15,7 @@ public AllInSensorOverrideCommand(IntakeSubsystem intakeSubsystem) { public void initialize() { intakeSubsystem.intakeIn(); intakeSubsystem.indexIn(); - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java b/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java index 38ac9166..e789a5dd 100644 --- a/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java +++ b/src/main/java/frc/team2412/robot/commands/intake/FeederInCommand.java @@ -13,7 +13,7 @@ public FeederInCommand(IntakeSubsystem intakeSubsystem) { @Override public void initialize() { - intakeSubsystem.feederIn(); + intakeSubsystem.feedUntilNoteLaunched(); } @Override diff --git a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java index cc208ed5..fcd2213e 100644 --- a/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/IntakeSubsystem.java @@ -276,7 +276,7 @@ public void indexStop() { } // feeder methods - public void feederIn() { + public void feedUntilNoteLaunched() { feederMotor.set(setFeederInSpeedEntry.getDouble(FEEDER_IN_SPEED)); } diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 374e7ea9..52ada726 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -348,9 +348,9 @@ public void setAngleManual(double joystickInput, boolean powerControl, boolean i this.ignoreLimits = ignoreLimits; if (powerControl || ignoreLimits) { if (!USE_THROUGHBORE) { - launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); + launcherAngleOneMotor.set(ignoreLimits ? joystickInput * 0.5 : joystickInput); } else { - launcherAngleTwoMotor.set(ignoreLimits ? joystickInput * 0.3 : joystickInput); + launcherAngleTwoMotor.set(ignoreLimits ? joystickInput * 0.5 : joystickInput); } manualAngleSetpoint = MathUtil.clamp( diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index dde16dfb..efd20097 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -465,7 +465,7 @@ public static Command setAngleSubwoofer() { .withName("Auto - SetPivotSubwooferCommand"); } - public static Command feederIn() { + public static Command feedUntilNoteLaunched() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) .andThen(Commands.waitUntil(hasNote())) From 63f219b0762c3d0c72f3bf96b0956af5f72715c1 Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Mon, 15 Apr 2024 16:17:30 -0700 Subject: [PATCH 48/49] address pr comments pt. 2 --- .../frc/team2412/robot/subsystems/LauncherSubsystem.java | 2 +- src/main/java/frc/team2412/robot/util/auto/AutoLogic.java | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index ebbd4197..8b55a939 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -519,7 +519,7 @@ public void zeroRelativeEncoder(double pivotAngle) { double offset = pivotAngle - currentRelativePosition; if (relativeEncoderStartPosition.isEmpty() - || Math.abs(relativeEncoderStartPosition.orElse(0.0) + offset) > OFFSET_SYNCING_TOLERANCE) { + || Math.abs(relativeEncoderStartPosition.orElse(0.0) - offset) > OFFSET_SYNCING_TOLERANCE) { relativeEncoderStartPosition = Optional.of(offset); } } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index e7526f8b..9df1141e 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -51,8 +51,6 @@ public class AutoLogic { public static final double REV_RPM = 2500; public static final double STAGE_ANGLE = 262; - // ErrorProne doesn't know Pose2d's are immutable - @SuppressWarnings("ImmutableEnumChecker") public static enum StartPosition { AMP_SIDE_SUBWOOFER( "Amp Side Subwoofer", new Pose2d(0.73, 6.62, new Rotation2d(Units.degreesToRadians(-120)))), @@ -239,7 +237,7 @@ public static void registerCommands() { /** * Takes a PathPlanner path and returns it as a command. * - * @param pathName the PathPlanner name for the path + * @param pathName * @return follow path command */ public static Command getAutoCommand(String pathName) { @@ -354,7 +352,7 @@ public static BooleanSupplier isReadyToLaunch() { // Should be able to launch if: // Launcher is at the correct angle and flywheel rpm // Note is finished indexing (intake all in command is finished) - return (INTAKE_ENABLED && LAUNCHER_ENABLED + return (INTAKE_ENABLED & LAUNCHER_ENABLED ? () -> (s.launcherSubsystem.isAtAngle() && s.launcherSubsystem.isAtSpeed() @@ -475,7 +473,7 @@ public static Command feedUntilNoteLaunched() { .andThen(new FeederInCommand(s.intakeSubsystem).until(untilFeederHasNoNote())) .andThen(new WaitCommand(0.4)) : Commands.none()) - .withName("Auto - FeedCommand"); + .withName("Auto - FeedUntilNoteLaunchedCommand"); } public static Command noteSteal() { From 6ef31da1009acec4dc314df3da73c7282922af3f Mon Sep 17 00:00:00 2001 From: kirbt <91921906+kirbt@users.noreply.github.com> Date: Wed, 17 Apr 2024 19:46:27 -0700 Subject: [PATCH 49/49] checker is real --- .../team2412/robot/subsystems/LauncherSubsystem.java | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 374e7ea9..e05d4240 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -279,12 +279,14 @@ public double convertPivotRotationsToEncoderRotations(double pivotRotations) { // returns the degrees of the angle of the launcher public double getAngle() { - // get position returns a double in the form of rotations + return Units.rotationsToDegrees(getPosition()); + } + + public double getPosition() { if (!USE_THROUGHBORE) { - return Units.rotationsToDegrees(launcherAngleEncoder.getPosition()); + return launcherAngleEncoder.getPosition(); } - return Units.rotationsToDegrees( - convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition())); + return convertEncoderRotationsToPivotRotations(launcherAngleThroughboreEncoder.getPosition()); } /** @@ -556,7 +558,7 @@ public void periodic() { false); } - if (launcherAngleEncoder.getPosition() + if (getPosition() >= (USE_THROUGHBORE ? PIVOT_SOFTSTOP_FORWARD_THROUGHBORE : PIVOT_SOFTSTOP_FORWARD) + PIVOT_DISABLE_OFFSET || launcherAngleEncoder.getPosition()