From 2e9dedbab002ff5ed8b87cd98ed8bb87cde1d7ef Mon Sep 17 00:00:00 2001 From: The-REAL-Bol Date: Tue, 24 Feb 2026 18:24:07 -0800 Subject: [PATCH 1/3] Changed CAN network --- src/main/java/frc/robot/subsystems/climb/ClimbConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java index 69c61e8..2fb8808 100644 --- a/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java +++ b/src/main/java/frc/robot/subsystems/climb/ClimbConstants.java @@ -27,7 +27,7 @@ public class ClimbConstants { public static final Distance DRUM_RADIUS = Inches.of(0.375); public static final int CANDI_CAN_ID = 40; public static final int MOTOR_ID = 50; - public static final String CANBUS_NAME = "subsystems"; + public static final String CANBUS_NAME = "drivetrain"; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Distance MIN_HEIGHT = Inches.of(0); From d8e455d34116730f9fd6b48b9e3f9e53af418bac Mon Sep 17 00:00:00 2001 From: allenx-afk Date: Tue, 24 Feb 2026 18:35:21 -0800 Subject: [PATCH 2/3] Commented out teleop controls that won't be used at week 0 --- .../frc/robot/controls/TeleopControls.java | 214 +++++++++--------- 1 file changed, 107 insertions(+), 107 deletions(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index b25ac31..618e5e1 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -1,12 +1,12 @@ package frc.robot.controls; -import static edu.wpi.first.units.Units.Radians; +// import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -import com.team6962.lib.swerve.commands.TeleopSwerveCommand; +// import com.team6962.lib.swerve.commands.TeleopSwerveCommand; import com.team6962.lib.swerve.commands.XBoxTeleopSwerveCommand; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; @@ -16,17 +16,17 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.RobotContainer; -import frc.robot.auto.AutoClimb; -import frc.robot.auto.DriveToClump; -import frc.robot.subsystems.climb.ClimbConstants; -import frc.robot.subsystems.hood.ShooterHoodConstants; +// import frc.robot.auto.AutoClimb; +// import frc.robot.auto.DriveToClump; +// import frc.robot.subsystems.climb.ClimbConstants; +// import frc.robot.subsystems.hood.ShooterHoodConstants; import frc.robot.subsystems.intakeextension.IntakeExtensionConstants; -import frc.robot.subsystems.turret.TurretConstants; +// import frc.robot.subsystems.turret.TurretConstants; public class TeleopControls { private RobotContainer robot; - private AutoClimb autoClimb; - private DriveToClump driveToClump; +// private AutoClimb autoClimb; +// private DriveToClump driveToClump; private CommandXboxController driver = new CommandXboxController(0); private CommandXboxController operator = new CommandXboxController(1); @@ -34,8 +34,8 @@ public class TeleopControls { public TeleopControls(RobotContainer robot) { this.robot = robot; - this.autoClimb = new AutoClimb(robot); - this.driveToClump = new DriveToClump(robot); + // this.autoClimb = new AutoClimb(robot); + // this.driveToClump = new DriveToClump(robot); } public void configureBindings() { @@ -62,44 +62,44 @@ public void configureBindings() { // Driver right trigger is boost (configured by XBoxTeleopSwerveCommand) // Driver left trigger is super boost (configured by XBoxTeleopSwerveCommand) - // Auto Climb and Unclimb - driver.b().onTrue(autoClimb.climb()); - driver.x().onTrue(autoClimb.unclimb()); - - // Auto Depot - driver - .leftBumper() - .whileTrue( - Commands.sequence( - this.robot - .getSwerveDrive() - .driveTo( - new Pose2d( - 1.518, - 5.947, - new Rotation2d( - Radians.of( - Math.PI)))), // rough position estimate based on simulation, not - // exact - this.robot.getIntakeExtension().extend(), - Commands.parallel( - this.robot - .getSwerveDrive() - .driveTo( - new Pose2d( - 0.546, - 5.947, - new Rotation2d(Radians.of(Math.PI)))), // also rough estimate - this.robot.getIntakeRollers().intake()))); - - driver // Auto Drive to Outpost - .rightBumper() - .whileTrue( - this.robot - .getSwerveDrive() - .driveTo( - new Pose2d( - 0.6, 0.65, new Rotation2d(Radians.of(Math.PI))))); // also a rough estimate + // // Auto Climb and Unclimb + // driver.b().onTrue(autoClimb.climb()); + // driver.x().onTrue(autoClimb.unclimb()); + + // // Auto Depot + // driver + // .leftBumper() + // .whileTrue( + // Commands.sequence( + // this.robot + // .getSwerveDrive() + // .driveTo( + // new Pose2d( + // 1.518, + // 5.947, + // new Rotation2d( + // Radians.of( + // Math.PI)))), // rough position estimate based on simulation, not + // // exact + // this.robot.getIntakeExtension().extend(), + // Commands.parallel( + // this.robot + // .getSwerveDrive() + // .driveTo( + // new Pose2d( + // 0.546, + // 5.947, + // new Rotation2d(Radians.of(Math.PI)))), // also rough estimate + // this.robot.getIntakeRollers().intake()))); + + // driver // Auto Drive to Outpost + // .rightBumper() + // .whileTrue( + // this.robot + // .getSwerveDrive() + // .driveTo( + // new Pose2d( + // 0.6, 0.65, new Rotation2d(Radians.of(Math.PI))))); // also a rough estimate // Dump fuel driver @@ -109,23 +109,23 @@ public void configureBindings() { this.robot.getIntakeRollers().outtake(), this.robot.getHopper().dump())); // Intake and drive to fuel clump - driver.rightStick().whileTrue(driveToClump.driveToClump()); + // driver.rightStick().whileTrue(driveToClump.driveToClump()); // Intake without driving driver .start() .whileTrue(this.robot.getIntakeRollers().intake()); // this might be switched with back - // Manual climb controls - operator.a().onTrue(robot.getClimb().descend()); // Lower climb - operator.b().onTrue(robot.getClimb().pullUp()); // Lift robot - operator.y().onTrue(robot.getClimb().elevate()); // Raise climb + // // Manual climb controls + // operator.a().onTrue(robot.getClimb().descend()); // Lower climb + // operator.b().onTrue(robot.getClimb().pullUp()); // Lift robot + // operator.y().onTrue(robot.getClimb().elevate()); // Raise climb // Unjam hopper operator.leftBumper().whileTrue(robot.getHopper().unjam()); // Disable shooting - operator.leftTrigger().whileTrue(robot.getShooterRollers().shoot(RotationsPerSecond.of(0))); + // operator.leftTrigger().whileTrue(robot.getShooterRollers().shoot(RotationsPerSecond.of(0))); // Toggle fine control mode operator @@ -136,40 +136,40 @@ public void configureBindings() { fineControl = !fineControl; })); - // Force shooting - operator.rightTrigger().whileTrue(Commands.print("Force Shoot")); - - // Pass fuel to alliance zone - operator.back().whileTrue(Commands.print("Pass Left")); // this might be switched with start - operator.start().whileTrue(Commands.print("Pass Right")); // this might be switched with back - - // Fine control - operator - .povUp() - .and(() -> fineControl) - .whileTrue( - this.robot.getShooterHood().moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE)); - - operator - .povDown() - .and(() -> fineControl) - .whileTrue( - this.robot - .getShooterHood() - .moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); - - operator - .povLeft() - .and(() -> fineControl) - .whileTrue(this.robot.getTurret().moveAtVoltage(TurretConstants.FINE_CONTROL_VOLTAGE)); - - operator - .povLeft() - .and(() -> fineControl) - .whileTrue( - this.robot - .getTurret() - .moveAtVoltage(TurretConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); + // Shoot + operator.rightTrigger().whileTrue(robot.getShooterRollers().shoot(RotationsPerSecond.of(1))); //Incorrect number + + // // Pass fuel to alliance zone + // operator.back().whileTrue(Commands.print("Pass Left")); // this might be switched with start + // operator.start().whileTrue(Commands.print("Pass Right")); // this might be switched with back + + // // Fine control + // operator + // .povUp() + // .and(() -> fineControl) + // .whileTrue( + // this.robot.getShooterHood().moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE)); + + // operator + // .povDown() + // .and(() -> fineControl) + // .whileTrue( + // this.robot + // .getShooterHood() + // .moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); + + // operator + // .povLeft() + // .and(() -> fineControl) + // .whileTrue(this.robot.getTurret().moveAtVoltage(TurretConstants.FINE_CONTROL_VOLTAGE)); + + // operator + // .povLeft() + // .and(() -> fineControl) + // .whileTrue( + // this.robot + // .getTurret() + // .moveAtVoltage(TurretConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); operator .axisGreaterThan(Axis.kLeftX.value, 0.5) @@ -187,18 +187,18 @@ public void configureBindings() { .getIntakeExtension() .moveAtVoltage(IntakeExtensionConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); - operator - .axisGreaterThan(Axis.kRightY.value, 0.5) - .and(() -> fineControl) - .whileTrue(this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE)); + // operator + // .axisGreaterThan(Axis.kRightY.value, 0.5) + // .and(() -> fineControl) + // .whileTrue(this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE)); - operator - .axisLessThan(Axis.kRightY.value, -0.5) - .and(() -> fineControl) - .whileTrue( - this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); + // operator + // .axisLessThan(Axis.kRightY.value, -0.5) + // .and(() -> fineControl) + // .whileTrue( + // this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); - // Intake extension and retraction + // // Intake extension and retraction Trigger intakeRetract = operator.rightStick().or(driver.back()); Trigger intakeExtend = intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled); @@ -208,12 +208,12 @@ public void configureBindings() { robot.getIntakeExtension().extend().alongWith(robot.getIntakeRollers().intake())); // Climb retraction - Command autodescend = robot.getClimb().descend(); - Trigger climbRetract = - new Trigger(() -> TeleopSwerveCommand.isClearToOverride(robot.getClimb(), autodescend)) - .and(RobotState::isTeleop) - .and(RobotState::isEnabled); + // Command autodescend = robot.getClimb().descend(); + // Trigger climbRetract = + // new Trigger(() -> TeleopSwerveCommand.isClearToOverride(robot.getClimb(), autodescend)) + // .and(RobotState::isTeleop) + // .and(RobotState::isEnabled); - climbRetract.onTrue(robot.getClimb().descend()); + // climbRetract.onTrue(robot.getClimb().descend()); } } From 9deb900dcecac69c46b5049095c4501d038edfa2 Mon Sep 17 00:00:00 2001 From: allenx-afk Date: Tue, 24 Feb 2026 18:52:56 -0800 Subject: [PATCH 3/3] Commented out code for week 0 Commented out unused controls, drive to clump, auto climb and fuel simulation, climb simulation, turret hood simulation --- src/main/java/frc/robot/RobotContainer.java | 50 +-- src/main/java/frc/robot/auto/AutoClimb.java | 366 +++++++++--------- .../java/frc/robot/auto/DriveToClump.java | 189 ++++----- .../frc/robot/controls/TeleopControls.java | 29 +- .../visualizer/RobotVisualizer.java | 189 ++++----- 5 files changed, 417 insertions(+), 406 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6a8395b..d7cd20f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,7 @@ import com.team6962.lib.logging.LoggingUtil; import com.team6962.lib.swerve.CommandSwerveDrive; import com.team6962.lib.vision.AprilTagVision; -import com.team6962.lib.vision.SphereClumpLocalization; +// import com.team6962.lib.vision.SphereClumpLocalization; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -20,28 +20,28 @@ import frc.robot.auto.ShooterFunctions; import frc.robot.constants.RobotConstants; import frc.robot.controls.TeleopControls; -import frc.robot.subsystems.climb.Climb; -import frc.robot.subsystems.hood.ShooterHood; +// import frc.robot.subsystems.climb.Climb; +// import frc.robot.subsystems.hood.ShooterHood; import frc.robot.subsystems.hopper.Hopper; import frc.robot.subsystems.intakeextension.IntakeExtension; import frc.robot.subsystems.intakerollers.IntakeRollers; import frc.robot.subsystems.shooterrollers.ShooterRollers; -import frc.robot.subsystems.turret.Turret; +// import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.visualizer.RobotVisualizer; public class RobotContainer { private final RobotConstants constants; private final CommandSwerveDrive swerveDrive; private final TeleopControls teleopControls; - private final Turret turret; + // private final Turret turret; private final DriveStraightAuto driveStraightAuto; private final IntakeExtension intakeExtension; - private final ShooterHood shooterHood; - private final SphereClumpLocalization fuelClumpLocalization; + // private final ShooterHood shooterHood; + // private final SphereClumpLocalization fuelClumpLocalization; private final ShooterRollers shooterRollers; private final IntakeRollers intakeRollers; private final AprilTagVision aprilTagVision; - private final Climb climb; + // private final Climb climb; private final Hopper hopper; private final RobotVisualizer visualizer; private final SendableChooser autoChooser = new SendableChooser<>(); @@ -54,17 +54,17 @@ public RobotContainer() { swerveDrive = new CommandSwerveDrive(constants.getDrivetrainConstants()); - climb = new Climb(); - shooterHood = new ShooterHood(); + // climb = new Climb(); + // shooterHood = new ShooterHood(); intakeRollers = new IntakeRollers(); shooterRollers = new ShooterRollers(); - turret = new Turret(); + // turret = new Turret(); intakeExtension = new IntakeExtension(); hopper = new Hopper(); aprilTagVision = new AprilTagVision(swerveDrive, constants.getAprilTagVisionConstants()); - fuelClumpLocalization = - new SphereClumpLocalization(swerveDrive, constants.getSphereCameraConstants()); + // fuelClumpLocalization = + // new SphereClumpLocalization(swerveDrive, constants.getSphereCameraConstants()); teleopControls = new TeleopControls(this); teleopControls.configureBindings(); @@ -119,9 +119,9 @@ public CommandSwerveDrive getSwerveDrive() { return swerveDrive; } - public Turret getTurret() { - return turret; - } + // public Turret getTurret() { + // return turret; + // } public AprilTagVision getAprilTagVision() { return aprilTagVision; @@ -135,13 +135,13 @@ public void latePeriodic() { swerveDrive.latePeriodic(); } - public SphereClumpLocalization getFuelLocalization() { - return fuelClumpLocalization; - } + // public SphereClumpLocalization getFuelLocalization() { + // return fuelClumpLocalization; + // } - public ShooterHood getShooterHood() { - return shooterHood; - } + // public ShooterHood getShooterHood() { + // return shooterHood; + // } public IntakeRollers getIntakeRollers() { return intakeRollers; @@ -151,9 +151,9 @@ public ShooterRollers getShooterRollers() { return shooterRollers; } - public Climb getClimb() { - return climb; - } + // public Climb getClimb() { + // return climb; + // } public Hopper getHopper() { return hopper; diff --git a/src/main/java/frc/robot/auto/AutoClimb.java b/src/main/java/frc/robot/auto/AutoClimb.java index 0875ed9..d1c1f30 100644 --- a/src/main/java/frc/robot/auto/AutoClimb.java +++ b/src/main/java/frc/robot/auto/AutoClimb.java @@ -1,179 +1,187 @@ -package frc.robot.auto; - -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotContainer; -import java.util.Set; - -public class AutoClimb { - /** - * The pose where the robot is at the end of the pole, but the climb mechanism is not quite over - * the end. This is the climb position that is closest to the blue outpost. - */ - public static Pose2d END_OF_POLE_POSE = - new Pose2d(1.117950, 2.823665, Rotation2d.fromDegrees(-90)); - - /** The center of the blue tower in the Y direction, in meters. */ - public static double CENTER_OF_TOWER = 3.745706; - - /** The velocity at which the robot should be moving at when it reaches the prepare climb pose. */ - public static LinearVelocity ENTER_VELOCITY = MetersPerSecond.of(1.0); - - /** - * The distance away from the pole that the robot should be when it reaches the prepare climb - * pose. - */ - public static Distance ENTER_DISTANCE = Meters.of(0.3); - - /** - * The distance away from the pole that the robot should be when it reaches the align climb pose. - */ - public static Distance ALIGN_DISTANCE = Meters.of(0.1); - - /** - * The distance that the robot should be from the pole when it is considered to have left the - * pole. - */ - public static Distance LEAVE_DISTANCE = Meters.of(0.1); - - /** - * The velocity at which the robot should be moving at when it is considered to have left the - * pole. - */ - public static LinearVelocity LEAVE_VELOCITY = MetersPerSecond.of(0.1); - - /** The distance that the robot should move along the pole to be fully over the pole. */ - public static Distance CLIMB_DISTANCE = Meters.of(0.131467); - - public static enum ClimbSide { - LEFT, - RIGHT - } - - /** - * Gets the pose for climbing based on the side of the pole and the distance along the pole. The - * distance along the pole is how far the robot has moved along the pole. A distance of 0 means - * the robot is at the end of the pole, and a positive distance means the climb mechanism is - * hooked onto the pole. - * - * @param side the side of the pole the robot is climbing on. The right side is the side closest - * to the blue outpost, and the left side is the side closest to the blue depot. - * @param distanceAlongPole the distance in meters along the pole that the robot has climbed. A - * distance of 0 means the robot is at the end of the pole, and a positive distance means the - * climb mechanism is hooked onto the pole. - * @return the climb pose on the given side of the field at the given distance along the pole. - */ - public static Pose2d getClimbPose(ClimbSide side, Distance distanceAlongPole) { - Pose2d pose = END_OF_POLE_POSE; - - pose = new Pose2d(pose.getX(), pose.getY() + distanceAlongPole.in(Meters), pose.getRotation()); - - if (side == ClimbSide.LEFT) { - pose = - new Pose2d( - pose.getX(), 2 * CENTER_OF_TOWER - pose.getY(), pose.getRotation().unaryMinus()); - } - - return pose; - } - - private RobotContainer robot; - - public AutoClimb(RobotContainer robot) { - this.robot = robot; - } - - public Command driveToClimbPose( - ClimbSide side, Distance distanceAlongPole, LinearVelocity velocity) { - double velocityMetersPerSecond = velocity.in(MetersPerSecond); - - return robot - .getSwerveDrive() - .driveTo( - getClimbPose(side, distanceAlongPole), - new ChassisSpeeds( - 0, side == ClimbSide.LEFT ? -velocityMetersPerSecond : velocityMetersPerSecond, 0)); - } - - public Command driveToPrepareClimb(ClimbSide side) { - return driveToClimbPose(side, ENTER_DISTANCE.unaryMinus(), ENTER_VELOCITY); - } - - public Command driveToAlignClimb(ClimbSide side) { - return driveToClimbPose(side, ALIGN_DISTANCE.unaryMinus(), MetersPerSecond.of(0)); - } - - public Command driveOverPole(ClimbSide side) { - return driveToClimbPose(side, CLIMB_DISTANCE, MetersPerSecond.of(0)); - } - - public Command driveToUnclimbPose(ClimbSide side) { - return driveToClimbPose(side, LEAVE_DISTANCE.unaryMinus(), LEAVE_VELOCITY.unaryMinus()); - } - - public Command climb() { - return Commands.defer( - () -> { - ClimbSide side = - robot.getSwerveDrive().getPosition2d().getY() > CENTER_OF_TOWER - ? ClimbSide.LEFT - : ClimbSide.RIGHT; - return Commands.sequence( - Commands.deadline(driveToPrepareClimb(side), robot.getClimb().elevate()), - Commands.deadline( - robot.getClimb().elevate(), - driveToAlignClimb(side) - .repeatedly() - .until( - () -> - robot - .getSwerveDrive() - .isNear( - getClimbPose(side, ALIGN_DISTANCE.unaryMinus()), - Inches.of(0.125), - Degrees.of(2)))), - driveToAlignClimb(side) - .repeatedly() - .until( - () -> - robot - .getSwerveDrive() - .isNear( - getClimbPose(side, ALIGN_DISTANCE.unaryMinus()), - Inches.of(0.125), - Degrees.of(2))), - driveOverPole(side), - robot.getClimb().pullUp()); - }, - Set.of( - robot.getClimb(), - robot.getSwerveDrive().useTranslation(), - robot.getSwerveDrive().useRotation())); - } - - public Command unclimb() { - return Commands.defer( - () -> { - ClimbSide side = - robot.getSwerveDrive().getPosition2d().getY() > CENTER_OF_TOWER - ? ClimbSide.LEFT - : ClimbSide.RIGHT; - - return Commands.sequence(robot.getClimb().elevate(), driveToUnclimbPose(side)); - }, - Set.of( - robot.getClimb(), - robot.getSwerveDrive().useTranslation(), - robot.getSwerveDrive().useRotation())); - } -} +// package frc.robot.auto; + +// import static edu.wpi.first.units.Units.Degrees; +// import static edu.wpi.first.units.Units.Inches; +// import static edu.wpi.first.units.Units.Meters; +// import static edu.wpi.first.units.Units.MetersPerSecond; + +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.units.measure.Distance; +// import edu.wpi.first.units.measure.LinearVelocity; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Commands; +// import frc.robot.RobotContainer; +// import java.util.Set; + +// public class AutoClimb { +// /** +// * The pose where the robot is at the end of the pole, but the climb mechanism is not quite +// over +// * the end. This is the climb position that is closest to the blue outpost. +// */ +// public static Pose2d END_OF_POLE_POSE = +// new Pose2d(1.117950, 2.823665, Rotation2d.fromDegrees(-90)); + +// /** The center of the blue tower in the Y direction, in meters. */ +// public static double CENTER_OF_TOWER = 3.745706; + +// /** The velocity at which the robot should be moving at when it reaches the prepare climb pose. +// */ +// public static LinearVelocity ENTER_VELOCITY = MetersPerSecond.of(1.0); + +// /** +// * The distance away from the pole that the robot should be when it reaches the prepare climb +// * pose. +// */ +// public static Distance ENTER_DISTANCE = Meters.of(0.3); + +// /** +// * The distance away from the pole that the robot should be when it reaches the align climb +// pose. +// */ +// public static Distance ALIGN_DISTANCE = Meters.of(0.1); + +// /** +// * The distance that the robot should be from the pole when it is considered to have left the +// * pole. +// */ +// public static Distance LEAVE_DISTANCE = Meters.of(0.1); + +// /** +// * The velocity at which the robot should be moving at when it is considered to have left the +// * pole. +// */ +// public static LinearVelocity LEAVE_VELOCITY = MetersPerSecond.of(0.1); + +// /** The distance that the robot should move along the pole to be fully over the pole. */ +// public static Distance CLIMB_DISTANCE = Meters.of(0.131467); + +// public static enum ClimbSide { +// LEFT, +// RIGHT +// } + +// /** +// * Gets the pose for climbing based on the side of the pole and the distance along the pole. +// The +// * distance along the pole is how far the robot has moved along the pole. A distance of 0 means +// * the robot is at the end of the pole, and a positive distance means the climb mechanism is +// * hooked onto the pole. +// * +// * @param side the side of the pole the robot is climbing on. The right side is the side +// closest +// * to the blue outpost, and the left side is the side closest to the blue depot. +// * @param distanceAlongPole the distance in meters along the pole that the robot has climbed. A +// * distance of 0 means the robot is at the end of the pole, and a positive distance means +// the +// * climb mechanism is hooked onto the pole. +// * @return the climb pose on the given side of the field at the given distance along the pole. +// */ +// public static Pose2d getClimbPose(ClimbSide side, Distance distanceAlongPole) { +// Pose2d pose = END_OF_POLE_POSE; + +// pose = new Pose2d(pose.getX(), pose.getY() + distanceAlongPole.in(Meters), +// pose.getRotation()); + +// if (side == ClimbSide.LEFT) { +// pose = +// new Pose2d( +// pose.getX(), 2 * CENTER_OF_TOWER - pose.getY(), pose.getRotation().unaryMinus()); +// } + +// return pose; +// } + +// private RobotContainer robot; + +// public AutoClimb(RobotContainer robot) { +// this.robot = robot; +// } + +// public Command driveToClimbPose( +// ClimbSide side, Distance distanceAlongPole, LinearVelocity velocity) { +// double velocityMetersPerSecond = velocity.in(MetersPerSecond); + +// return robot +// .getSwerveDrive() +// .driveTo( +// getClimbPose(side, distanceAlongPole), +// new ChassisSpeeds( +// 0, side == ClimbSide.LEFT ? -velocityMetersPerSecond : velocityMetersPerSecond, +// 0)); +// } + +// public Command driveToPrepareClimb(ClimbSide side) { +// return driveToClimbPose(side, ENTER_DISTANCE.unaryMinus(), ENTER_VELOCITY); +// } + +// public Command driveToAlignClimb(ClimbSide side) { +// return driveToClimbPose(side, ALIGN_DISTANCE.unaryMinus(), MetersPerSecond.of(0)); +// } + +// public Command driveOverPole(ClimbSide side) { +// return driveToClimbPose(side, CLIMB_DISTANCE, MetersPerSecond.of(0)); +// } + +// public Command driveToUnclimbPose(ClimbSide side) { +// return driveToClimbPose(side, LEAVE_DISTANCE.unaryMinus(), LEAVE_VELOCITY.unaryMinus()); +// } + +// public Command climb() { +// return Commands.defer( +// () -> { +// ClimbSide side = +// robot.getSwerveDrive().getPosition2d().getY() > CENTER_OF_TOWER +// ? ClimbSide.LEFT +// : ClimbSide.RIGHT; +// return Commands.sequence( +// Commands.deadline(driveToPrepareClimb(side), robot.getClimb().elevate()), +// Commands.deadline( +// robot.getClimb().elevate(), +// driveToAlignClimb(side) +// .repeatedly() +// .until( +// () -> +// robot +// .getSwerveDrive() +// .isNear( +// getClimbPose(side, ALIGN_DISTANCE.unaryMinus()), +// Inches.of(0.125), +// Degrees.of(2)))), +// driveToAlignClimb(side) +// .repeatedly() +// .until( +// () -> +// robot +// .getSwerveDrive() +// .isNear( +// getClimbPose(side, ALIGN_DISTANCE.unaryMinus()), +// Inches.of(0.125), +// Degrees.of(2))), +// driveOverPole(side), +// robot.getClimb().pullUp()); +// }, +// Set.of( +// robot.getClimb(), +// robot.getSwerveDrive().useTranslation(), +// robot.getSwerveDrive().useRotation())); +// } + +// public Command unclimb() { +// return Commands.defer( +// () -> { +// ClimbSide side = +// robot.getSwerveDrive().getPosition2d().getY() > CENTER_OF_TOWER +// ? ClimbSide.LEFT +// : ClimbSide.RIGHT; + +// return Commands.sequence(robot.getClimb().elevate(), driveToUnclimbPose(side)); +// }, +// Set.of( +// robot.getClimb(), +// robot.getSwerveDrive().useTranslation(), +// robot.getSwerveDrive().useRotation())); +// } +// } diff --git a/src/main/java/frc/robot/auto/DriveToClump.java b/src/main/java/frc/robot/auto/DriveToClump.java index 8dddd36..5f1be4e 100644 --- a/src/main/java/frc/robot/auto/DriveToClump.java +++ b/src/main/java/frc/robot/auto/DriveToClump.java @@ -1,102 +1,107 @@ -package frc.robot.auto; +// package frc.robot.auto; -import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import frc.robot.RobotContainer; -import java.util.Set; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Translation2d; +// import edu.wpi.first.math.kinematics.ChassisSpeeds; +// import edu.wpi.first.wpilibj2.command.Command; +// import edu.wpi.first.wpilibj2.command.Commands; +// import frc.robot.RobotContainer; +// import java.util.Set; -public class DriveToClump { - private RobotContainer robot; +// public class DriveToClump { +// private RobotContainer robot; - public DriveToClump(RobotContainer robot) { - this.robot = robot; - } +// public DriveToClump(RobotContainer robot) { +// this.robot = robot; +// } - /** - * Creates a command that repeatedly attempts to drive the robot to a detected clump of fuel while - * running the intake rollers when the intake is extended. - * - *

Behavior: - * - *

    - *
  • On each execution, the command queries the provided {@code fuelClumpLocalization} for the - * current clump position. - *
  • If no clump position is available (null), the command does nothing for that iteration. - *
  • If a clump position is available and the intake extension reports it is extended, the - * command runs the intake rollers and commands the swerve drive to drive to the clump - * position in parallel. - *
  • If a clump position is available but the intake is not extended, the command does nothing - * for that iteration. - *
  • The composed command is created with {@code Commands.defer(...).repeatedly()}, so the - * check-and-act behavior repeats until the returned command is cancelled or interrupted. - *
- * - * @param intakeExtension subsystem that reports whether the intake is extended; used as the - * condition to decide whether to intake and drive - * @param intakeRollers subsystem used to run the intake rollers when approaching a clump - * @param fuelClumpLocalization provider used to obtain the current Translation2d position of the - * detected fuel clump (may return {@code null} if no fuel is visible) - * @param swerveDrive swerve-drive command provider used to drive to the detected fuel clump - * position - * @return a {@code Command} that repeatedly checks for a clump and, when appropriate, runs the - * intake rollers while driving to the fuel clump; returns {@code Commands.none()} for - * iterations where no action is taken - */ - public Command driveToClump() { - return Commands.defer( - () -> { - Translation2d fuelPosition = robot.getFuelLocalization().getClumpPosition(); - if (fuelPosition == null) { - return Commands.none(); - } +// /** +// * Creates a command that repeatedly attempts to drive the robot to a detected clump of fuel +// while +// * running the intake rollers when the intake is extended. +// * +// *

Behavior: +// * +// *

    +// *
  • On each execution, the command queries the provided {@code fuelClumpLocalization} for +// the +// * current clump position. +// *
  • If no clump position is available (null), the command does nothing for that iteration. +// *
  • If a clump position is available and the intake extension reports it is extended, the +// * command runs the intake rollers and commands the swerve drive to drive to the clump +// * position in parallel. +// *
  • If a clump position is available but the intake is not extended, the command does +// nothing +// * for that iteration. +// *
  • The composed command is created with {@code Commands.defer(...).repeatedly()}, so the +// * check-and-act behavior repeats until the returned command is cancelled or interrupted. +// *
+// * +// * @param intakeExtension subsystem that reports whether the intake is extended; used as the +// * condition to decide whether to intake and drive +// * @param intakeRollers subsystem used to run the intake rollers when approaching a clump +// * @param fuelClumpLocalization provider used to obtain the current Translation2d position of +// the +// * detected fuel clump (may return {@code null} if no fuel is visible) +// * @param swerveDrive swerve-drive command provider used to drive to the detected fuel clump +// * position +// * @return a {@code Command} that repeatedly checks for a clump and, when appropriate, runs the +// * intake rollers while driving to the fuel clump; returns {@code Commands.none()} for +// * iterations where no action is taken +// */ +// public Command driveToClump() { +// return Commands.defer( +// () -> { +// Translation2d fuelPosition = robot.getFuelLocalization().getClumpPosition(); +// if (fuelPosition == null) { +// return Commands.none(); +// } - Translation2d error = - fuelPosition.minus(robot.getSwerveDrive().getPosition2d().getTranslation()); +// Translation2d error = +// fuelPosition.minus(robot.getSwerveDrive().getPosition2d().getTranslation()); - double maxVelocity = - robot - .getConstants() - .getDrivetrainConstants() - .Driving - .MaxLinearVelocity - .in(MetersPerSecond); - double maxAcceleration = - robot - .getConstants() - .getDrivetrainConstants() - .Driving - .MaxLinearAcceleration - .in(MetersPerSecondPerSecond); +// double maxVelocity = +// robot +// .getConstants() +// .getDrivetrainConstants() +// .Driving +// .MaxLinearVelocity +// .in(MetersPerSecond); +// double maxAcceleration = +// robot +// .getConstants() +// .getDrivetrainConstants() +// .Driving +// .MaxLinearAcceleration +// .in(MetersPerSecondPerSecond); - double distance = error.getNorm(); - double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * distance)); +// double distance = error.getNorm(); +// double finalSpeed = Math.min(maxVelocity, Math.sqrt(2 * maxAcceleration * +// distance)); - ChassisSpeeds finalVelocity = - new ChassisSpeeds( - error.getX() / distance * finalSpeed, - error.getY() / distance * finalSpeed, - 0); +// ChassisSpeeds finalVelocity = +// new ChassisSpeeds( +// error.getX() / distance * finalSpeed, +// error.getY() / distance * finalSpeed, +// 0); - return Commands.either( - Commands.deadline( - robot - .getSwerveDrive() - .driveTo(new Pose2d(fuelPosition, error.getAngle()), finalVelocity), - robot.getIntakeRollers().intake()), - Commands.none(), - robot.getIntakeExtension()::isExtended); - }, - Set.of( - robot.getIntakeExtension(), - robot.getIntakeRollers(), - robot.getSwerveDrive().useTranslation(), - robot.getSwerveDrive().useRotation())) - .repeatedly(); - } -} +// return Commands.either( +// Commands.deadline( +// robot +// .getSwerveDrive() +// .driveTo(new Pose2d(fuelPosition, error.getAngle()), finalVelocity), +// robot.getIntakeRollers().intake()), +// Commands.none(), +// robot.getIntakeExtension()::isExtended); +// }, +// Set.of( +// robot.getIntakeExtension(), +// robot.getIntakeRollers(), +// robot.getSwerveDrive().useTranslation(), +// robot.getSwerveDrive().useRotation())) +// .repeatedly(); +// } +// } diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index 618e5e1..d8ec23d 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -1,12 +1,8 @@ package frc.robot.controls; -// import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RotationsPerSecond; -// import com.team6962.lib.swerve.commands.TeleopSwerveCommand; import com.team6962.lib.swerve.commands.XBoxTeleopSwerveCommand; -// import edu.wpi.first.math.geometry.Pose2d; -// import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.RobotState; @@ -16,17 +12,12 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.RobotContainer; -// import frc.robot.auto.AutoClimb; -// import frc.robot.auto.DriveToClump; -// import frc.robot.subsystems.climb.ClimbConstants; -// import frc.robot.subsystems.hood.ShooterHoodConstants; import frc.robot.subsystems.intakeextension.IntakeExtensionConstants; -// import frc.robot.subsystems.turret.TurretConstants; public class TeleopControls { private RobotContainer robot; -// private AutoClimb autoClimb; -// private DriveToClump driveToClump; + // private AutoClimb autoClimb; + // private DriveToClump driveToClump; private CommandXboxController driver = new CommandXboxController(0); private CommandXboxController operator = new CommandXboxController(1); @@ -79,7 +70,8 @@ public void configureBindings() { // 5.947, // new Rotation2d( // Radians.of( - // Math.PI)))), // rough position estimate based on simulation, not + // Math.PI)))), // rough position estimate based on simulation, + // not // // exact // this.robot.getIntakeExtension().extend(), // Commands.parallel( @@ -99,7 +91,8 @@ public void configureBindings() { // .getSwerveDrive() // .driveTo( // new Pose2d( - // 0.6, 0.65, new Rotation2d(Radians.of(Math.PI))))); // also a rough estimate + // 0.6, 0.65, new Rotation2d(Radians.of(Math.PI))))); // also a rough + // estimate // Dump fuel driver @@ -137,7 +130,9 @@ public void configureBindings() { })); // Shoot - operator.rightTrigger().whileTrue(robot.getShooterRollers().shoot(RotationsPerSecond.of(1))); //Incorrect number + operator + .rightTrigger() + .whileTrue(robot.getShooterRollers().shoot(RotationsPerSecond.of(1))); // Incorrect number // // Pass fuel to alliance zone // operator.back().whileTrue(Commands.print("Pass Left")); // this might be switched with start @@ -148,7 +143,8 @@ public void configureBindings() { // .povUp() // .and(() -> fineControl) // .whileTrue( - // this.robot.getShooterHood().moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE)); + // + // this.robot.getShooterHood().moveAtVoltage(ShooterHoodConstants.FINE_CONTROL_VOLTAGE)); // operator // .povDown() @@ -196,7 +192,8 @@ public void configureBindings() { // .axisLessThan(Axis.kRightY.value, -0.5) // .and(() -> fineControl) // .whileTrue( - // this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); + // + // this.robot.getClimb().moveAtVoltage(ClimbConstants.FINE_CONTROL_VOLTAGE.unaryMinus())); // // Intake extension and retraction Trigger intakeRetract = operator.rightStick().or(driver.back()); diff --git a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java index 17a7844..75a9b46 100644 --- a/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java +++ b/src/main/java/frc/robot/subsystems/visualizer/RobotVisualizer.java @@ -1,9 +1,9 @@ package frc.robot.subsystems.visualizer; -import static edu.wpi.first.units.Units.Degrees; +// import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.MetersPerSecond; +// import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Volts; @@ -11,19 +11,20 @@ import dev.doglog.DogLog; 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.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.units.measure.Angle; +// import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; -import frc.robot.subsystems.hood.ShooterHoodConstants; -import java.util.ArrayList; -import java.util.List; +// import frc.robot.subsystems.hood.ShooterHoodConstants; +// import java.util.ArrayList; +// import java.util.List; import org.ironmaple.simulation.IntakeSimulation; -import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; + +// import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly; /** Displays the articulated components on the robot in AdvantageScope. */ public class RobotVisualizer extends SubsystemBase { @@ -51,23 +52,23 @@ public RobotVisualizer(RobotContainer robot) { 48); } - private Translation3d getFuelTranslation(int index) { - double spacing = RobotVisualizationConstants.fuelSpacing.in(Meters); - Translation3d basePose = new Translation3d(0, -spacing * 1.5, 0.1); + // private Translation3d getFuelTranslation(int index) { + // double spacing = RobotVisualizationConstants.fuelSpacing.in(Meters); + // Translation3d basePose = new Translation3d(0, -spacing * 1.5, 0.1); - int length = - robot.getIntakeExtension().isExtended() - ? RobotVisualizationConstants.hopperExtendedLength - : RobotVisualizationConstants.hopperRetractedLength; - int width = RobotVisualizationConstants.hopperWidth; + // int length = + // robot.getIntakeExtension().isExtended() + // ? RobotVisualizationConstants.hopperExtendedLength + // : RobotVisualizationConstants.hopperRetractedLength; + // int width = RobotVisualizationConstants.hopperWidth; - int layer = index / length / width; - int indexInLayer = index % (length * width); - int row = indexInLayer / length; - int column = indexInLayer % length; + // int layer = index / length / width; + // int indexInLayer = index % (length * width); + // int row = indexInLayer / length; + // int column = indexInLayer % length; - return basePose.plus(new Translation3d(column * spacing, row * spacing, layer * spacing)); - } + // return basePose.plus(new Translation3d(column * spacing, row * spacing, layer * spacing)); + // } @Override public void periodic() { @@ -82,70 +83,70 @@ public void periodic() { } // Visualize fuel in robot - int fuelCount = intakeSim.getGamePiecesAmount(); - - if (!robot.getIntakeExtension().isExtended() - && fuelCount > RobotVisualizationConstants.maxRetractedFuel) { - for (int i = 0; i < fuelCount - RobotVisualizationConstants.maxRetractedFuel; i++) { - Translation3d fuelTranslation = getFuelTranslation(i); - - robot - .getSwerveDrive() - .getSimulation() - .getMapleSim() - .getArena() - .addGamePieceProjectile( - new RebuiltFuelOnFly( - robot.getSwerveDrive().getPosition2d().getTranslation(), - fuelTranslation - .toTranslation2d() - .rotateBy(robot.getSwerveDrive().getPosition2d().getRotation()), - robot.getSwerveDrive().getVelocity(), - Rotation2d.fromRotations(Math.random()), - Meters.of(fuelTranslation.getZ()), - MetersPerSecond.of(Math.random() * 2.0 + 2.5), - Degrees.of(65.0 + Math.random() * 20.0))); - } - - intakeSim.setGamePiecesCount(RobotVisualizationConstants.maxRetractedFuel); - fuelCount = RobotVisualizationConstants.maxRetractedFuel; - } - - List fuelPositions = - robot - .getSwerveDrive() - .getSimulation() - .getMapleSim() - .getArena() - .getGamePiecesByType("Fuel") - .stream() - .map(gamePiece -> gamePiece.getPose3d().getTranslation()) - .toList(); - - robot.getFuelLocalization().setSimulatedSpherePositions(fuelPositions); - - List additionalFuelPoses = new ArrayList<>(); - - for (int i = 0; i < fuelCount; i++) { - Translation3d fuelTranslation = getFuelTranslation(i); - additionalFuelPoses.add(new Pose3d(fuelTranslation, new Rotation3d())); - } - - robot - .getSwerveDrive() - .getSimulation() - .getMapleSim() - .setHeldFuelPoses(additionalFuelPoses.toArray(Pose3d[]::new)); - - DogLog.log("RobotVisualizer/HeldFuelCount", fuelCount); + // int fuelCount = intakeSim.getGamePiecesAmount(); + + // if (!robot.getIntakeExtension().isExtended() + // && fuelCount > RobotVisualizationConstants.maxRetractedFuel) { + // for (int i = 0; i < fuelCount - RobotVisualizationConstants.maxRetractedFuel; i++) { + // Translation3d fuelTranslation = getFuelTranslation(i); + + // robot + // .getSwerveDrive() + // .getSimulation() + // .getMapleSim() + // .getArena() + // .addGamePieceProjectile( + // new RebuiltFuelOnFly( + // robot.getSwerveDrive().getPosition2d().getTranslation(), + // fuelTranslation + // .toTranslation2d() + // .rotateBy(robot.getSwerveDrive().getPosition2d().getRotation()), + // robot.getSwerveDrive().getVelocity(), + // Rotation2d.fromRotations(Math.random()), + // Meters.of(fuelTranslation.getZ()), + // MetersPerSecond.of(Math.random() * 2.0 + 2.5), + // Degrees.of(65.0 + Math.random() * 20.0))); + // } + + // intakeSim.setGamePiecesCount(RobotVisualizationConstants.maxRetractedFuel); + // fuelCount = RobotVisualizationConstants.maxRetractedFuel; + // } + + // List fuelPositions = + // robot + // .getSwerveDrive() + // .getSimulation() + // .getMapleSim() + // .getArena() + // .getGamePiecesByType("Fuel") + // .stream() + // .map(gamePiece -> gamePiece.getPose3d().getTranslation()) + // .toList(); + + // robot.getFuelLocalization().setSimulatedSpherePositions(fuelPositions); + + // List additionalFuelPoses = new ArrayList<>(); + + // for (int i = 0; i < fuelCount; i++) { + // Translation3d fuelTranslation = getFuelTranslation(i); + // additionalFuelPoses.add(new Pose3d(fuelTranslation, new Rotation3d())); + // } + + // robot + // .getSwerveDrive() + // .getSimulation() + // .getMapleSim() + // .setHeldFuelPoses(additionalFuelPoses.toArray(Pose3d[]::new)); + + // DogLog.log("RobotVisualizer/HeldFuelCount", fuelCount); } finally { robot.getSwerveDrive().getSimulation().getArenaLock().unlock(); } // Get robot state - Angle turretAngle = robot.getTurret().getPosition(); - Angle hoodAngle = robot.getShooterHood().getPosition(); - Distance climbPosition = robot.getClimb().getPosition(); + // Angle turretAngle = robot.getTurret().getPosition(); + // Angle hoodAngle = robot.getShooterHood().getPosition(); + // Distance climbPosition = robot.getClimb().getPosition(); Distance intakePosition = robot.getIntakeExtension().getPosition(); SwerveModulePosition[] modulePositions = robot.getSwerveDrive().getModulePositions(); @@ -165,18 +166,18 @@ public void periodic() { } // Shooter and hood pose - Pose3d shooterPose = - new Pose3d( - RobotVisualizationConstants.shooterTranslation, - new Rotation3d(0, 0, turretAngle.in(Radians))); - Pose3d shooterRelativeHoodPose = - new Pose3d( - RobotVisualizationConstants.hoodTranslation, - new Rotation3d(0, hoodAngle.minus(ShooterHoodConstants.MIN_ANGLE).in(Radians), 0)); - Pose3d hoodPose = shooterPose.plus(shooterRelativeHoodPose.minus(new Pose3d())); - - poses[4] = shooterPose; - poses[8] = hoodPose; + // Pose3d shooterPose = + // new Pose3d( + // RobotVisualizationConstants.shooterTranslation, + // new Rotation3d(0, 0, turretAngle.in(Radians))); + // Pose3d shooterRelativeHoodPose = + // new Pose3d( + // RobotVisualizationConstants.hoodTranslation, + // new Rotation3d(0, hoodAngle.minus(ShooterHoodConstants.MIN_ANGLE).in(Radians), 0)); + // Pose3d hoodPose = shooterPose.plus(shooterRelativeHoodPose.minus(new Pose3d())); + + // poses[4] = shooterPose; + // poses[8] = hoodPose; // Intake and extending hopper wall poses double intakeHorizontalDistance = @@ -191,7 +192,7 @@ public void periodic() { new Rotation3d()); // Climb pose - poses[7] = new Pose3d(new Translation3d(0, 0, climbPosition.in(Meters)), new Rotation3d()); + // poses[7] = new Pose3d(new Translation3d(0, 0, climbPosition.in(Meters)), new Rotation3d()); // Log the poses to NetworkTables for visualization in AdvantageScope DogLog.log("RobotVisualizer/ArticulatedComponents", poses);