diff --git a/README.md b/README.md index 86f52fce..9ae71f9c 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ ### Mechanisms - Swerve -- L1 Climb + Vertically Expanding Hopper +- L1 Climb + ~~Vertically Expanding Hopper~~ - Handoff -> Turret + Hooded Shooter - Spindexer -- Intake + Horizontally Expanding Hopper +- Intake + Passive Horizontally Expanding Hopper - Vision with 3 LimeLight 4s diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 8411c15c..b7124041 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -11,7 +11,9 @@ import com.stuypulse.robot.commands.BuzzController; import com.stuypulse.robot.commands.auton.DoNothingAuton; +import com.stuypulse.robot.commands.climberhopper.ClimberDown; import com.stuypulse.robot.commands.climberhopper.ClimberHopperDefaultCommand; +import com.stuypulse.robot.commands.climberhopper.ClimberUp; import com.stuypulse.robot.commands.handoff.HandoffReverse; import com.stuypulse.robot.commands.handoff.HandoffRun; import com.stuypulse.robot.commands.handoff.HandoffStop; @@ -22,9 +24,9 @@ import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterRightCorner; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterShoot; import com.stuypulse.robot.commands.hoodedshooter.HoodedShooterStow; -import com.stuypulse.robot.commands.intake.IntakeIntake; -import com.stuypulse.robot.commands.intake.IntakeOutake; -import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.intake.IntakeDeploy; +import com.stuypulse.robot.commands.intake.IntakeStopRollers; +import com.stuypulse.robot.commands.intake.IntakeStow; import com.stuypulse.robot.commands.spindexer.SpindexerRun; import com.stuypulse.robot.commands.spindexer.SpindexerStop; import com.stuypulse.robot.commands.swerve.SwerveClimbAlign; @@ -73,7 +75,6 @@ public interface EnabledSubsystems { // Gamepads public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER); - public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR); // Subsystem private final ClimberHopper climberHopper = ClimberHopper.getInstance(); @@ -115,18 +116,22 @@ private void configureDefaultCommands() { /***************/ private void configureButtonBindings() { + // Intake Up and Off + driver.getLeftTriggerButton() + .onTrue(new IntakeStow()); - driver.getDPadDown() - .onTrue(new TurretIdle()) - .onTrue(new TurretSeed()); + // Intake Down and On + driver.getRightTriggerButton() + .onTrue(new IntakeDeploy()); + // Reset Heading driver.getDPadUp() .onTrue(new SwerveResetHeading()); - // SCORING ROUTINE + // Scoring Routine using Interpolation Settings driver.getTopButton() - .whileTrue(new TurretShoot() - .alongWith(new HoodedShooterShoot()) + .whileTrue(new HoodedShooterInterpolation() + .alongWith(new TurretShoot()) .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) @@ -135,39 +140,28 @@ private void configureButtonBindings() { .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); - driver.getTopButton() - .whileTrue(new TurretShoot() - .alongWith(new HoodedShooterInterpolation()) + // Ferry Routine using Interpolation Settings + driver.getBottomButton() + .onTrue(new HoodedShooterFerry() + .alongWith(new TurretFerry()) .alongWith(new WaitUntilCommand(() -> hoodedShooter.bothAtTolerance())) .andThen(new HandoffRun().onlyIf(() -> hoodedShooter.bothAtTolerance()) .alongWith(new WaitUntilCommand(() -> handoff.atTolerance())) - .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance())))) + .andThen(new SpindexerRun().onlyIf(() -> handoff.atTolerance() && hoodedShooter.bothAtTolerance()))) + ) .onFalse(new SpindexerStop() .alongWith(new HoodedShooterStow()) .alongWith(new HandoffStop())); - // driver.getDPadDown() - // .onTrue(new HoodedShooterShoot()) - // .onFalse(new HoodedShooterStow()); - - // driver.getDPadUp() - // .onTrue(new HoodedShooterFerry()) - // .onFalse(new HoodedShooterStow()); - - // driver.getDPadUp().whileTrue(new HoodedShooterShoot() - // .alongWith(new WaitUntilCommand(() -> hoodedShooter.isShooterAtTolerance()) - // .andThen(new FeederFeed()))) - // .onFalse(new HoodedShooterStow() - // .alongWith(new FeederStop())); - //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ //-------------------------------------------------------------------------------------------------------------------------\\ + /* // Climb Align driver.getTopButton() - .whileTrue(new SwerveClimbAlign()); + .whileTrue(new SwerveClimbAlign().alongWith(new ClimberUp())); // Left Corner Shoot driver.getLeftButton() @@ -217,21 +211,22 @@ private void configureButtonBindings() { new HandoffStop())) ); - // Intake On + // Intake Up and Off driver.getLeftTriggerButton() - .onTrue(new IntakeIntake()); + .onTrue(new IntakeStow()); - // Intake Off + // Intake Down and On driver.getRightTriggerButton() - .onTrue(new IntakeStop()); + .onTrue(new IntakeDeploy()); // Climb Down Placeholder driver.getLeftBumper() - .onTrue(new BuzzController(driver)); + .onTrue(new BuzzController(driver).alongWith(new ClimberDown())); // Climb Up Placeholder driver.getRightBumper() - .onTrue(new BuzzController(driver)); + .onTrue(new BuzzController(driver)) + .whileTrue(new ClimberUp()); // Reset Heading driver.getDPadUp() @@ -268,19 +263,7 @@ private void configureButtonBindings() { new SpindexerRun().alongWith( new HandoffStop())) ); - - // Unjam - driver.getDPadDown() - .whileTrue( - new HoodedShooterReverse().alongWith( - new HandoffReverse().alongWith( - new IntakeOutake()))) - .onFalse( - new HoodedShooterStow().alongWith( - new SpindexerRun().alongWith( - new HandoffStop().alongWith( - new IntakeStop()))) - ); + */ } /**************/ @@ -305,35 +288,35 @@ public void configureSysids() { // autonChooser.addOption("SysID Module Rotation Quasi Forwards", swerve.sysIdRotQuasi(Direction.kForward)); // autonChooser.addOption("SysID Module Rotation Quasi Backwards", swerve.sysIdRotQuasi(Direction.kReverse)); - SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); - autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); - autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); - autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine(); - autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); - - SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); - autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse)); - autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); - autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); + // SysIdRoutine shooterSysId = shooter.getShooterSysIdRoutine(); + // autonChooser.addOption("SysID Shooter Dynamic Forward", shooterSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Shooter Dynamic Backwards", shooterSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Shooter Quasi Forwards", shooterSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Shooter Quasi Backwards", shooterSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine hoodSysId = hood.getHoodSysIdRoutine(); + // autonChooser.addOption("SysID Hood Dynamic Forward", hoodSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Hood Dynamic Backwards", hoodSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Hood Quasi Forwards", hoodSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Hood Quasi Backwards", hoodSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine intakePivotSysId = intake.getPivotSysIdRoutine(); + // autonChooser.addOption("SysID Intake Pivot Dynamic Forward", intakePivotSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Intake Pivot Dynamic Backwards", intakePivotSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Intake Pivot Quasi Forwards", intakePivotSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Intake Pivot Quasi Backwards", intakePivotSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine spindexerSysId = spindexer.getSysIdRoutine(); + // autonChooser.addOption("SysID Spindexer Dynamic Forward", spindexerSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Spindexer Dynamic Backwards", spindexerSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Spindexer Quasi Forwards", spindexerSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Spindexer Quasi Backwards", spindexerSysId.quasistatic(Direction.kReverse)); + + // SysIdRoutine handoffSysId = handoff.getSysIdRoutine(); + // autonChooser.addOption("SysID Handoff Forward", handoffSysId.dynamic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.dynamic(Direction.kReverse)); + // autonChooser.addOption("SysID Handoff Forwards", handoffSysId.quasistatic(Direction.kForward)); + // autonChooser.addOption("SysID Handoff Backwards", handoffSysId.quasistatic(Direction.kReverse)); } diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java index ccd03002..434b1b25 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperDefaultCommand.java @@ -5,26 +5,22 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class ClimberHopperDefaultCommand extends Command { private final ClimberHopper climberHopper; - private final CommandSwerveDrivetrain swerve; - private Pose2d pose; - private boolean flag = false; // To prevent repeated stalling under trench + // private final CommandSwerveDrivetrain swerve; + // private Pose2d pose; + // private boolean flag = false; // To prevent repeated stalling under trench public ClimberHopperDefaultCommand() { climberHopper = ClimberHopper.getInstance(); - swerve = CommandSwerveDrivetrain.getInstance(); - pose = swerve.getPose(); + // swerve = CommandSwerveDrivetrain.getInstance(); + // pose = swerve.getPose(); addRequirements(climberHopper); @@ -34,43 +30,49 @@ public ClimberHopperDefaultCommand() { public void execute() { // === Robot Position Logic === // Reminder from driver's perspective, positive X to the opposite alliance and positive Y points to the left. - pose = swerve.getPose(); + // pose = swerve.getPose(); - boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); + // boolean isBetweenRightTrenchesY = Field.NearRightTrench.rightEdge.getY() < pose.getY() && Field.NearRightTrench.leftEdge.getY() > pose.getY(); - boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); + // boolean isBetweenLeftTrenchesY = Field.NearLeftTrench.rightEdge.getY() < pose.getY() && Field.NearLeftTrench.leftEdge.getY() > pose.getY(); - boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + // boolean isCloseToNearTrenchesX = Math.abs(pose.getX() - Field.NearRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; + // boolean isCloseToFarTrenchesX = Math.abs(pose.getX() - Field.FarRightTrench.rightEdge.getX()) < Field.trenchXTolerance; - boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance + // boolean isUnderTrench = (isBetweenRightTrenchesY || isBetweenLeftTrenchesY) && (isCloseToNearTrenchesX || isCloseToFarTrenchesX); // Far X tolerance - // === Climber Position and State Logic === - boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; - - boolean stalledByBalls = climberHopper.getStalling() && inDownState; - // boolean stalledByBalls = true; - - if (isUnderTrench) { - if (!stalledByBalls && !flag) { - climberHopper.setState(ClimberHopperState.HOPPER_DOWN); - } else { - climberHopper.setState(ClimberHopperState.HOPPER_UP); - // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! - flag = true; // prevent hopper from going back down while still under trench with too many balls - } - } else { // If not under trench... - if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { - // Set the hopper up - climberHopper.setState(ClimberHopperState.HOPPER_UP); - } + // // === Climber Position and State Logic === + // boolean inDownState = climberHopper.getState() == ClimberHopperState.CLIMBER_DOWN || climberHopper.getState() == ClimberHopperState.HOPPER_DOWN; + + // boolean stalledByBalls = climberHopper.getStalling() && inDownState; + // // boolean stalledByBalls = true; + + // if (isUnderTrench) { + // if (!stalledByBalls && !flag) { + // climberHopper.setState(ClimberHopperState.HOPPER_DOWN); + // } else { + // climberHopper.setState(ClimberHopperState.HOPPER_UP); + // // TODO: Flash LEDs or have Controller buzz when this happens. Also we need to somehow log this state!!! + // flag = true; // prevent hopper from going back down while still under trench with too many balls + // } + // } else { // If not under trench... + // if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { + // // Set the hopper up + // climberHopper.setState(ClimberHopperState.HOPPER_UP); + // } + // } + + // if (!isUnderTrench) { + // flag = false; + // } + + // SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); + + // !!! AFTER ABANDONING VERTICAL EXPANSION: + if (climberHopper.getState() != ClimberHopperState.CLIMBER_DOWN) { + // Set the hopper down + climberHopper.setState(ClimberHopperState.HOPPER_DOWN); } - - if (!isUnderTrench) { - flag = false; - } - - SmartDashboard.putBoolean("ClimberHopper/UnderTrench", isUnderTrench); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java index 18bfb424..fd669f38 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperReset.java @@ -5,19 +5,42 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; +// import java.util.Optional; + +// import com.stuypulse.robot.Robot; +// import com.stuypulse.robot.constants.Settings; +// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper; import edu.wpi.first.wpilibj2.command.Command; public class ClimberHopperReset extends Command { - private final ClimberHopper climberHopper; + // private final ClimberHopper climberHopper; - public ClimberHopperReset() { - climberHopper = ClimberHopper.getInstance(); - } + // public ClimberHopperReset() { + // climberHopper = ClimberHopper.getInstance(); + // } + + // @Override + // public void initialize() { + // climberHopper.setVoltageOverride(Optional.of(Settings.ClimberHopper.MOTOR_VOLTAGE)); + // } + + // @Override + // public boolean isFinished() { + // if (climberHopper.getStalling()) { + // climberHopper.resetPostionUpper(); + // return true; + // } + // return false; + // } + + // @Override + // public void end(boolean interrupted) { + // climberHopper.setVoltageOverride(Optional.empty()); + // } - @Override - public void execute() { + // @Override + // public void execute() { - } + // } } diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java index 6849ef97..85d8d6cd 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/ClimberHopperSetState.java @@ -8,9 +8,9 @@ import com.stuypulse.robot.subsystems.climberhopper.*; import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -public class ClimberHopperSetState extends Command { +public class ClimberHopperSetState extends InstantCommand { private final ClimberHopper climberHopper = ClimberHopper.getInstance(); private final ClimberHopperState state; diff --git a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java index d4044578..03fca2a9 100644 --- a/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java +++ b/src/main/java/com/stuypulse/robot/commands/climberhopper/HopperUp.java @@ -5,10 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.climberhopper; -import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; +// import com.stuypulse.robot.subsystems.climberhopper.ClimberHopper.ClimberHopperState; -public class HopperUp extends ClimberHopperSetState { - public HopperUp() { - super(ClimberHopperState.HOPPER_UP); - } -} \ No newline at end of file +// public class HopperUp extends ClimberHopperSetState { +// public HopperUp() { +// super(ClimberHopperState.HOPPER_UP); +// } +// } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java similarity index 59% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java index 2e89be86..de0fac60 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeIntake.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeploy.java @@ -5,13 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeIntake extends IntakeSetState { - /** - * Sets the State of the Intake to Intaking - */ - public IntakeIntake() { - super(IntakeState.INTAKE); +public class IntakeDeploy extends IntakeSetState { + public IntakeDeploy() { + super(PivotState.DEPLOYED, RollerState.INTAKE); } } diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java index f26bff52..cde76db4 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeSetState.java @@ -6,23 +6,48 @@ package com.stuypulse.robot.commands.intake; import com.stuypulse.robot.subsystems.intake.Intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; import edu.wpi.first.wpilibj2.command.InstantCommand; +import java.util.Optional; + public class IntakeSetState extends InstantCommand { - private Intake intake; - private IntakeState intakeState; - public IntakeSetState(IntakeState state) { + private final Intake intake; + private final Optional pivotState; + private final Optional rollerState; + + public IntakeSetState(RollerState rollerState) { + intake = Intake.getInstance(); + this.pivotState = Optional.empty(); + this.rollerState = Optional.of(rollerState); + addRequirements(intake); + } + + public IntakeSetState(PivotState pivotState) { intake = Intake.getInstance(); - this.intakeState = state; + this.pivotState = Optional.of(pivotState); + this.rollerState = Optional.empty(); + addRequirements(intake); + } + public IntakeSetState(PivotState pivotState, RollerState rollerState) { + intake = Intake.getInstance(); + this.pivotState = Optional.of(pivotState); + this.rollerState = Optional.of(rollerState); addRequirements(intake); } @Override public void initialize() { - intake.setState(intakeState); + if (pivotState.isPresent()) { + intake.setPivotState(pivotState.get()); + } + + if (rollerState.isPresent()) { + intake.setRollerState(rollerState.get()); + } } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java similarity index 62% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java index 472a2243..319dfe3f 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStopRollers.java @@ -5,13 +5,10 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeStop extends IntakeSetState { - /** - * Sets the State of the Intake to Stowing - */ - public IntakeStop() { - super(IntakeState.STOW); +public class IntakeStopRollers extends IntakeSetState { + public IntakeStopRollers() { + super(RollerState.STOP); } -} +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java similarity index 60% rename from src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java rename to src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java index 5b9e88fb..56f2cb0c 100644 --- a/src/main/java/com/stuypulse/robot/commands/intake/IntakeOutake.java +++ b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStow.java @@ -5,13 +5,11 @@ /***************************************************************/ package com.stuypulse.robot.commands.intake; -import com.stuypulse.robot.subsystems.intake.Intake.IntakeState; +import com.stuypulse.robot.subsystems.intake.Intake.PivotState; +import com.stuypulse.robot.subsystems.intake.Intake.RollerState; -public class IntakeOutake extends IntakeSetState { - /** - * Sets the State of the Intake to Outaking - */ - public IntakeOutake() { - super(IntakeState.OUTAKE); +public class IntakeStow extends IntakeSetState { + public IntakeStow() { + super(PivotState.STOWED, RollerState.STOP); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java index 0fd73f71..b68b37f7 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/pidToPose/SwerveDrivePIDToPose.java @@ -70,7 +70,7 @@ public SwerveDrivePIDToPose(Supplier targetPose) { new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new PIDController(Alignment.XY.kP, Alignment.XY.kI, Alignment.XY.kD), new AnglePIDController(Alignment.THETA.kP, Alignment.THETA.kI, Alignment.THETA.kD) - .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFUALT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); + .setSetpointFilter(new AMotionProfile(Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_VELOCITY, Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ANGULAR_ACCELERATION))); maxVelocity = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_VELOCITY; maxAcceleration = Settings.Swerve.Alignment.Constraints.DEFAULT_MAX_ACCELERATION; diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java index f5b4f0eb..8d45aed9 100644 --- a/src/main/java/com/stuypulse/robot/constants/Field.java +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -52,7 +52,7 @@ public static boolean closerToTop(){ // 1.0 meters from driverstation wall and field wall public final Pose2d leftFerryZone = new Pose2d(1.0, WIDTH - 1.0, new Rotation2d()); - public final Pose2d rightFerryZone = new Pose2d(1.0, 1.0, new Rotation2d()); + public final Pose2d rightFerryZone = new Pose2d(1.0 + Units.feetToMeters(6), 1.0 + Units.feetToMeters(3), new Rotation2d()); //TODO: GET ACTUAL POS public static Pose2d getFerryZonePose(Translation2d robot) { if (robot.getDistance(leftFerryZone.getTranslation()) > robot.getDistance(rightFerryZone.getTranslation())) { diff --git a/src/main/java/com/stuypulse/robot/constants/Motors.java b/src/main/java/com/stuypulse/robot/constants/Motors.java index bcc80e26..61bf55e2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Motors.java +++ b/src/main/java/com/stuypulse/robot/constants/Motors.java @@ -18,150 +18,190 @@ import com.ctre.phoenix6.configs.Slot2Configs; import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -/*- - * File containing all of the configurations that different motors require. - * - * Such configurations include: - * - If it is Inverted - * - The Idle Mode of the Motor - * - The Current Limit - * - The Open Loop Ramp Rate - */ public interface Motors { public interface ClimberHopper { - TalonFXConfig MOTOR = new TalonFXConfig() + public final TalonFXConfig MOTOR = new TalonFXConfig() .withInvertedValue(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Brake) - .withCurrentLimitAmps(50) - .withSupplyCurrentLimitAmps(50) + .withCurrentLimitAmps(50.0) + .withSupplyCurrentLimitAmps(50.0) .withRampRate(Settings.ClimberHopper.RAMP_RATE); + + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP) + .withReverseSoftLimitThreshold(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); } - public interface HoodedShooter { - public interface Shooter { - TalonFXConfig SHOOTER = new TalonFXConfig() - // .withSupplyCurrentLimitAmps(100.0) - // .withCurrentLimitAmps(100.0) - // .withRampRate(0.25) - .withCurrentLimitEnable(false) - .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, - Gains.HoodedShooter.Shooter.kD, 0) - .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, - Gains.HoodedShooter.Shooter.kA, 0) - .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); - } + public interface Handoff { + public final TalonFXConfig HANDOFF = new TalonFXConfig() + .withCurrentLimitAmps(80.0) + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) + .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) + .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); + } + public interface HoodedShooter { public interface Hood { - TalonFXConfig HOOD = new TalonFXConfig() - .withCurrentLimitAmps(80) + public final TalonFXConfig HOOD = new TalonFXConfig() + .withCurrentLimitAmps(80.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.HoodedShooter.Hood.kP, Gains.HoodedShooter.Hood.kI, Gains.HoodedShooter.Hood.kD, 0) .withFFConstants(Gains.HoodedShooter.Hood.kS, Gains.HoodedShooter.Hood.kV, Gains.HoodedShooter.Hood.kA, 0) .withSensorToMechanismRatio(Settings.HoodedShooter.Hood.GEAR_RATIO); - - SoftwareLimitSwitchConfigs hoodSoftwareLimitSwitchConfigs = new SoftwareLimitSwitchConfigs() + + public final Slot0Configs SLOT_0 = new Slot0Configs() + .withKP(Gains.HoodedShooter.Hood.kP) + .withKI(Gains.HoodedShooter.Hood.kI) + .withKD(Gains.HoodedShooter.Hood.kD) + .withKS(Gains.HoodedShooter.Hood.kS) + .withKV(Gains.HoodedShooter.Hood.kV) + .withKA(Gains.HoodedShooter.Hood.kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() .withForwardSoftLimitEnable(true) .withReverseSoftLimitEnable(true) .withForwardSoftLimitThreshold(Settings.HoodedShooter.Angles.MAX_ANGLE.getRotations()) .withReverseSoftLimitThreshold(Settings.HoodedShooter.Angles.MIN_ANGLE.getRotations()); - CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() + public final CANcoderConfiguration HOOD_ENCODER = new CANcoderConfiguration() .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0) + .withMagnetOffset(Settings.HoodedShooter.Hood.ENCODER_OFFSET.getRotations())); + } + + public interface Shooter { + public final TalonFXConfig SHOOTER = new TalonFXConfig() + .withCurrentLimitEnable(false) + .withNeutralMode(NeutralModeValue.Coast) + .withInvertedValue(InvertedValue.CounterClockwise_Positive) + .withPIDConstants(Gains.HoodedShooter.Shooter.kP, Gains.HoodedShooter.Shooter.kI, + Gains.HoodedShooter.Shooter.kD, 0) + .withFFConstants(Gains.HoodedShooter.Shooter.kS, Gains.HoodedShooter.Shooter.kV, + Gains.HoodedShooter.Shooter.kA, 0) + .withSensorToMechanismRatio(Settings.HoodedShooter.Shooter.GEAR_RATIO); } } public interface Intake { - TalonFXConfig ROLLER = new TalonFXConfig() - .withCurrentLimitAmps(40) + public final TalonFXConfig ROLLER = new TalonFXConfig() + .withCurrentLimitAmps(40.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Coast) - .withInvertedValue(InvertedValue.Clockwise_Positive); // TODO: add gear ratio, find inversions + .withInvertedValue(InvertedValue.Clockwise_Positive); - TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40) + public final TalonFXConfig PIVOT = new TalonFXConfig() + .withCurrentLimitAmps(40.0) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) // TODO: find inversions + .withInvertedValue(InvertedValue.Clockwise_Positive) .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); - } + } - public interface Spindexeer { - TalonFXConfig PIVOT = new TalonFXConfig() - .withCurrentLimitAmps(40) + public interface Spindexer { + public final TalonFXConfig SPINDEXER = new TalonFXConfig() + .withCurrentLimitEnable(false) .withRampRate(0.25) .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) // TODO: find inversions - .withPIDConstants(Gains.Intake.Pivot.kP, Gains.Intake.Pivot.kI, Gains.Intake.Pivot.kD, 0) - .withFFConstants(Gains.Intake.Pivot.kS, Gains.Intake.Pivot.kV, Gains.Intake.Pivot.kA, Gains.Intake.Pivot.kG, 0) - .withMotionProfile(Settings.Intake.PIVOT_MAX_VEL.getRotations(), Settings.Intake.PIVOT_MAX_ACCEL.getRotations()); + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) + .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) + .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); } - public interface Spindexer { - TalonFXConfig SPINDEXER = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withFFConstants(Gains.Spindexer.kS, Gains.Spindexer.kV, Gains.Spindexer.kA, 0) - .withPIDConstants(Gains.Spindexer.kP, Gains.Spindexer.kI, Gains.Spindexer.kD, 0) - .withSensorToMechanismRatio(Settings.Spindexer.Constants.GEAR_RATIO); + public interface Turret { + public final TalonFXConfig TURRET = new TalonFXConfig() + .withRampRate(0.25) + .withNeutralMode(NeutralModeValue.Brake) + .withInvertedValue(InvertedValue.Clockwise_Positive) + .withPIDConstants(Gains.Turret.kP, 0.0, Gains.Turret.kD, 0) + .withFFConstants(Gains.Turret.kS, 0.0, 0.0, 0) + .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); + + public final Slot0Configs SLOT_0 = new Slot0Configs() + .withKP(Gains.Turret.kP) + .withKI(Gains.Turret.kI) + .withKD(Gains.Turret.kD) + .withKS(Gains.Turret.kS) + .withKV(Gains.Turret.kV) + .withKA(Gains.Turret.kA) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + public final CANCoderConfig ENCODER_17T = new CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + public final CANCoderConfig ENCODER_18T = new CANCoderConfig() + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withAbsoluteSensorDiscontinuityPoint(1.0); + + public final SoftwareLimitSwitchConfigs SOFT_LIMITS = new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true) + .withForwardSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.FORWARD_MAX_ROTATIONS) + .withReverseSoftLimitThreshold(Settings.Turret.Constants.SoftwareLimit.BACKWARDS_MAX_ROTATIONS); } - public interface Turret { - TalonFXConfig TURRET = new TalonFXConfig() - .withCurrentLimitEnable(false) - .withRampRate(.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.Clockwise_Positive) - .withPIDConstants(Gains.Turret.kP, Gains.Turret.kI, Gains.Turret.kD, 0) - .withFFConstants(Gains.Turret.kS, Gains.Turret.kV, Gains.Turret.kA, 0) - .withSensorToMechanismRatio(Settings.Turret.Constants.GEAR_RATIO_MOTOR_TO_MECH); + public static class CANCoderConfig { + private final CANcoderConfiguration configuration = new CANcoderConfiguration(); + private final MagnetSensorConfigs magnetSensorConfigs = new MagnetSensorConfigs(); - SoftwareLimitSwitchConfigs SOFT_LIMIT = new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(true) - .withReverseSoftLimitEnable(true) - .withForwardSoftLimitThreshold(0.75) // 0.75 - .withReverseSoftLimitThreshold(-2.0 / 3.0); // -0.66 + public void configure(CANcoder encoder) { + CANcoderConfiguration defaultConfig = new CANcoderConfiguration(); + encoder.getConfigurator().apply(defaultConfig); - CANcoderConfiguration ENCODER_17T = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.Turret.Constants.Encoder17t.OFFSET.getRotations())); + encoder.getConfigurator().apply(configuration); + } - CANcoderConfiguration ENCODER_18T = new CANcoderConfiguration() - .withMagnetSensor(new MagnetSensorConfigs() - .withSensorDirection(SensorDirectionValue.Clockwise_Positive) - .withAbsoluteSensorDiscontinuityPoint(1) - .withMagnetOffset(Settings.Turret.Constants.Encoder18t.OFFSET.getRotations())); - } + public CANcoderConfiguration getConfiguration() { + return this.configuration; + } - public interface Handoff { - TalonFXConfig HANDOFF = new TalonFXConfig() - .withCurrentLimitAmps(80) - .withRampRate(0.25) - .withNeutralMode(NeutralModeValue.Brake) - .withInvertedValue(InvertedValue.CounterClockwise_Positive) - .withFFConstants(Gains.Handoff.kS, Gains.Handoff.kV, Gains.Handoff.kA, 0) - .withPIDConstants(Gains.Handoff.kP, Gains.Handoff.kI, Gains.Handoff.kD, 0) - .withSensorToMechanismRatio(Settings.Handoff.GEAR_RATIO); + // MAGNET SENSOR CONFIGS + + public CANCoderConfig withSensorDirection(SensorDirectionValue sensorDirection) { + magnetSensorConfigs.SensorDirection = sensorDirection; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } + + public CANCoderConfig withAbsoluteSensorDiscontinuityPoint(double discontinuityPoint) { + magnetSensorConfigs.AbsoluteSensorDiscontinuityPoint = discontinuityPoint; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } + + public CANCoderConfig withMagnetOffset(double magnetOffset) { + magnetSensorConfigs.MagnetOffset = magnetOffset; + + configuration.withMagnetSensor(magnetSensorConfigs); + + return this; + } } public static class TalonFXConfig { @@ -177,14 +217,13 @@ public static class TalonFXConfig { private final MotionMagicConfigs motionMagicConfigs = new MotionMagicConfigs(); public void configure(TalonFX motor) { - // We want to reset configs here before applying configs; prevents unwanted configs from persisting TalonFXConfiguration defaultConfig = new TalonFXConfiguration(); motor.getConfigurator().apply(defaultConfig); motor.getConfigurator().apply(configuration); } - // SLOT 0 CONFIGS + // SLOT CONFIGS public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) { switch (slot) { @@ -211,7 +250,7 @@ public TalonFXConfig withPIDConstants(double kP, double kI, double kD, int slot) } public TalonFXConfig withFFConstants(double kS, double kV, double kA, int slot) { - return withFFConstants(kS, kV, kA, 0, slot); + return withFFConstants(kS, kV, kA, 0.0, slot); } public TalonFXConfig withFFConstants(double kS, double kV, double kA, double kG, int slot) { @@ -300,7 +339,7 @@ public TalonFXConfig withCurrentLimitAmps(double currentLimitAmps) { } public TalonFXConfig withLowerLimitSupplyCurrent(double currentLowerLimitAmps) { - currentLimitsConfigs.SupplyCurrentLowerLimit= currentLowerLimitAmps; + currentLimitsConfigs.SupplyCurrentLowerLimit = currentLowerLimitAmps; currentLimitsConfigs.StatorCurrentLimitEnable = true; configuration.withCurrentLimits(currentLimitsConfigs); diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java index 924ec08a..0d8e164a 100644 --- a/src/main/java/com/stuypulse/robot/constants/Ports.java +++ b/src/main/java/com/stuypulse/robot/constants/Ports.java @@ -10,49 +10,47 @@ /** This file contains the different ports of motors, solenoids and sensors */ public interface Ports { // TODO: Get bus name - public CANBus bus = new CANBus("rio"); + public CANBus RIO = new CANBus("rio"); + public CANBus CANIVORE = new CANBus("CANIVORE"); public interface Gamepad { int DRIVER = 0; - int OPERATOR = 1; - int DEBUGGER = 2; } public interface ClimberHopper { - int CLIMBER_HOPPER = 3; + int CLIMBER_HOPPER = 60; } public interface Handoff { - int HANDOFF = 4; + int HANDOFF = 43; } public interface HoodedShooter { - // all these ports are copied from alphabot public interface Hood { - int MOTOR = 25; - int THROUGHBORE_ENCODER = 37; + int MOTOR = 45; + int THROUGHBORE_ENCODER = 44; } public interface Shooter { - int MOTOR_LEAD = 17; - int MOTOR_FOLLOW = 14; + int MOTOR_LEAD = 47; + int MOTOR_FOLLOW = 46; } } public interface Intake { - int PIVOT = 8; - int ROLLER_LEADER = 9; - int ROLLER_FOLLOWER = 10; + int PIVOT = 20; + int ROLLER_LEADER = 21; + int ROLLER_FOLLOWER = 22; } public interface Spindexer { - int SPINDEXER_LEAD_MOTOR = 11; - int SPINDEXER_FOLLOW_MOTOR = 12; + int SPINDEXER_LEAD_MOTOR = 30; + int SPINDEXER_FOLLOW_MOTOR = 31; } public interface Turret { - int MOTOR = 50; - int ENCODER17T = 38; - int ENCODER18T = 21; + int MOTOR = 40; + int ENCODER17T = 42; + int ENCODER18T = 41; } } diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 7675dea4..0caa5baa 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -5,6 +5,8 @@ /***************************************************************/ package com.stuypulse.robot.constants; +import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.path.PathConstraints; import com.stuypulse.stuylib.network.SmartNumber; import edu.wpi.first.math.VecBuilder; @@ -15,38 +17,74 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; -import com.ctre.phoenix6.CANBus; -import com.pathplanner.lib.path.PathConstraints; - /*- * File containing constants and tunable settings for every subsystem on the robot. * * We use StuyLib's SmartNumber / SmartBoolean in order to have tunable * values that we can edit on Shuffleboard. */ +import com.ctre.phoenix6.CANBus; +import com.pathplanner.lib.path.PathConstraints; + public interface Settings { - double DT = 0.020; - double SECONDS_IN_A_MINUTE = 60.0; - boolean DEBUG_MODE = true; - CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); + public final double DT = 0.020; + public final double SECONDS_IN_A_MINUTE = 60.0; + public final boolean DEBUG_MODE = true; + public final CANBus CANIVORE = new CANBus("canivore", "./logs/example.hoot"); + + public interface ClimberHopper { + public interface Constants { + public final double GEAR_RATIO = 45.0; + + public final double MIN_HEIGHT_METERS = 0.0; + public final double MAX_HEIGHT_METERS = 1.0; + + public final double MASS_KG = 1.0; + + public final double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13.0); + public final double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; + public final double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60.0; + + public final double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2.0 / Math.PI; + } + + public final double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; + public final double CLIMBER_DOWN_HEIGHT_METERS = 0.2; + public final double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; + public final double HOPPER_UP_HEIGHT_METERS = 0.5; + + public final double STALL = 10.0; + + public final double ROTATIONS_AT_BOTTOM = 0.0; + + public final double DEBOUNCE = 0.25; + + public final double GYRO_TOLERANCE = 0.0; + + public final double HEIGHT_TOLERANCE_METERS = 0.02; + + public final double RAMP_RATE = 50.0; + + public final double MOTOR_VOLTAGE = 3.5; + } public interface Handoff { - double GEAR_RATIO = 1.0; + public final double GEAR_RATIO = 3.0 / 1.0; double HANDOFF_STOP = 0.0; double HANDOFF_MAX = 4800.0; double HANDOFF_REVERSE = -500.0; double RPM_TOLERANCE = 200.0; - public final SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); + SmartNumber HANDOFF_RPM = new SmartNumber("Handoff/Target RPM", HANDOFF_MAX); } public interface Intake { Rotation2d PIVOT_STOW_ANGLE = Rotation2d.fromDegrees(90.0); Rotation2d PIVOT_INTAKE_OUTAKE_ANGLE = Rotation2d.fromDegrees(0.0); - public final Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); - public final double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; + Rotation2d PIVOT_ANGLE_TOLERANCE = Rotation2d.fromDegrees(3.0); + double FORWARD_MAX_ROTATIONS = -30.0 / 360.0; + double BACKWARDS_MAX_ROTATIONS = 90.0 / 360.0; Rotation2d PIVOT_ANGLE_OFFSET = new Rotation2d(); Rotation2d PIVOT_MAX_ANGLE = Rotation2d.fromDegrees(190); @@ -71,180 +109,178 @@ public interface Constants { } public interface HoodedShooter { - double SHOOTER_TOLERANCE_RPM = 50.0; - double HOOD_TOLERANCE_DEG = 0.5; + public final double SHOOTER_TOLERANCE_RPM = 50.0; + public final double HOOD_TOLERANCE_DEG = 0.5; + + public interface RPMs { + public final SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); + public final SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); + public final double REVERSE = 0.0; + public final double HUB_RPM = 0.0; + public final double LEFT_CORNER_RPM = 0.0; + public final double RIGHT_CORNER_RPM = 0.0; + } + public interface Angles { + public final SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); + public final SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); + + public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7.0); + public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(40.0); + public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12.0); + public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); + public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10.0); + } public interface AngleInterpolation { - double[][] distanceAngleInterpolationValues = { - {1.43, Units.degreesToRadians(21.0)}, // meters, radians + public final double[][] distanceAngleInterpolationValues = { + {1.30, Units.degreesToRadians(16.5)}, + {1.43, Units.degreesToRadians(21.0)}, + {2.15, Units.degreesToRadians(23.23)}, + {2.864967, Units.degreesToRadians(25.460189)}, {3.65, Units.degreesToRadians(28.0)}, + {4.43, Units.degreesToRadians(30.65)}, {5.32, Units.degreesToRadians(33.5)} }; } - public interface RPMInterpolation{ - double[][] distanceRPMInterpolationValues = { - {1.43, 3000.0}, // meters, RPM + + public interface RPMInterpolation { + public final double[][] distanceRPMInterpolationValues = { + {1.30, 3000.0}, + {1.43, 3000.0}, + {2.15, 3050.0}, + {2.864967, 3215.271125}, {3.65, 3400.0}, - {5.32, 3850.0} + {4.43, 3650.0}, + {5.32, 3950.0} }; } - public interface RPMs { - SmartNumber SHOOT_RPM = new SmartNumber("HoodedShooter/Shoot State Target RPM", 3400.0); - SmartNumber FERRY_RPM = new SmartNumber("HoodedShooter/Ferry State Target RPM", 2000.0); - public final double REVERSE = -0.0; - public final double HUB_RPM = 0.0; - public final double LEFT_CORNER_RPM = 0.0; // TBD - public final double RIGHT_CORNER_RPM = 0.0; // TBD - public final double STOW = 0.0; // TBD + public interface FerryRPMInterpolation { + public final double[][] distanceRPMInterpolationValues = { + {3.79, 3450.0} + }; } - public interface Angles { - SmartNumber SHOOT_ANGLE = new SmartNumber("HoodedShooter/Shoot State Target Angle (deg)", 15.0); - SmartNumber FERRY_ANGLE = new SmartNumber("HoodedShooter/Ferry State Target Angle (deg)", 20.0); - - public final Rotation2d MIN_ANGLE = Rotation2d.fromDegrees(7); - public final Rotation2d MAX_ANGLE = Rotation2d.fromDegrees(36.80); - public final Rotation2d HUB_ANGLE = Rotation2d.fromDegrees(12); - public final Rotation2d LEFT_CORNER_ANGLE = Rotation2d.fromDegrees(10); - public final Rotation2d RIGHT_CORNER_ANGLE = Rotation2d.fromDegrees(10); + public interface Shooter { + public final double GEAR_RATIO = 1.0; + public final double FLYWHEEL_RADIUS = Units.inchesToMeters(3.965 / 2); } public interface Hood { - public final double GEAR_RATIO = 1290300.0 / 5967.0; - public final double SENSOR_TO_HOOD_RATIO = 360.0 / 36.0; + /** + * + * The absolute encoder is mounted on a 10.67:1 gear reduction relative to the + * hood mechanism. This means: + * + * - The encoder rotates 10.67 times for every 1 full rotation of the hood. + * - The hood's physical range of motion is only 33 degrees. + * + * Because 33° * 10.67 = ~352°, the encoder will never exceed 360° over the + * entire hood travel. Therefore, the absolute encoder reading (0–360°) + * uniquely maps to the hood’s 0–33° mechanical range without any ambiguity. + * + */ + public final double GEAR_RATIO = 17024.0 / 135.0; + public final double ENCODER_TO_MECH = 32.0 / 3.0; public final Rotation2d ENCODER_OFFSET = Rotation2d.fromDegrees(0.0); } - public interface Shooter { - public final double GEAR_RATIO = 1.0; - } } - - public interface Turret { - Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); - Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); - double TOLERANCE_DEG = 2.0; - - Rotation2d HUB = Rotation2d.fromDegrees(0.0); - Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); - Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - - public interface Constants { - double RANGE = 210.0; - - Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); - double TURRET_HEIGHT = Units.inchesToMeters(0.0); - public interface Encoder18t { - public final int TEETH = 18; - public final Rotation2d OFFSET = new Rotation2d(); - } + public interface ShootOnTheFly { + public final int MAX_ITERATIONS = 5; + public final double TIME_TOLERANCE = 0.01; + public final SmartNumber UPDATE_DELAY = new SmartNumber("HoodedShooter/SOTM/Update Delay", 0.00); + } - public interface Encoder17t { - public final int TEETH = 17; - public final Rotation2d OFFSET = new Rotation2d(); - } + public interface Swerve { + public final double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; + public final double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - public interface BigGear { - public final int TEETH = 95; + public interface Alignment { + public interface Constraints { + public final double DEFAULT_MAX_VELOCITY = 4.3; + public final double DEFAULT_MAX_ACCELERATION = 15.0; + public final double DEFAULT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400.0); + public final double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900.0); } - public interface SoftwareLimit { - public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; - public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; - } - - public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; - } - } - - public interface ClimberHopper { - public interface Constants { - double GEAR_RATIO = 45.0; + public interface Targets {} - double MIN_HEIGHT_METERS = 0; - double MAX_HEIGHT_METERS = 1; + public interface Tolerances { + public final double X_TOLERANCE = Units.inchesToMeters(2.0); + public final double Y_TOLERANCE = Units.inchesToMeters(2.0); + public final double THETA_TOLERANCE_DEG = 2.0; - double MASS_KG = 1; + public final Pose2d POSE_TOLERANCE = new Pose2d( + Units.inchesToMeters(2.0), + Units.inchesToMeters(2.0), + Rotation2d.fromDegrees(2.0)); - double NUM_ROTATIONS_TO_REACH_TOP = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (0.480 / 13); // Number of rotations that the motor has to spin, NOT the gear - double POSITION_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP; - double VELOCITY_CONVERSION_FACTOR = (MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / NUM_ROTATIONS_TO_REACH_TOP / 60; + public final double MAX_VELOCITY_WHEN_ALIGNED = 0.15; - double DRUM_RADIUS_METERS = ((MAX_HEIGHT_METERS - MIN_HEIGHT_METERS) / (NUM_ROTATIONS_TO_REACH_TOP / GEAR_RATIO)) / 2 / Math.PI; + public final double ALIGNMENT_DEBOUNCE = 0.15; + } } - double CLIMBER_UP_HEIGHT_METERS = Constants.MAX_HEIGHT_METERS; - double CLIMBER_DOWN_HEIGHT_METERS = 0.2; - double HOPPER_DOWN_HEIGHT_METERS = Constants.MIN_HEIGHT_METERS; - double HOPPER_UP_HEIGHT_METERS = 0.5; - - double STALL = 10.0; + public interface Constraints { + public final double MAX_VELOCITY_M_PER_S = 4.3; + public final double MAX_ACCEL_M_PER_S_SQUARED = 15.0; + public final double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400.0); + public final double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900.0); - double ROTATIONS_AT_BOTTOM = 0.0; - - double DEBOUNCE = 0.25; - - double GYRO_TOLERANCE = 0.0; - - double HEIGHT_TOLERANCE_METERS = 0.02; - - double RAMP_RATE = 50.0; - - double MOTOR_VOLTAGE = 3.5; - } - - public interface Vision { - Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); - Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694); - } - - public interface ShootOnTheFly { - int MAX_ITERATIONS = 5; - double TIME_TOLERANCE = 0.01; - } - - public interface Swerve { - double MODULE_VELOCITY_DEADBAND_M_PER_S = 0.1; - double ROTATIONAL_DEADBAND_RAD_PER_S = 0.1; - public interface Constraints { - double MAX_VELOCITY_M_PER_S = 4.3; - double MAX_ACCEL_M_PER_S_SQUARED = 15.0; - double MAX_ANGULAR_VEL_RAD_PER_S = Units.degreesToRadians(400); - double MAX_ANGULAR_ACCEL_RAD_PER_S = Units.degreesToRadians(900); - - PathConstraints DEFAULT_CONSTRAINTS = + public final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( MAX_VELOCITY_M_PER_S, MAX_ACCEL_M_PER_S_SQUARED, MAX_ANGULAR_VEL_RAD_PER_S, MAX_ANGULAR_ACCEL_RAD_PER_S); } + } - public interface Alignment { - public interface Constraints { - double DEFAULT_MAX_VELOCITY = 4.3; - double DEFAULT_MAX_ACCELERATION = 15.0; - double DEFUALT_MAX_ANGULAR_VELOCITY = Units.degreesToRadians(400); - double DEFAULT_MAX_ANGULAR_ACCELERATION = Units.degreesToRadians(900); - } + public interface Turret { + public final Rotation2d MAX_VEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final Rotation2d MAX_ACCEL = new Rotation2d(Units.degreesToRadians(600.0)); + public final double TOLERANCE_DEG = 2.0; - public interface Tolerances { - double X_TOLERANCE = Units.inchesToMeters(2.0); - double Y_TOLERANCE = Units.inchesToMeters(2.0); - double THETA_TOLERANCE_DEG = 2.0; + public final Rotation2d HUB = Rotation2d.fromDegrees(0.0); + public final Rotation2d LEFT_CORNER = Rotation2d.fromDegrees(0.0); + public final Rotation2d RIGHT_CORNER = Rotation2d.fromDegrees(0.0); - Pose2d POSE_TOLERANCE = new Pose2d( - Units.inchesToMeters(2.0), - Units.inchesToMeters(2.0), - Rotation2d.fromDegrees(2.0)); + double RESOLUTION_OF_ABSOLUTE_ENCODER = 0.1; + Rotation2d MAX_THEORETICAL_ROTATION = Rotation2d.fromDegrees(612); + Rotation2d MIN_THEORETICAL_ROTATION = Rotation2d.fromDegrees(-612); + + public interface Constants { + public final double RANGE = 210.0; - double MAX_VELOCITY_WHEN_ALIGNED = 0.15; + public final Transform2d TURRET_OFFSET = new Transform2d(Units.inchesToMeters(0.0), Units.inchesToMeters(0.0), Rotation2d.kZero); + public final double TURRET_HEIGHT = Units.inchesToMeters(0.0); - double ALIGNMENT_DEBOUNCE = 0.15; + public final double GEAR_RATIO_MOTOR_TO_MECH = 1425.0 / 36.0; + + public interface BigGear { + public final int TEETH = 95; } - public interface Targets {} + public interface Encoder17t { + public final int TEETH = 17; + public final Rotation2d OFFSET = new Rotation2d(); + } + + public interface Encoder18t { + public final int TEETH = 18; + public final Rotation2d OFFSET = new Rotation2d(); + } + + public interface SoftwareLimit { + public final double FORWARD_MAX_ROTATIONS = 210.0 / 360.0; + public final double BACKWARDS_MAX_ROTATIONS = -210.0 / 360.0; + } } } -} + + public interface Vision { + public final Vector MT1_STDEVS = VecBuilder.fill(0.5, 0.5, 1.0); + public final Vector MT2_STDEVS = VecBuilder.fill(0.7, 0.7, 694694.0); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java index b2d70988..35197ae1 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopper.java @@ -11,6 +11,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.Optional; + public abstract class ClimberHopper extends SubsystemBase { private static final ClimberHopper instance; @@ -29,7 +31,7 @@ public static ClimberHopper getInstance() { public enum ClimberHopperState { CLIMBER_UP(Settings.ClimberHopper.CLIMBER_UP_HEIGHT_METERS), CLIMBER_DOWN(Settings.ClimberHopper.CLIMBER_DOWN_HEIGHT_METERS), - HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), + // HOPPER_UP(Settings.ClimberHopper.HOPPER_UP_HEIGHT_METERS), HOPPER_DOWN(Settings.ClimberHopper.HOPPER_DOWN_HEIGHT_METERS); private double targetHeight; @@ -44,7 +46,7 @@ public double getTargetHeight() { } - protected ClimberHopperState state; + private ClimberHopperState state; protected ClimberHopper() { this.state = ClimberHopperState.CLIMBER_UP; @@ -61,8 +63,13 @@ public void setState(ClimberHopperState state) { public abstract boolean getStalling(); public abstract double getCurrentHeight(); public abstract boolean atTargetHeight(); + + /** + * Resets the encoder postition to the upper hardstop + */ + public abstract void resetPostionUpper(); - // public abstract void setVoltageOverride(Optional voltage); + public abstract void setVoltageOverride(Optional voltage); @Override public void periodic() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java index de373ca9..fed7859b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperImpl.java @@ -7,6 +7,7 @@ import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounce; +import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; @@ -20,6 +21,8 @@ public class ClimberHopperImpl extends ClimberHopper { private final TalonFX motor; + private final VoltageOut controller; + private final BStream stalling; private double voltage; @@ -27,12 +30,20 @@ public class ClimberHopperImpl extends ClimberHopper { public ClimberHopperImpl() { super(); - motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER); - Motors.ClimberHopper.MOTOR.configure(motor); - motor.setPosition(0); + motor = new TalonFX(Ports.ClimberHopper.CLIMBER_HOPPER, Ports.CANIVORE); + Motors.ClimberHopper.MOTOR.configure(motor); + motor.getConfigurator().apply(Motors.ClimberHopper.SOFT_LIMITS); + + motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM); stalling = BStream.create(() -> motor.getStatorCurrent().getValueAsDouble() > Settings.ClimberHopper.STALL) .filtered(new BDebounce.Both(Settings.ClimberHopper.DEBOUNCE)); + + // TODO: initialize voltage to default voltage and pass to controller initialization below + controller = new VoltageOut(0) + .withEnableFOC(true); + + voltageOverride = Optional.empty(); } @Override @@ -41,7 +52,7 @@ public boolean getStalling() { } @Override - public double getCurrentHeight() { // TODO: convert motor encoder position to meters somehow + public double getCurrentHeight() { return this.motor.getPosition().getValueAsDouble() * Settings.ClimberHopper.Constants.POSITION_CONVERSION_FACTOR; } @@ -54,36 +65,46 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } + + public void resetPostionUpper() { + motor.setPosition(Settings.ClimberHopper.ROTATIONS_AT_BOTTOM + Settings.ClimberHopper.Constants.NUM_ROTATIONS_TO_REACH_TOP); + } @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } + } + + if (EnabledSubsystems.CLIMBER_HOPPER.get()) { + motor.setControl(controller.withOutput(voltage)); } else { - voltage = 0; + motor.stopMotor(); } - // TODO: Figure out some way to reset the encoder reading when stall - // if (atTargetHeight() && getState() == ClimberHopperState.HOPPER_DOWN) { - // if (voltageOverride.isPresent()) { - - // } - // } - - motor.setControl(new VoltageOut(voltage).withEnableFOC(true)); - - SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); - SmartDashboard.putNumber("ClimberHopper/Current", motor.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putBoolean("ClimberHopper/Stalling", getStalling()); + + if (Settings.DEBUG_MODE) { + SmartDashboard.putNumber("ClimberHopper/Current Height", getCurrentHeight()); + SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); + SmartDashboard.putNumber("ClimberHopper/Applied Voltage", motor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("ClimberHopper/Supply Current", motor.getSupplyCurrent().getValueAsDouble()); + } } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java index e6d6bbb6..9ae11e3e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java +++ b/src/main/java/com/stuypulse/robot/subsystems/climberhopper/ClimberHopperSim.java @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj.simulation.ElevatorSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Optional; + public class ClimberHopperSim extends ClimberHopper { @@ -19,6 +21,8 @@ public class ClimberHopperSim extends ClimberHopper { private final ClimberHopperVisualizer visualizer; private double voltage; + private Optional voltageOverride; + public ClimberHopperSim() { visualizer = ClimberHopperVisualizer.getInstance(); @@ -32,6 +36,8 @@ public ClimberHopperSim() { false, Settings.ClimberHopper.Constants.MIN_HEIGHT_METERS ); + + voltageOverride = Optional.empty(); } public boolean getStalling() { @@ -52,27 +58,35 @@ public boolean atTargetHeight() { return isWithinTolerance(Settings.ClimberHopper.HEIGHT_TOLERANCE_METERS); } - // @Override - // public void setVoltageOverride(Optional voltage) { - // this.voltageOverride = voltage; - // } + @Override + public void setVoltageOverride(Optional voltage) { + this.voltageOverride = voltage; + } + + @Override + public void resetPostionUpper() { + // No encoder reset for sim + } + @Override public void periodic() { super.periodic(); - if (!atTargetHeight()) { - if (getCurrentHeight() < getState().getTargetHeight()) { - voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + if (voltageOverride.isPresent()) { + voltage = voltageOverride.get(); + } else { + if (!atTargetHeight()) { + if (getCurrentHeight() < getState().getTargetHeight()) { + voltage = Settings.ClimberHopper.MOTOR_VOLTAGE; + } else { + voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + } } else { - voltage = - Settings.ClimberHopper.MOTOR_VOLTAGE; + voltage = 0; } - } else { - voltage = 0; } - // TODO: Figure out some way to reset the encoder reading when stall - sim.setInputVoltage(voltage); SmartDashboard.putNumber("ClimberHopper/Voltage", voltage); diff --git a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java index d431d722..2bb053ad 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/handoff/HandoffImpl.java @@ -25,10 +25,11 @@ public class HandoffImpl extends Handoff { private Optional voltageOverride; public HandoffImpl() { - motor = new TalonFX(Ports.Handoff.HANDOFF); + motor = new TalonFX(Ports.Handoff.HANDOFF, Ports.RIO); Motors.Handoff.HANDOFF.configure(motor); - controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE); + controller = new VelocityVoltage(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE) + .withEnableFOC(true); voltageOverride = Optional.empty(); } @@ -51,7 +52,7 @@ public void periodic() { } else if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + motor.setControl(controller.withVelocity(getTargetRPM() / 60.0)); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java index 286dad3e..b1945ee9 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/HoodedShooter.java @@ -32,7 +32,7 @@ public static HoodedShooter getInstance(){ private final Shooter shooter; public HoodedShooter() { - state = HoodedShooterState.STOW; + state = HoodedShooterState.INTERPOLATION; hood = Hood.getInstance(); shooter = Shooter.getInstance(); } @@ -117,5 +117,8 @@ public void periodic() { SmartDashboard.putBoolean("HoodedShooter/Shooter At Tolerance?", isShooterAtTolerance()); SmartDashboard.putBoolean("HoodedShooter/Hood At Tolerance?", isHoodAtTolerance()); + + SmartDashboard.putNumber("InterpolationTesting/Hood Angle", getHoodAngle().getDegrees()); + SmartDashboard.putNumber("InterpolationTesting/Shooter RPM", getShooterRPM()); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java index c2ce8e7e..9a637d5a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/Hood.java @@ -33,8 +33,8 @@ public enum HoodState { HUB, LEFT_CORNER, RIGHT_CORNER, - IDLE, - INTERPOLATION; + INTERPOLATION, + IDLE; } public Hood() { diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java index 4cab66ce..6b8c814e 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/hood/HoodImpl.java @@ -19,6 +19,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import java.util.Optional; + public class HoodImpl extends Hood { private final TalonFX hoodMotor; private final CANcoder hoodEncoder; @@ -26,19 +27,22 @@ public class HoodImpl extends Hood { private final PositionVoltage controller; private Optional voltageOverride; - + private boolean hasUsedAbsoluteEncoder; public HoodImpl() { - hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR); - hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER); + hoodMotor = new TalonFX(Ports.HoodedShooter.Hood.MOTOR, Ports.RIO); + hoodEncoder = new CANcoder(Ports.HoodedShooter.Hood.THROUGHBORE_ENCODER, Ports.RIO); Motors.HoodedShooter.Hood.HOOD.configure(hoodMotor); - hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.hoodSoftwareLimitSwitchConfigs); + hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SLOT_0); + hoodMotor.getConfigurator().apply(Motors.HoodedShooter.Hood.SOFT_LIMITS); + hoodEncoder.getConfigurator().apply(Motors.HoodedShooter.Hood.HOOD_ENCODER); - controller = new PositionVoltage(getTargetAngle().getRotations()); + controller = new PositionVoltage(getTargetAngle().getRotations()) + .withEnableFOC(true); voltageOverride = Optional.empty(); } @@ -53,22 +57,36 @@ public void periodic() { super.periodic(); if (!hasUsedAbsoluteEncoder) { - hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); + /* + * Example: + * Let's say the hood rotates 0.1 rotations. Then, the encoder has rotated 0.1 * 10.67 rotations + * To convert the encoder reading to the mechanism position, we simply do (0.1 * 10.67) / 10.67 = 0.1 + */ + hoodMotor.setPosition(hoodEncoder.getAbsolutePosition().getValueAsDouble() / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + hasUsedAbsoluteEncoder = true; } if (EnabledSubsystems.HOOD.get()) { if (voltageOverride.isPresent()) { hoodMotor.setVoltage(voltageOverride.get()); } else { - hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations()).withEnableFOC(true)); + hoodMotor.setControl(controller.withPosition(getTargetAngle().getRotations())); } } else { hoodMotor.stopMotor(); } if (Settings.DEBUG_MODE) { - SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.SENSOR_TO_HOOD_RATIO); //* 360.0 / (360.0/35.0) / .97); - SmartDashboard.putNumber("HoodedShooter/Hood/Input Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Hood Absolute Angle (deg)", hoodEncoder.getPosition().getValueAsDouble() * 360.0 / Settings.HoodedShooter.Hood.ENCODER_TO_MECH); + + SmartDashboard.putNumber("HoodedShooter/Hood/Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("HoodedShooter/Hood/Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); + + SmartDashboard.putNumber("HoodedShooter/Hood/Closed Loop Error (deg)", hoodMotor.getClosedLoopError().getValueAsDouble() * 360.0); + SmartDashboard.putBoolean("HoodedShooter/Hood/Has Used Absolute Encoder", hasUsedAbsoluteEncoder); + + SmartDashboard.putNumber("InterpolationTesting/Hood Applied Voltage", hoodMotor.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("InterpolationTesting/Hood Supply Current", hoodMotor.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java index 8dae4c8c..c18ed7c3 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/Shooter.java @@ -57,7 +57,7 @@ public double getTargetRPM() { return switch(state) { case STOP -> 0; case SHOOT -> getShootRPM(); - case FERRY -> getFerryRPM(); + case FERRY -> HoodAngleCalculator.interpolateFerryingRPM().get(); case REVERSE -> Settings.HoodedShooter.RPMs.REVERSE; case HUB -> Settings.HoodedShooter.RPMs.HUB_RPM; case LEFT_CORNER -> Settings.HoodedShooter.RPMs.LEFT_CORNER_RPM; @@ -67,11 +67,7 @@ public double getTargetRPM() { } public double getShootRPM() { - return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // will return different speeds in future based on distance to hub - } - - public double getFerryRPM() { - return Settings.HoodedShooter.RPMs.FERRY_RPM.get(); + return Settings.HoodedShooter.RPMs.SHOOT_RPM.get(); // Adjustable RPM on Glass } public boolean atTolerance() { @@ -90,5 +86,7 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Current RPM", getShooterRPM()); SmartDashboard.putNumber("HoodedShooter/Shooter/Target RPM", getTargetRPM()); + + SmartDashboard.putNumber("InterpolationTesting/Shooter Interpolated Target Shoot RPM", HoodAngleCalculator.interpolateShooterRPM().get()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java index 688826a0..2a88eff0 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/hoodedshooter/shooter/ShooterImpl.java @@ -30,10 +30,11 @@ public class ShooterImpl extends Shooter { private Optional voltageOverride; public ShooterImpl() { - shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD); - shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW); + shooterLeader = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_LEAD, Ports.RIO); + shooterFollower = new TalonFX(Ports.HoodedShooter.Shooter.MOTOR_FOLLOW, Ports.RIO); - shooterController = new VelocityVoltage(getTargetRPM() / 60.0); + shooterController = new VelocityVoltage(getTargetRPM() / 60.0) + .withEnableFOC(true); follower = new Follower(Ports.HoodedShooter.Shooter.MOTOR_LEAD, MotorAlignmentValue.Opposed); Motors.HoodedShooter.Shooter.SHOOTER.configure(shooterLeader); @@ -69,7 +70,7 @@ public void periodic() { shooterLeader.setVoltage(voltageOverride.get()); shooterFollower.setControl(follower); } else { - shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0).withEnableFOC(true)); + shooterLeader.setControl(shooterController.withVelocity(getTargetRPM() / 60.0)); shooterFollower.setControl(follower); } } else { @@ -83,6 +84,12 @@ public void periodic() { SmartDashboard.putNumber("HoodedShooter/Shooter/Leader Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("HoodedShooter/Shooter/Follower Voltage", shooterFollower.getMotorVoltage().getValueAsDouble()); + + SmartDashboard.putNumber("HoodedShooter/Shooter/Follower RPM", getFollowerRPM()); + + SmartDashboard.putNumber("InterpolationTesting/Shooter Closed Loop Error", shooterLeader.getClosedLoopError().getValueAsDouble() * 60.0); + SmartDashboard.putNumber("InterpolationTesting/Shooter Applied Voltage", shooterLeader.getMotorVoltage().getValueAsDouble()); + SmartDashboard.putNumber("InterpolationTesting/Shooter Supply Current", shooterLeader.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java index 1798f11e..bcde7954 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/Intake.java @@ -16,7 +16,6 @@ public abstract class Intake extends SubsystemBase { private static final Intake instance; - private IntakeState state; static { instance = new IntakeImpl(); @@ -26,56 +25,78 @@ public static Intake getInstance() { return instance; } - public Intake() { - state = IntakeState.STOW; - } - - public enum IntakeState { - INTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, 1.0), - OUTAKE(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE, -1.0), - STOW(Settings.Intake.PIVOT_STOW_ANGLE, 0.0); + public enum PivotState { + DEPLOYED(Settings.Intake.PIVOT_INTAKE_OUTAKE_ANGLE), + STOWED(Settings.Intake.PIVOT_STOW_ANGLE); - private double targetDutyCycle; - private Rotation2d targetAngle; + private final Rotation2d targetAngle; - private IntakeState(Rotation2d targetAngle, double targetDutyCycle) { + private PivotState(Rotation2d targetAngle) { this.targetAngle = targetAngle; - this.targetDutyCycle = targetDutyCycle; } public Rotation2d getTargetAngle() { return targetAngle; } + } + + public enum RollerState { + INTAKE(1.0), + OUTTAKE(-1.0), + STOP(0.0); + + private final double targetDutyCycle; + + private RollerState(double targetDutyCycle) { + this.targetDutyCycle = targetDutyCycle; + } public double getTargetDutyCycle() { return targetDutyCycle; } } - public IntakeState getState() { - return state; + private PivotState pivotState; + private RollerState rollerState; + + protected Intake() { + this.pivotState = PivotState.STOWED; + this.rollerState = RollerState.STOP; + } + + public PivotState getPivotState() { + return pivotState; + } + + public void setPivotState(PivotState state) { + this.pivotState = state; + setPivotVoltageOverride(Optional.empty()); } - public void setState(IntakeState state) { - this.state = state; + public RollerState getRollerState() { + return rollerState; + } + + public void setRollerState(RollerState state) { + this.rollerState = state; } public abstract boolean pivotAtTolerance(); public abstract Rotation2d getPivotAngle(); - public abstract SysIdRoutine getPivotSysIdRoutine(); public abstract void setPivotVoltageOverride(Optional voltage); - + public abstract SysIdRoutine getPivotSysIdRoutine(); + @Override public void periodic() { - SmartDashboard.putString("Intake/State", getState().toString()); - SmartDashboard.putString("Intake/State", getState().toString()); + SmartDashboard.putString("Intake/Pivot State", getPivotState().toString()); + SmartDashboard.putString("Intake/Roller State", getRollerState().toString()); SmartDashboard.putNumber("Intake/Current Angle (deg)", getPivotAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Angle (deg)", getState().getTargetAngle().getDegrees()); + SmartDashboard.putNumber("Intake/Target Angle (deg)", getPivotState().getTargetAngle().getDegrees()); - SmartDashboard.putNumber("Intake/Target Duty Cycle", getState().getTargetDutyCycle()); + SmartDashboard.putNumber("Intake/Target Duty Cycle", getRollerState().getTargetDutyCycle()); SmartDashboard.putBoolean("Intake/Pivot At Tolerance?", pivotAtTolerance()); } - + } diff --git a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java index 43a9462c..b89ccd0a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/intake/IntakeImpl.java @@ -27,25 +27,26 @@ public class IntakeImpl extends Intake { private final TalonFX rollerLeader; private final TalonFX rollerFollower; - private final DutyCycleOut rollerController; private final MotionMagicVoltage pivotController; - + private final DutyCycleOut rollerController; private final Follower follower; private Optional pivotVoltageOverride; public IntakeImpl() { - pivot = new TalonFX(Ports.Intake.PIVOT); + pivot = new TalonFX(Ports.Intake.PIVOT, Ports.RIO); Motors.Intake.PIVOT.configure(pivot); - rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER); + rollerLeader = new TalonFX(Ports.Intake.ROLLER_LEADER, Ports.RIO); Motors.Intake.ROLLER.configure(rollerLeader); - rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER); + rollerFollower = new TalonFX(Ports.Intake.ROLLER_FOLLOWER, Ports.RIO); Motors.Intake.ROLLER.configure(rollerFollower); - pivotController = new MotionMagicVoltage(getState().getTargetAngle().getRotations()); - rollerController = new DutyCycleOut(getState().getTargetDutyCycle()); + pivotController = new MotionMagicVoltage(getPivotState().getTargetAngle().getRotations()) + .withEnableFOC(true); + rollerController = new DutyCycleOut(getRollerState().getTargetDutyCycle()) + .withEnableFOC(true); follower = new Follower(Ports.Intake.ROLLER_LEADER, MotorAlignmentValue.Opposed); pivotVoltageOverride = Optional.empty(); @@ -54,22 +55,25 @@ public IntakeImpl() { @Override public boolean pivotAtTolerance() { return Math.abs( - (getPivotAngle().getRotations()) - getState().getTargetAngle().getRotations()) - < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); + getPivotAngle().getRotations() - getPivotState().getTargetAngle().getRotations()) + < Settings.Intake.PIVOT_ANGLE_TOLERANCE.getRotations(); } + @Override public Rotation2d getPivotAngle() { return Rotation2d.fromRotations(pivot.getPosition().getValueAsDouble()); } - + @Override public void periodic() { + super.periodic(); + if (EnabledSubsystems.INTAKE.get()) { if (pivotVoltageOverride.isPresent()) { pivot.setVoltage(pivotVoltageOverride.get()); } else { - pivot.setControl(pivotController.withPosition(getState().getTargetAngle().getRotations()).withEnableFOC(true)); - rollerLeader.setControl(rollerController.withOutput(getState().getTargetDutyCycle()).withEnableFOC(true)); + pivot.setControl(pivotController.withPosition(getPivotState().getTargetAngle().getRotations())); + rollerLeader.setControl(rollerController.withOutput(getRollerState().getTargetDutyCycle())); rollerFollower.setControl(follower); } } else { @@ -79,7 +83,7 @@ public void periodic() { } SmartDashboard.putNumber("Intake/Pivot Angle Error (deg)", - Math.abs(getState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); + Math.abs(getPivotState().getTargetAngle().getDegrees() - getPivotAngle().getDegrees())); if (Settings.DEBUG_MODE) { // PIVOT @@ -103,13 +107,13 @@ public void setPivotVoltageOverride(Optional voltage) { @Override public SysIdRoutine getPivotSysIdRoutine() { return SysId.getRoutine( - 2, - 6, - "Intake Pivot", - voltage -> setPivotVoltageOverride(Optional.of(voltage)), - () -> getPivotAngle().getRotations(), - () -> pivot.getVelocity().getValueAsDouble(), - () -> pivot.getMotorVoltage().getValueAsDouble(), - getInstance()); + 2, + 6, + "Intake Pivot", + voltage -> setPivotVoltageOverride(Optional.of(voltage)), + () -> getPivotAngle().getRotations(), + () -> pivot.getVelocity().getValueAsDouble(), + () -> pivot.getMotorVoltage().getValueAsDouble(), + getInstance()); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java index af7e2a82..7eb88853 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/spindexer/SpindexerImpl.java @@ -30,15 +30,16 @@ public class SpindexerImpl extends Spindexer { private Optional voltageOverride; public SpindexerImpl() { - leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR); - followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR); + leadMotor = new TalonFX(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, Ports.CANIVORE); + followerMotor = new TalonFX(Ports.Spindexer.SPINDEXER_FOLLOW_MOTOR, Ports.CANIVORE); Motors.Spindexer.SPINDEXER.configure(leadMotor); Motors.Spindexer.SPINDEXER.configure(followerMotor); - follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); + controller = new VelocityVoltage(getTargetRPM()) + .withEnableFOC(true); - controller = new VelocityVoltage(getTargetRPM()); + follower = new Follower(Ports.Spindexer.SPINDEXER_LEAD_MOTOR, MotorAlignmentValue.Opposed); voltageOverride = Optional.empty(); } @@ -63,7 +64,7 @@ public void periodic() { leadMotor.setVoltage(voltageOverride.get()); followerMotor.setControl(follower); } else { - leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE).withEnableFOC(true)); + leadMotor.setControl(controller.withVelocity(getTargetRPM() / Settings.SECONDS_IN_A_MINUTE)); followerMotor.setControl(follower); } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java index 4d79f238..65944d27 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/com/stuypulse/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -440,6 +440,7 @@ public void periodic() { turret2d.setPose(Robot.isBlue() ? turretPose : Field.transformToOppositeAlliance(turretPose)); SmartDashboard.putNumber("Turret/Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); + SmartDashboard.putNumber("InterpolationTesting/Turret Dist From Hub", turretPose.getTranslation().getDistance(Field.hubCenter.getTranslation())); SmartDashboard.putNumber("Swerve/Pose/X", pose.getX()); SmartDashboard.putNumber("Swerve/Pose/Y", pose.getY()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java index bfa74453..7dc79e49 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/Turret.java @@ -60,10 +60,10 @@ public Rotation2d getTargetAngle() { case ZERO -> Rotation2d.kZero; case SHOOTING -> getScoringAngle(); case FERRYING -> getFerryAngle(); - case TESTING -> driverInputToAngle(); case HUB -> Settings.Turret.HUB; case LEFT_CORNER -> Settings.Turret.LEFT_CORNER; case RIGHT_CORNER -> Settings.Turret.RIGHT_CORNER; + case TESTING -> driverInputToAngle(); }; } @@ -117,7 +117,7 @@ public void periodic() { public Rotation2d getPointAtTargetAngle(Pose2d targetPose) { Pose2d robotPose = CommandSwerveDrivetrain.getInstance().getPose(); - Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); // TODO: TEST IF THIS PLUS SHOULD BE MINUS + Pose2d turretPose = CommandSwerveDrivetrain.getInstance().getTurretPose(); Vector2D turret = new Vector2D(turretPose.getTranslation()); Vector2D target = new Vector2D(targetPose.getTranslation()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java index b190e035..97d58657 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/turret/TurretImpl.java @@ -5,13 +5,13 @@ /***************************************************************/ package com.stuypulse.robot.subsystems.turret; - import com.stuypulse.robot.RobotContainer.EnabledSubsystems; import com.stuypulse.robot.constants.Motors; import com.stuypulse.robot.constants.Ports; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Turret.Constants; import com.stuypulse.robot.util.SysId; +import com.stuypulse.robot.util.turret.TurretAngleCalculator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -30,24 +30,24 @@ public class TurretImpl extends Turret { private boolean hasUsedAbsoluteEncoder; private Optional voltageOverride; private final PositionVoltage controller; - + public TurretImpl() { - motor = new TalonFX(Ports.Turret.MOTOR, Ports.bus); - encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.bus); - encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.bus); + motor = new TalonFX(Ports.Turret.MOTOR, Ports.RIO); + encoder17t = new CANcoder(Ports.Turret.ENCODER17T, Ports.RIO); + encoder18t = new CANcoder(Ports.Turret.ENCODER18T, Ports.RIO); Motors.Turret.TURRET.configure(motor); - encoder17t.getConfigurator().apply(Motors.Turret.ENCODER_17T); - encoder18t.getConfigurator().apply(Motors.Turret.ENCODER_18T); - // motor.getConfigurator().apply(Motors.Turret.turretSoftwareLimitSwitchConfigs); + motor.getConfigurator().apply(Motors.Turret.SLOT_0); + motor.getConfigurator().apply(Motors.Turret.SOFT_LIMITS); seedTurret(); hasUsedAbsoluteEncoder = false; voltageOverride = Optional.empty(); - controller = new PositionVoltage(getTargetAngle().getRotations()); + controller = new PositionVoltage(getTargetAngle().getRotations()) + .withEnableFOC(true); } private Rotation2d getEncoderPos17t() { @@ -58,33 +58,34 @@ private Rotation2d getEncoderPos18t() { return Rotation2d.fromRotations(this.encoder18t.getAbsolutePosition().getValueAsDouble()); } - public Rotation2d getAbsoluteTurretAngle() { - final int inverseMod17t = 1; - final int inverseMod18t = -1; + public Rotation2d getVectorSpaceAngle() { + return TurretAngleCalculator.getAbsoluteAngle(getEncoderPos17t().getDegrees(), getEncoderPos18t().getDegrees()); + } - final Rotation2d encoder17tPosition = getEncoderPos17t(); - final double numberOfGearTeethRotated17 = (encoder17tPosition.getRotations() - * (double) Constants.Encoder17t.TEETH); + public void zeroEncoders() { + double encoderPos17T = encoder17t.getAbsolutePosition().getValueAsDouble(); + double encoderPos18T = encoder18t.getAbsolutePosition().getValueAsDouble(); - final Rotation2d encoder18tPosition = getEncoderPos18t(); - final double numberOfGearTeethRotated18 = (encoder18tPosition.getRotations() - * (double) Constants.Encoder18t.TEETH); + encoder17t.getConfigurator().refresh(Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor); + encoder18t.getConfigurator().refresh(Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor); - final double crt_Partial17 = numberOfGearTeethRotated17 * inverseMod17t * Constants.Encoder17t.TEETH; - final double crt_Partial18 = numberOfGearTeethRotated18 * inverseMod18t * Constants.Encoder18t.TEETH; + double currentOffset17T = Motors.Turret.ENCODER_17T.getConfiguration().MagnetSensor.MagnetOffset; + double currentOffset18T = Motors.Turret.ENCODER_18T.getConfiguration().MagnetSensor.MagnetOffset; - double crt_pos = (crt_Partial17 + crt_Partial18) - % (Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH); + double newOffset17T = currentOffset17T - encoderPos17T; + double newOffset18T = currentOffset18T - encoderPos18T; - // Java's % operator is not actually the same as the modulo operator, the lines below account for that - crt_pos = (crt_pos < 0) ? (crt_pos + Constants.Encoder17t.TEETH * Constants.Encoder18t.TEETH) - : crt_pos; + Motors.Turret.ENCODER_17T.withMagnetOffset(newOffset17T); + Motors.Turret.ENCODER_18T.withMagnetOffset(newOffset18T); - final double turretAngle = (crt_pos / (double) Constants.BigGear.TEETH); + Motors.Turret.ENCODER_17T.configure(encoder17t); + Motors.Turret.ENCODER_18T.configure(encoder18t); + } - return Rotation2d.fromRotations(turretAngle); + public void seedTurret() { + motor.setPosition(getVectorSpaceAngle().getRotations()); } - + @Override public Rotation2d getAngle() { return Rotation2d.fromDegrees(motor.getPosition().getValueAsDouble()); @@ -102,20 +103,16 @@ private double getDelta(double target, double current) { else if (delta < -180) delta += 360; if (Math.abs(current + delta) < Constants.RANGE) return delta; - + return delta < 0 ? delta + 360 : delta - 360; } - public void seedTurret() { - motor.setPosition(0.0); - } - @Override public void periodic() { super.periodic(); - if (!hasUsedAbsoluteEncoder || getAbsoluteTurretAngle().getRotations() > 1.0 || getAngle().getRotations() < 0.0) { - motor.setPosition((getAbsoluteTurretAngle().getDegrees() % 360.0) / 360.0); + if (!hasUsedAbsoluteEncoder) { + motor.setPosition(getVectorSpaceAngle().getRotations()); hasUsedAbsoluteEncoder = true; System.out.println("Absolute Encoder Reset triggered"); } @@ -130,7 +127,7 @@ public void periodic() { if (voltageOverride.isPresent()) { motor.setVoltage(voltageOverride.get()); } else { - motor.setControl(controller.withPosition(actualTargetDeg / 360.0).withEnableFOC(true)); + motor.setControl(controller.withPosition(actualTargetDeg / 360.0)); } } else { motor.stopMotor(); @@ -139,7 +136,7 @@ public void periodic() { if (Settings.DEBUG_MODE) { SmartDashboard.putNumber("Turret/Encoder18t Abs Position (Rot)", encoder18t.getAbsolutePosition().getValueAsDouble()); SmartDashboard.putNumber("Turret/Encoder17t Abs Position (Rot)", encoder17t.getAbsolutePosition().getValueAsDouble()); - SmartDashboard.putNumber("Turret/CRT Position (Rot)", getAbsoluteTurretAngle().getRotations()); + SmartDashboard.putNumber("Turret/Vector Space Position (Rot)", getVectorSpaceAngle().getRotations()); SmartDashboard.putNumber("Turret/Relative Encoder Position (Rot)", motor.getPosition().getValueAsDouble() * 360.0); SmartDashboard.putNumber("Turret/Voltage", motor.getMotorVoltage().getValueAsDouble()); SmartDashboard.putNumber("Turret/Error", motor.getClosedLoopError().getValueAsDouble() * 360.0); diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java index df3faf2f..551f1ac7 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/HoodAngleCalculator.java @@ -4,31 +4,38 @@ /* that can be found in the repository LICENSE file. */ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; - +import com.stuypulse.robot.Robot; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.HoodedShooter.FerryRPMInterpolation; import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; -import com.stuypulse.robot.subsystems.hoodedshooter.HoodedShooter; import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain; -import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.AlignAngleSolution; +import com.stuypulse.robot.util.hoodedshooter.ShotCalculator.SOTMSolution; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.function.Supplier; public class HoodAngleCalculator { + + public static SOTMSolution sol; + + private static FieldObject2d hubPose2d; + private static FieldObject2d virtualHubPose2d; + private static FieldObject2d futureTurretPose2d; + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - + public static InterpolatingDoubleTreeMap ferryingDistanceRPMInterpolator; + static { distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { @@ -43,12 +50,19 @@ public class HoodAngleCalculator { } } + static { + ferryingDistanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for(double[] pair: FerryRPMInterpolation.distanceRPMInterpolationValues) { + ferryingDistanceRPMInterpolator.put(pair[0], pair[1]); + } + } + public static Supplier interpolateHoodAngle() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getPose().getTranslation(); + Translation2d currentPose = swerve.getTurretPose().getTranslation(); double distanceMeters = hubPose.getDistance(currentPose); @@ -64,54 +78,83 @@ public static Supplier interpolateShooterRPM() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - Translation2d hubPose = Field.getHubPose().getTranslation(); - Translation2d currentPose = swerve.getPose().getTranslation(); - - double distanceMeters = hubPose.getDistance(currentPose); - + Pose2d hubPose = Field.getHubPose(); + Pose2d turretPose = swerve.getTurretPose(); - double targetRPM = distanceRPMInterpolator.get(distanceMeters); + double targetRPM = ShotCalculator.solveInterpolation(turretPose, hubPose).targetRPM(); - SmartDashboard.putNumber("HoodedShooter/Interpolated RPM ", targetRPM); + SmartDashboard.putNumber("HoodedShooter/Interpolated RPM", targetRPM); return targetRPM; }; } - public static Supplier calculateHoodAngleSOTM() { + public static Supplier interpolateFerryingRPM() { return () -> { CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); - HoodedShooter hdsr = HoodedShooter.getInstance(); - - Pose2d currentPose = swerve.getPose(); - Pose2d turretPose = swerve.getTurretPose(); - - ChassisSpeeds robotRelSpeeds = swerve.getChassisSpeeds(); - ChassisSpeeds fieldRelSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( - robotRelSpeeds, - currentPose.getRotation() - ); - - Pose3d targetPose = Field.hubCenter3d; - Pose3d turretPose3d = new Pose3d(new Translation3d(turretPose.getX(), turretPose.getY(), Settings.Turret.Constants.TURRET_HEIGHT), new Rotation3d()); - double axMetersPerSecondSquared = swerve.getPigeon2().getAccelerationX().getValueAsDouble(); - double ayMetersPerSecondSquared = swerve.getPigeon2().getAccelerationY().getValueAsDouble(); - - double shooterRPS = hdsr.getTargetRPM() / 60.0; - - AlignAngleSolution sol = ShotCalculator.solveShootOnTheFly( - turretPose3d, - targetPose, - axMetersPerSecondSquared, - ayMetersPerSecondSquared, - fieldRelSpeeds, // current speeds - shooterRPS, - Settings.ShootOnTheFly.MAX_ITERATIONS, - Settings.ShootOnTheFly.TIME_TOLERANCE - ); + Translation2d currentPose = swerve.getTurretPose().getTranslation(); + Translation2d cornerPose = Field.getFerryZonePose(currentPose).getTranslation(); + + double distanceMeters = cornerPose.getDistance(currentPose); + + double targetRPM = ferryingDistanceRPMInterpolator.get(distanceMeters); + + SmartDashboard.putNumber("HoodedShooter/Interpolated Ferrying RPM", targetRPM); - return sol.launchPitchAngle(); + return targetRPM; }; } + + public static void updateSOTMSolution() { + + CommandSwerveDrivetrain swerve = CommandSwerveDrivetrain.getInstance(); + + Pose2d robotPose = swerve.getPose(); + Pose2d hubPose = Field.getHubPose(); + + ChassisSpeeds robotRelativeSpeeds = swerve.getChassisSpeeds(); + ChassisSpeeds fieldRelativeSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( + robotRelativeSpeeds, + robotPose.getRotation() + ); + + Pose2d futureTurretPose = swerve.getTurretPose().exp( + new Twist2d( + robotRelativeSpeeds.vxMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), + robotRelativeSpeeds.vyMetersPerSecond * Settings.ShootOnTheFly.UPDATE_DELAY.doubleValue(), + 0 + ) + ); + + + SOTMSolution solution = ShotCalculator.solveShootOnTheMove( + futureTurretPose, + robotPose, + hubPose, + fieldRelativeSpeeds, + Settings.ShootOnTheFly.MAX_ITERATIONS, + Settings.ShootOnTheFly.TIME_TOLERANCE + ); + + sol = solution; + + hubPose2d.setPose(Robot.isBlue() ? hubPose : Field.transformToOppositeAlliance(hubPose)); + virtualHubPose2d.setPose((Robot.isBlue() ? sol.virtualPose() : Field.transformToOppositeAlliance(sol.virtualPose()))); + futureTurretPose2d.setPose((Robot.isBlue() ? futureTurretPose : Field.transformToOppositeAlliance(futureTurretPose))); + + + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Turret Angle", sol.targetTurretAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Hood Angle", sol.targetHoodAngle().getDegrees()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Calculated Flight time", sol.flightTime()); + SmartDashboard.putNumber("HoodedShooter/SOTM/Turret Dist to Virtual Pose", futureTurretPose.getTranslation().getDistance(sol.virtualPose().getTranslation())); + } + + public static Supplier calculateHoodAngleSOTM() { + return () -> sol.targetHoodAngle(); + } + + public static Supplier calculateTurretAngleSOTM() { + return () -> sol.targetTurretAngle(); + } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java b/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java index 314ca5ed..0d5930cd 100644 --- a/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java +++ b/src/main/java/com/stuypulse/robot/util/hoodedshooter/ShotCalculator.java @@ -5,133 +5,160 @@ /***************************************************************/ package com.stuypulse.robot.util.hoodedshooter; -import edu.wpi.first.math.geometry.Pose3d; +import com.stuypulse.robot.Robot; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.HoodedShooter.AngleInterpolation; +import com.stuypulse.robot.constants.Settings.HoodedShooter.RPMInterpolation; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.ChassisSpeeds; public final class ShotCalculator { - public static final double g = 9.81; // gravity is not a number - - public record ShotSolution( - Rotation2d launchPitchAngle, - double flightTimeSeconds) { - } + public static final double g = 9.81; - public static ShotSolution solveBallisticWithSpeed( - Pose3d shooterPose, - Pose3d targetPose, - double launchSpeed) { + public static InterpolatingDoubleTreeMap distanceAngleInterpolator; + public static InterpolatingDoubleTreeMap distanceRPMInterpolator; - Translation3d s = shooterPose.getTranslation(); - Translation3d t = targetPose.getTranslation(); - - double dx = t.getX() - s.getX(); - double dy = t.getY() - s.getY(); - double dz = t.getZ() - s.getZ(); + static { + distanceAngleInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : AngleInterpolation.distanceAngleInterpolationValues) { + distanceAngleInterpolator.put(pair[0], pair[1]); + } + } - double d = Math.hypot(dx, dy); - if (d < 1e-9) { - throw new IllegalArgumentException("Horizontal distance too small"); + static { + distanceRPMInterpolator = new InterpolatingDoubleTreeMap(); + for (double[] pair : RPMInterpolation.distanceRPMInterpolationValues) { + distanceRPMInterpolator.put(pair[0], pair[1]); } + } - double v2 = launchSpeed * launchSpeed; + public record StationarySolution( + Rotation2d targetHoodAngle, + double targetRPM, + double flightTimeSeconds) { + } - double discriminant = v2 * v2 - g * (g * d * d + 2.0 * dz * v2); - if (discriminant < 0) { - return new ShotSolution(Rotation2d.kZero, 0); - } + public static StationarySolution solveInterpolation(Pose2d turretPose, Pose2d targetPose) { + + double distanceMeters = turretPose.getTranslation().getDistance(targetPose.getTranslation()); - // LOW-ARC solution (use + for high arc) - double tanTheta = (v2 - Math.sqrt(discriminant)) / (g * d); + // Interpolated Angle + Rotation2d targetHoodAngle = Rotation2d.fromRadians(distanceAngleInterpolator.get(distanceMeters)); - double launchPitch = Math.atan(tanTheta); + // Interpolated RPM + double targetRPM = distanceRPMInterpolator.get(distanceMeters); - double vHoriz = launchSpeed * Math.cos(launchPitch); - double time = d / vHoriz; + // Physics-based TOF + double launchSpeed = 0.5 * targetRPM * (2 * Math.PI / 60.0) * Settings.HoodedShooter.Shooter.FLYWHEEL_RADIUS; + Rotation2d launchAngle = Rotation2d.kCCW_Pi_2.minus(targetHoodAngle); - return new ShotSolution(Rotation2d.fromRadians(launchPitch), time); + double v_x = launchSpeed * Math.cos(launchAngle.getRadians()); + double flightTime = distanceMeters / v_x; + + return new StationarySolution( + targetHoodAngle, + targetRPM, + flightTime + ); } - - public record AlignAngleSolution( - Rotation2d launchPitchAngle, - Rotation2d requiredYaw, - Pose3d estimateTargetPose) { + public record SOTMSolution( + Rotation2d targetHoodAngle, + Rotation2d targetTurretAngle, + Pose2d virtualPose, + double flightTime) { } - public static AlignAngleSolution solveShootOnTheFly( - Pose3d shooterPose, - Pose3d targetPose, - double axMetersPerSecondSquared, - double ayMetersPerSecondSquared, - ChassisSpeeds fieldRelSpeeds, - double targetSpeedRps, + public static SOTMSolution solveShootOnTheMove( + Pose2d turretPose, + Pose2d robotPose, + Pose2d targetPose, + ChassisSpeeds fieldRelativeSpeeds, int maxIterations, double timeTolerance) { - ShotSolution sol = solveBallisticWithSpeed( - shooterPose, - targetPose, - targetSpeedRps + /* + * Start with v_ball * flightTime = distanceToTargetPose. + * + * We know that v_ball = v_robot + v_shooter, so + * (v_robot + v_shooter) * flightTime = distanceToTargetPose + * + * Rearranging, we can get + * (v_shooter) * flight_time = distanceToTargetPose - v_robot * flightTime + * + * So we can instead shoot at a virtual pose and treat the robot as stationary: + * distanceToVirtualPose = distanceToTargetPose - v_robot * flightTime + * (v_shooter) * flight_time = distanceToVirtualPose + * + * Looking at the first equation, we can find the virtual pose with the flight time, + * but looking at the second equation, to get the flight time we need to solveBallisticWithSpeed() + * using the virtual pose, so we have a circular dependence. + * + * Thus, we can make an initial guess for the flight time: the flight time if the robot were stationary + * We want our guess to converge such that the left side equals the right side: + * (v_shooter) * t_guess = distanceToVirtualPose = distance - v_robot * t_guess, which would make t_guess = flightTime + * + * We do the right side first using our inital guess, and then update t_guess with a new guess by + * calculating the flightTime to that virtualPose. + * + * The pose is that the flightTime converges within maxIterations. + */ + + StationarySolution sol = solveInterpolation( + turretPose, + targetPose ); + double t_guess = sol.flightTimeSeconds(); - double t = sol.flightTimeSeconds(); - - Pose3d effectiveTarget = targetPose; + Pose2d virtualPose = targetPose; - Translation3d s = shooterPose.getTranslation(); - + for (int i = 0; i < maxIterations; i++) { - double dx = fieldRelSpeeds.vxMetersPerSecond * t - + 0.5 * axMetersPerSecondSquared * t * t; + double dx = fieldRelativeSpeeds.vxMetersPerSecond * t_guess; + double dy = fieldRelativeSpeeds.vyMetersPerSecond * t_guess; - double dy = fieldRelSpeeds.vyMetersPerSecond * t - + 0.5 * ayMetersPerSecondSquared * t * t; - - effectiveTarget = new Pose3d( + virtualPose = new Pose2d( targetPose.getX() - dx, targetPose.getY() - dy, - targetPose.getZ(), targetPose.getRotation()); - - // SmartDashboard.putNumber("HoodedShooter/Target Pose X", targetPose.getX()); - // SmartDashboard.putNumber("HoodedShooter/Target Pose Y", targetPose.getY()); - - // SmartDashboard.putNumber("HoodedShooter/Virtual Pose X", effectiveTarget.getX()); - // SmartDashboard.putNumber("HoodedShooter/Virtual Pose Y", effectiveTarget.getY()); - - ShotSolution newSol = solveBallisticWithSpeed( - shooterPose, - effectiveTarget, - targetSpeedRps); + StationarySolution newSol = solveInterpolation( + turretPose, + virtualPose + ); - sol = newSol; - if (Math.abs(newSol.flightTimeSeconds() - t) < timeTolerance) { + if (Math.abs(newSol.flightTimeSeconds() - t_guess) < timeTolerance) { break; } - t = newSol.flightTimeSeconds(); + t_guess = newSol.flightTimeSeconds(); + sol = newSol; } - - Translation3d et = effectiveTarget.getTranslation(); + Translation2d virtualTranslation = virtualPose.getTranslation(); + Translation2d turretTranslation = turretPose.getTranslation(); - // all the poses we pass in are field relative, - // so calculated yaw (turret angle) should be field relative as well... right - - double yaw = Math.atan2( //atan2(dy, dx) - et.getY() - s.getY(), - et.getX() - s.getX() + double yaw = Math.atan2( + virtualTranslation.getY() - turretTranslation.getY(), + virtualTranslation.getX() - turretTranslation.getX() ); - return new AlignAngleSolution( - sol.launchPitchAngle(), - Rotation2d.fromRadians(yaw), - effectiveTarget); + Rotation2d targetTurretAngle = Robot.isReal() ? + Rotation2d.fromRadians(-yaw).plus(robotPose.getRotation()) : + Rotation2d.fromRadians(yaw).minus(robotPose.getRotation()); + + return new SOTMSolution( + sol.targetHoodAngle(), + targetTurretAngle, + virtualPose, + sol.flightTimeSeconds() + ); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java new file mode 100644 index 00000000..263ae98e --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/turret/TurretAngleCalculator.java @@ -0,0 +1,64 @@ +/************************ PROJECT TRIBECBOT *************************/ +/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ +package com.stuypulse.robot.util.turret; + +import com.stuypulse.robot.constants.Settings; + +import edu.wpi.first.math.geometry.Rotation2d; + +public class TurretAngleCalculator { + + private static final double MAX_ANGLE_DEGREES = Settings.Turret.MAX_THEORETICAL_ROTATION.getDegrees(); + private static final double MIN_ANGLE_DEGREES = Settings.Turret.MIN_THEORETICAL_ROTATION.getDegrees(); + private static final double RESOLUTION = Settings.Turret.RESOLUTION_OF_ABSOLUTE_ENCODER; + private static final int NUM_POINTS = (int) ((MAX_ANGLE_DEGREES - MIN_ANGLE_DEGREES) / RESOLUTION); + private static int leastDistanceIndex = 0; + + private static final double[] mechanismAngles = new double[NUM_POINTS]; + private static final double[] ARRAY_17T = generateEncoderValues(17); + private static final double[] ARRAY_18T = generateEncoderValues(18); + + private static double[] generateEncoderValues(int teeth) { + double[] values = new double[NUM_POINTS]; + double gearRatio = 1.0 * Settings.Turret.Constants.BigGear.TEETH / teeth; + int i = 0; + + for (double angle = MIN_ANGLE_DEGREES; angle < MAX_ANGLE_DEGREES; angle += RESOLUTION) { + mechanismAngles[i] = angle; + values[i] = (((angle * gearRatio) % 360.0) + 360.0) % 360.0; + i++; + } + + return values; + } + + public static Rotation2d getAbsoluteAngle(double encoder17TValue, double encoder18TValue) { + double leastDistance = Double.MAX_VALUE; + for (int i = 0; i < NUM_POINTS; i++) { + double diff17 = Math.abs(encoder17TValue - ARRAY_17T[i]); + double diff18 = Math.abs(encoder18TValue - ARRAY_18T[i]); + + diff17 = Math.min(diff17, 360.0 - diff17); + diff18 = Math.min(diff18, 360.0 - diff18); + + double distance17 = diff17 * diff17; + double distance18 = diff18 * diff18; + + double distance = distance17 + distance18; + + if (distance < leastDistance) { + leastDistance = distance; + leastDistanceIndex = i; + } + } + + return Rotation2d.fromDegrees(mechanismAngles[leastDistanceIndex]); + } + + public static int lowestDistanceIndex() { + return leastDistanceIndex; + } +}