From ed6ebb67c1b7240eca04d5175bcd6df5d2a7c723 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 16 Apr 2026 13:23:59 -0400 Subject: [PATCH] IDK --- .../deploy/pathplanner/autos/HUB_START.auto | 19 +++++++ .../pathplanner/autos/LEFT_TRENCH_START.auto | 19 +++++++ .../pathplanner/autos/RIGHT_TRENCH_START.auto | 19 +++++++ .../pathplanner/paths/1.1 Hub to Shoot.path | 2 +- .../paths/1.2 Shoot to Depot Path.path | 2 +- .../paths/9.1 Chaos Monkey Path.path | 4 +- .../deploy/pathplanner/paths/HUB_START.path | 54 +++++++++++++++++++ .../pathplanner/paths/LEFT_TRENCH_START.path | 54 +++++++++++++++++++ .../deploy/pathplanner/paths/M. Depot. .path | 2 +- .../pathplanner/paths/RIGHT_TRENCH_START.path | 54 +++++++++++++++++++ .../pathplanner/paths/Relay. L-Tr..path | 2 +- .../pathplanner/paths/Relay. R-Tr..path | 4 +- .../paths/S. Hub. shoot 8. E-Hub.path | 2 +- .../pathplanner/paths/S. L-Tre. Preload..path | 4 +- .../pathplanner/paths/S. R-Trench. E-Hub.path | 2 +- src/main/deploy/pathplanner/paths/Spin.path | 2 +- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/Utils.java | 10 ++++ .../frc/robot/commands/AutoTurretCommand.java | 11 ++-- ...mmand.java => AutoTurretShootCommand.java} | 32 ++++++++--- .../frc/robot/subsystems/DriveSubsystem.java | 8 +-- .../frc/robot/subsystems/TurretSubsystem.java | 18 ++++++- 23 files changed, 298 insertions(+), 32 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/HUB_START.auto create mode 100644 src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto create mode 100644 src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto create mode 100644 src/main/deploy/pathplanner/paths/HUB_START.path create mode 100644 src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path create mode 100644 src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path rename src/main/java/frc/robot/commands/{RelayCommand.java => AutoTurretShootCommand.java} (56%) diff --git a/src/main/deploy/pathplanner/autos/HUB_START.auto b/src/main/deploy/pathplanner/autos/HUB_START.auto new file mode 100644 index 0000000..90fb0e0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/HUB_START.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "HUB_START" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto b/src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto new file mode 100644 index 0000000..b8aef49 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LEFT_TRENCH_START.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LEFT_TRENCH_START" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto b/src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto new file mode 100644 index 0000000..4ef3703 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/RIGHT_TRENCH_START.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "RIGHT_TRENCH_START" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path b/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path index 47a2903..23e1232 100644 --- a/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path +++ b/src/main/deploy/pathplanner/paths/1.1 Hub to Shoot.path @@ -35,7 +35,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path b/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path index 55c2201..554ebf4 100644 --- a/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path +++ b/src/main/deploy/pathplanner/paths/1.2 Shoot to Depot Path.path @@ -35,7 +35,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path b/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path index b209333..821bbf1 100644 --- a/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path +++ b/src/main/deploy/pathplanner/paths/9.1 Chaos Monkey Path.path @@ -83,14 +83,14 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": -1.2282908533168926 + "rotation": -1.2282908533168928 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/HUB_START.path b/src/main/deploy/pathplanner/paths/HUB_START.path new file mode 100644 index 0000000..1d9ba33 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/HUB_START.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.519556041182133, + "y": 4.059804750409042 + }, + "prevControl": null, + "nextControl": { + "x": 3.769556041182133, + "y": 4.059804750409042 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.519556041182133, + "y": 4.059804750409042 + }, + "prevControl": { + "x": 3.269556041182133, + "y": 4.059804750409042 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path b/src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path new file mode 100644 index 0000000..f5b86c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LEFT_TRENCH_START.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 7.423545951843262 + }, + "prevControl": null, + "nextControl": { + "x": 4.25, + "y": 7.423545951843262 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 7.423545951843262 + }, + "prevControl": { + "x": 3.75, + "y": 7.423545951843262 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/M. Depot. .path b/src/main/deploy/pathplanner/paths/M. Depot. .path index 2e111ae..cc99241 100644 --- a/src/main/deploy/pathplanner/paths/M. Depot. .path +++ b/src/main/deploy/pathplanner/paths/M. Depot. .path @@ -88,7 +88,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path b/src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path new file mode 100644 index 0000000..9d17709 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RIGHT_TRENCH_START.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0, + "y": 0.6236959156237152 + }, + "prevControl": null, + "nextControl": { + "x": 4.25, + "y": 0.6236959156237152 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 0.6236959156237152 + }, + "prevControl": { + "x": 3.75, + "y": 0.6236959156237152 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Relay. L-Tr..path b/src/main/deploy/pathplanner/paths/Relay. L-Tr..path index 4c91300..bd0ea9e 100644 --- a/src/main/deploy/pathplanner/paths/Relay. L-Tr..path +++ b/src/main/deploy/pathplanner/paths/Relay. L-Tr..path @@ -174,7 +174,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path index abea361..ffc2c03 100644 --- a/src/main/deploy/pathplanner/paths/Relay. R-Tr..path +++ b/src/main/deploy/pathplanner/paths/Relay. R-Tr..path @@ -85,7 +85,7 @@ }, "prevControl": { "x": 6.020824888067715, - "y": 0.8531028227756466 + "y": 0.8531028227756465 }, "nextControl": { "x": 5.667271497474452, @@ -178,7 +178,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false 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 87aa838..07406af 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 @@ -68,7 +68,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path b/src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path index 3728c7a..cf35782 100644 --- a/src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path +++ b/src/main/deploy/pathplanner/paths/S. L-Tre. Preload..path @@ -24,7 +24,7 @@ "y": 4.024072289156626 }, "nextControl": { - "x": 2.421654288011767, + "x": 2.421654288011768, "y": 3.9837489460910525 }, "isLocked": false, @@ -63,7 +63,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path index 8b6f670..10ed205 100644 --- a/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path +++ b/src/main/deploy/pathplanner/paths/S. R-Trench. E-Hub.path @@ -68,7 +68,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/paths/Spin.path b/src/main/deploy/pathplanner/paths/Spin.path index 5ce99cd..2e01917 100644 --- a/src/main/deploy/pathplanner/paths/Spin.path +++ b/src/main/deploy/pathplanner/paths/Spin.path @@ -72,7 +72,7 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 360.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 724a5d1..76cd67a 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -6,7 +6,7 @@ "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, + "defaultMaxAngVel": 360.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, "robotMass": 44.4521, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70ec505..50846b9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.AutoShootCommandChassisTurret; import frc.robot.commands.AutoTurretCommand; +import frc.robot.commands.AutoTurretShootCommand; import frc.robot.commands.Reverse; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.HopperSubsystem; @@ -80,7 +81,8 @@ public class RobotContainer { m_driverController.rightBumper().onTrue(Commands.parallel(intakeSubsystem.intake())); //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.x().toggleOnTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem)); + m_driverController.x().whileTrue(new AutoTurretShootCommand(hopperSubsystem, turretSubsystem, intakeSubsystem, driveSubsystem)); m_driverController.b().onTrue(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 9899985..d034284 100644 --- a/src/main/java/frc/robot/Utils.java +++ b/src/main/java/frc/robot/Utils.java @@ -5,6 +5,7 @@ import frc.robot.Constants; import frc.robot.Constants.ShootingRegion; import frc.robot.Constants.ShootingRegionDimensions; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.units.measure.Angle; public class Utils { @@ -93,7 +94,16 @@ public class Utils { return region; } + public static double getHoodAngleForDistance(double distanceInMeters){ + if(distanceInMeters >= 0){ + return 0; + } + return 0; + } + public static double getLauncherRPMForDistance(double distanceInMeters){ + return (667.557*distanceInMeters) + 1024.699; + } /** * Determines the appropriate shooting target for the robot based on its alliance diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 7ff09b5..6140e59 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -29,9 +29,6 @@ public class AutoTurretCommand extends Command { private int[] shootingTagFilters; - private Pose3d closestTag; - private Pose3d scoringHubPose; - public AutoTurretCommand(TurretSubsystem turretSubsystem, DriveSubsystem driveSubsystem) { this.turretSubsystem = turretSubsystem; this.driveSubsystem = driveSubsystem; @@ -47,12 +44,12 @@ public class AutoTurretCommand extends Command { DriverStation.getAlliance().ifPresent((aliance) -> { if (aliance == Alliance.Blue) { shootingTagFilters = Constants.CameraConstanst.IDFilters.BLUE_HUB_SHOOTING_IDS; - scoringHubPose = Constants.FieldPoses.BLUE_HUB_POSE; + turretSubsystem.setTurretTargetPose(Constants.FieldPoses.BLUE_HUB_POSE); } else { shootingTagFilters = Constants.CameraConstanst.IDFilters.RED_HUB_SHOOTING_IDS; - scoringHubPose = Constants.FieldPoses.RED_HUB_POSE; + turretSubsystem.setTurretTargetPose(Constants.FieldPoses.RED_HUB_POSE); } - Logger.recordOutput("AutoTurret-ScoringTarget", scoringHubPose); + Logger.recordOutput("AutoTurret-ScoringTarget", turretSubsystem.getTurretTargetPose()); }); } @@ -65,7 +62,7 @@ public class AutoTurretCommand extends Command { new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5)))); Logger.recordOutput("AutoTurret-TurretPose", turretPose); - Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); + Translation2d targetTranslation = turretSubsystem.getTurretTargetPose().getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); Logger.recordOutput("AutoTurret-Translation", targetTranslation); if(turretSubsystem.isTurretReady()){ diff --git a/src/main/java/frc/robot/commands/RelayCommand.java b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java similarity index 56% rename from src/main/java/frc/robot/commands/RelayCommand.java rename to src/main/java/frc/robot/commands/AutoTurretShootCommand.java index aafa387..162071d 100644 --- a/src/main/java/frc/robot/commands/RelayCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretShootCommand.java @@ -1,43 +1,63 @@ package frc.robot.commands; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; 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; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.TurretSubsystem; -public class RelayCommand extends Command{ +public class AutoTurretShootCommand extends Command{ private HopperSubsystem hopperSubsystem; private TurretSubsystem turretSubsystem; private IntakeSubsystem intakeSubsystem; + private DriveSubsystem driveSubsystem; private Command waitCommand = Commands.waitSeconds(1.5); - public RelayCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem){ + private Alliance alliance; + + public AutoTurretShootCommand(HopperSubsystem hopperSubsystem, TurretSubsystem turretSubsystem, IntakeSubsystem intakeSubsystem, DriveSubsystem driveSubsystem){ this.hopperSubsystem = hopperSubsystem; this.turretSubsystem = turretSubsystem; this.intakeSubsystem = intakeSubsystem; + this.driveSubsystem = driveSubsystem; addRequirements(hopperSubsystem, intakeSubsystem); } @Override public void initialize() { - // turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3); - turretSubsystem.setLauncher(3000); hopperSubsystem.spindexerOn(); CommandScheduler.getInstance().schedule(waitCommand); - turretSubsystem.setTurretMotorRotation(1); // Return to 0 after MIAMI VALLEY + alliance = DriverStation.getAlliance().get(); } @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)); + if(turretSubsystem.isLauncherUpToSpeed() && waitCommand.isFinished()){ hopperSubsystem.feedKicker(); }else{ @@ -58,7 +78,7 @@ public class RelayCommand extends Command{ turretSubsystem.setLauncher(0); turretSubsystem.setTargetElevation(Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE); - turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY + // turretSubsystem.setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY } } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 98d2ad7..c2cdf9d 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -163,15 +163,15 @@ public class DriveSubsystem extends TestableSubsystem { driveSystem::getRobotPose2d, driveSystem::setPose2d, driveSystem::getRobotRelativeSpeeds, - (speeds, feedforwards) -> driveSystem.setChassisSpeed(speeds, new Translation2d()), + (speeds, feedforwards) -> {driveSystem.setChassisSpeed(ChassisSpeeds.fromRobotRelativeSpeeds(-speeds.vxMetersPerSecond, -speeds.vyMetersPerSecond, -speeds.omegaRadiansPerSecond, driveSystem.getRobotHeading()), new Translation2d());}, new PPHolonomicDriveController( - new PIDConstants(3, 0, 0), + new PIDConstants(5, 0, 0), new PIDConstants(Math.PI, 0, 0)), config, () -> { var alliance = DriverStation.getAlliance(); if (alliance.isPresent()) { - return false;//alliance.get() == DriverStation.Alliance.Red; + return alliance.get() == DriverStation.Alliance.Red; } return false; }, @@ -188,6 +188,8 @@ public class DriveSubsystem extends TestableSubsystem { e.printStackTrace(); } + + } @Override diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 2855184..85d43de 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -62,6 +62,7 @@ public class TurretSubsystem extends TestableSubsystem { private StatusSignal turretMotorPosition; private StatusSignal turretMotorForwardLimit; private StatusSignal turretMotorReverseLimit; + private StatusSignal turretPositionError; private double forwardLimit = 0; private double reverseLimit = 0; @@ -72,6 +73,8 @@ public class TurretSubsystem extends TestableSubsystem { private Servo hood; + private Pose3d turretTarget; + public TurretSubsystem() { // Turret configuration @@ -103,6 +106,7 @@ public class TurretSubsystem extends TestableSubsystem { turretMotorPosition = turretMotor.getPosition(); turretMotorForwardLimit = turretMotor.getForwardLimit(); turretMotorReverseLimit = turretMotor.getReverseLimit(); + turretPositionError = turretMotor.getClosedLoopError(); turretSim = turretMotor.getSimState(); @@ -185,6 +189,14 @@ public class TurretSubsystem extends TestableSubsystem { return getTurretMotorPosition() / Constants.ShooterConstants.TURRET_GEAR_RATIO; } + public void setTurretTargetPose(Pose3d targetPose){ + turretTarget = targetPose; + } + + public Pose3d getTurretTargetPose(){ + return turretTarget; + } + private double getTurretMotorPosition(){ return turretMotorPosition.refresh().getValueAsDouble() - turretOffsetInRotations; } @@ -215,7 +227,7 @@ public class TurretSubsystem extends TestableSubsystem { .withReverseSoftLimitEnable(true) .withReverseSoftLimitThreshold(reverseLimit) ); - setTurretRobotRelative(0); + // setTurretRobotRelative(0); // setTurretMotorRotation(-0.34); // Return to 0 after MIAMI VALLEY } } @@ -292,6 +304,10 @@ Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotation turretSim.setRawRotorPosition(posWithOffset); } + public boolean isTurretAtTarget(){ + return turretPositionError.getValueAsDouble() <= 0.5; + } + private double getRequestedPosTurretRelative() { return (turretControlMode.getPositionMeasure().in(Units.Rotations) - turretOffsetInRotations) / Constants.ShooterConstants.TURRET_GEAR_RATIO; }