From 9a58b542773b3117403277898227cf6e421474ff Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 16 Apr 2026 15:58:14 -0400 Subject: [PATCH] Distance measurements --- simgui-ds.json | 3 +-- .../pathplanner/autos/Shoot 8. Hub.auto | 2 +- .../paths/S. Hub. shoot 8. E-Hub.path | 6 +++--- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/Utils.java | 6 +++--- .../frc/robot/commands/AutoTurretCommand.java | 19 +++++++++++++---- .../commands/AutoTurretShootCommand.java | 21 +++++++++++-------- .../frc/robot/subsystems/TurretSubsystem.java | 6 ++---- 9 files changed, 41 insertions(+), 28 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 1f4a549..512b0fe 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -99,8 +99,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto index 216b0a1..3cffb68 100644 --- a/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto +++ b/src/main/deploy/pathplanner/autos/Shoot 8. Hub.auto @@ -23,7 +23,7 @@ { "type": "named", "data": { - "name": "ChassisAlignShoot" + "name": "TurretShoot" } } ] diff --git a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path index 07406af..7d46429 100644 --- a/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. Hub. shoot 8. E-Hub.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 2.9483700538099855, + "x": 2.677383349903843, "y": 4.00221686746988 }, "prevControl": { - "x": 2.5986833068220334, - "y": 4.024072289156626 + "x": 2.4274139067638107, + "y": 4.006125511257969 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 218b824..da05f41 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -139,7 +139,7 @@ public final class Constants { public static final int TURRET_LIMIT_CHANNEL = 9; public static final Angle LIMIT_AXIMUTH_POS = edu.wpi.first.units.Units.Degrees.of(165); public static final Angle LIMIT_AXIMUTH_NEG = edu.wpi.first.units.Units.Degrees.of(5); - public static final double HOMING_SPEED = .35; + public static final double HOMING_SPEED = .05; public static final double TURRET_GEAR_RATIO = 6; public static final int HOOD_SERVO_CHANNEL = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 50846b9..cd578a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,6 +55,7 @@ public class RobotContainer { NamedCommands.registerCommand("ChassisAlignShoot", new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); NamedCommands.registerCommand("RunIntake", intakeSubsystem.intake()); + NamedCommands.registerCommand("TurretShoot", new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); turretSubsystem.setDefaultCommand(new AutoTurretCommand(turretSubsystem, driveSubsystem)); } @@ -82,8 +83,9 @@ public class RobotContainer { //m_driverController.a().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem)); m_driverController.start().whileTrue(intakeSubsystem.resetDeploy()); // m_driverController.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.a().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); m_driverController.x().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); - m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem)); + m_driverController.b().whileTrue(new Reverse(intakeSubsystem, hopperSubsystem)); m_driverController.y().onTrue(intakeSubsystem.deploy()); } diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java index d034284..06a1a91 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/src/main/java/frc/robot/Utils.java @@ -95,14 +95,14 @@ public class Utils { } public static double getHoodAngleForDistance(double distanceInMeters){ - if(distanceInMeters >= 0){ - return 0; + if(distanceInMeters >= 3){ + return 200; } return 0; } public static double getLauncherRPMForDistance(double distanceInMeters){ - return (667.557*distanceInMeters) + 1024.699; + return Math.min((667.557*distanceInMeters) + 1024.699, 6000); } /** diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 6140e59..b0c5da9 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -17,6 +17,8 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; +import frc.robot.Constants.ShootingRegion; +import frc.robot.Utils; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.TurretSubsystem; @@ -27,7 +29,7 @@ public class AutoTurretCommand extends Command { private TurretSubsystem turretSubsystem; private DriveSubsystem driveSubsystem; - private int[] shootingTagFilters; + private Alliance alliance; public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) { this.turretSubsystem = turretSubsystem; @@ -43,13 +45,12 @@ public class AutoTurretCommand extends Command { DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { - shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); + aliance = Alliance.Blue; } else { - shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE); + aliance = Alliance.Red; } - Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose()); }); } @@ -57,6 +58,16 @@ public class AutoTurretCommand extends Command { @Override public void execute() { + ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), DriverStation.getAlliance().get()); + + if(shootingRegion != ShootingRegion.NON_SHOOTING_REGION){ + Pose3d targetPose = Utils.findShootingTarget(shootingRegion, DriverStation.getAlliance().get()); + + turretSubsystem.setTurretTargetPose(targetPose); + } + + Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose()); + Pose3d botPose = driveSubsystem.getBotPose(); Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5)))); diff --git a/src/main/java/frc/robot/commands/AutoTurretShootCommand.java b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java index 162071d..7681348 100644 --- a/src/main/java/frc/robot/commands/AutoTurretShootCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java @@ -2,6 +2,7 @@ package frc.robot.commands; import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DriverStation; @@ -11,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; -import frc.robot.Constants.ShootingRegion; import frc.robot.Utils; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -28,7 +28,6 @@ public class AutoTurretShootCommand extends Command{ private Command waitCommand = Commands.waitSeconds(1.5); private Alliance alliance; - public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){ this.hopperSubsystem = hopperSubsystem; this.turretSubsystem = turretSubsystem; @@ -47,16 +46,11 @@ public class AutoTurretShootCommand extends Command{ @Override public void execute() { - - ShootingRegion shootingRegion = Utils.findRobotShootingRegion(driveSubsystem.getBotPose(), alliance); - - // if(shootingRegion == ) - double distance = turretSubsystem.getTurretTargetPose().toPose2d().getTranslation().getDistance(driveSubsystem.getBotPose().getTranslation().toTranslation2d()); Logger.recordOutput("AutoTurretShoot-DistanceToTarget(Meters)", distance); - turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(0)); - turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(0)); + turretSubsystem.setTargetElevation(Utils.getHoodAngleForDistance(distance)); + turretSubsystem.setLauncher(Utils.getLauncherRPMForDistance(distance)); if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){ hopperSubsystem.feedKicker(); @@ -78,6 +72,15 @@ public class AutoTurretShootCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); + + DriverStation.getAlliance().ifPresent((aliance) -> { + if (aliance == Alliance.Blue) { + turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); + } else { + turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE); + } + }); + // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY } diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 85d43de..f45b61e 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -206,14 +206,12 @@ public class TurretSubsystem extends TestableSubsystem { if(turretState == TurretState.HOMING) { - turretMotor.set(0.05); - if(turretLimitSwitch.get()) { // Turret has tripped the switch turretMotor.stopMotor(); turretOffsetInRotations = (turretMotorPosition.refresh().getValueAsDouble() - 1.323); if(!Robot.isReal()){ - turretOffsetInRotations = 1.5; // Flip turret around + turretOffsetInRotations = 0; // Flip turret around } turretState = TurretState.READY; forwardLimit = (1.5 + turretOffsetInRotations); @@ -301,7 +299,7 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation } turretMotor.setControl(turretControlMode.withPosition(posWithOffset)); - turretSim.setRawRotorPosition(posWithOffset); + turretSim.setRawRotorPosition(-posWithOffset); } public boolean isTurretAtTarget(){