From cb1c7ba0e3f011d8c8ee742634cca747284781f6 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 21 Mar 2026 16:08:26 -0400 Subject: [PATCH] auto getting there everything else gucci --- .../pathplanner/autos/left to center.auto | 18 +++ .../pathplanner/paths/back from center.path | 63 ++++++++++ .../paths/left start to center.path | 88 +++++--------- .../pathplanner/paths/over bump to pile.path | 112 ++++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 18 ++- .../frc/robot/constants/HoodConstants.java | 10 +- .../frc/robot/constants/ShooterConstants.java | 18 +-- src/main/java/frc/robot/subsystems/Hood.java | 31 +++++ .../java/frc/robot/subsystems/Shooter.java | 15 ++- 9 files changed, 291 insertions(+), 82 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/back from center.path create mode 100644 src/main/deploy/pathplanner/paths/over bump to pile.path diff --git a/src/main/deploy/pathplanner/autos/left to center.auto b/src/main/deploy/pathplanner/autos/left to center.auto index 6722e6f..73a0044 100644 --- a/src/main/deploy/pathplanner/autos/left to center.auto +++ b/src/main/deploy/pathplanner/autos/left to center.auto @@ -15,6 +15,24 @@ "data": { "pathName": "left start to center" } + }, + { + "type": "path", + "data": { + "pathName": "over bump to pile" + } + }, + { + "type": "path", + "data": { + "pathName": "back from center" + } + }, + { + "type": "named", + "data": { + "name": "auto shoot" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/back from center.path b/src/main/deploy/pathplanner/paths/back from center.path new file mode 100644 index 0000000..5a44fbd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/back from center.path @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.050842639593909, + "y": 5.715482233502538 + }, + "prevControl": null, + "nextControl": { + "x": 5.130030456852792, + "y": 5.7523147208121825 + }, + "isLocked": false, + "linkedName": "over bump" + }, + { + "anchor": { + "x": 3.002954314720812, + "y": 5.310324873096447 + }, + "prevControl": { + "x": 4.052680203045686, + "y": 5.9825177664974625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.10454545454545605, + "rotationDegrees": -115.0 + }, + { + "waypointRelativePos": 0.7931818181818296, + "rotationDegrees": -115.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -36.158185439808385 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -123.34070734647689 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left start to center.path b/src/main/deploy/pathplanner/paths/left start to center.path index 377be13..6aedd5b 100644 --- a/src/main/deploy/pathplanner/paths/left start to center.path +++ b/src/main/deploy/pathplanner/paths/left start to center.path @@ -8,111 +8,77 @@ }, "prevControl": null, "nextControl": { - "x": 3.1594923857868027, - "y": 6.424507614213196 + "x": 2.9108730964467004, + "y": 6.498172588832488 }, "isLocked": false, "linkedName": "start left" }, { "anchor": { - "x": 2.634629441624366, - "y": 5.558944162436549 + "x": 2.5885888324873094, + "y": 5.936477157360406 }, "prevControl": { - "x": 2.4136345177664977, - "y": 5.908852791878173 + "x": 2.3675939086294413, + "y": 6.28638578680203 }, "nextControl": { - "x": 2.895419769065523, - "y": 5.146026143988051 + "x": 2.8493791599284664, + "y": 5.523559138911907 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 4.522294416243654, - "y": 5.558944162436549 + "x": 4.577543147208122, + "y": 5.715482233502538 }, "prevControl": { - "x": 3.5095162657293457, - "y": 5.533304209258972 + "x": 3.565840773084476, + "y": 5.768729726877464 }, "nextControl": { - "x": 6.704619289340102, - "y": 5.614192893401015 + "x": 5.102406091370558, + "y": 5.687857868020306 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.478101522849547, - "y": 7.244030456855094 + "x": 6.050842639593909, + "y": 5.715482233502538 }, "prevControl": { - "x": 6.765542310662505, - "y": 7.465169522706244 - }, - "nextControl": { - "x": 8.012172588839395, - "y": 7.078284263961693 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.671472081218274, - "y": 4.7670456852791885 - }, - "prevControl": { - "x": 7.726720812182741, - "y": 7.639979695431472 + "x": 5.525979695431473, + "y": 5.807563451776649 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "over bump" } ], "rotationTargets": [ { - "waypointRelativePos": 1.5636363636363326, - "rotationDegrees": -45.0 - }, - { - "waypointRelativePos": 3.0318181818182266, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 2.6208291203235685, - "maxWaypointRelativePos": 4.0, - "constraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } + "waypointRelativePos": 1.62272727272727, + "rotationDegrees": -135.0 } ], + "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { "name": "Intake Start", - "waypointRelativePos": 0, + "waypointRelativePos": 2.1, "endWaypointRelativePos": null, "command": null } ], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.5, + "maxVelocity": 3.0, + "maxAcceleration": 2.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -120,7 +86,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -89.09061955080092 + "rotation": -123.34070734647689 }, "reversed": false, "folder": null, @@ -128,5 +94,5 @@ "velocity": 0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/over bump to pile.path b/src/main/deploy/pathplanner/paths/over bump to pile.path new file mode 100644 index 0000000..1760a40 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/over bump to pile.path @@ -0,0 +1,112 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.050842639593909, + "y": 5.715482233502538 + }, + "prevControl": null, + "nextControl": { + "x": 6.520456852791878, + "y": 6.406091370558377 + }, + "isLocked": false, + "linkedName": "over bump" + }, + { + "anchor": { + "x": 7.017695431472081, + "y": 7.216406091370559 + }, + "prevControl": { + "x": 6.017695431472081, + "y": 7.216406091370559 + }, + "nextControl": { + "x": 8.017695431472081, + "y": 7.216406091370559 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.883258883248731, + "y": 4.831502538071066 + }, + "prevControl": { + "x": 7.941269035532995, + "y": 6.042370558375635 + }, + "nextControl": { + "x": 7.825248730964466, + "y": 3.620634517766498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.050842639593909, + "y": 5.715482233502538 + }, + "prevControl": { + "x": 6.99789847715736, + "y": 5.183713197969544 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0045454545454569, + "rotationDegrees": -115.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.1243680485338854, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake Start", + "waypointRelativePos": 0.7886754297269942, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -120.34324888423971 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -123.34070734647689 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 98c5c1b..770104c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -84,7 +84,7 @@ public class RobotContainer { resetOdometryToVisualPose = false; - //vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); + vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose); vision.addPoseEstimateConsumer((vp) -> { Logger.recordOutput( "Vision/" + vp.cameraName() + "/Pose", @@ -243,7 +243,7 @@ public class RobotContainer { // Useful for testing PID and FF responses of the shooter // You need to have graphs up of the logged data to make sure the response is correct - secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)); + //secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)); secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed)); // Useful for testing PID and FF responses of the intake pivot @@ -276,6 +276,7 @@ public class RobotContainer { driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true)); driver.y().whileTrue(drivetrain.zeroHeading()); + driver.x().whileTrue(drivetrain.setX()); driver.leftTrigger().whileTrue( drivetrain.lockRotationToHub( @@ -320,10 +321,12 @@ public class RobotContainer { //40 good for feeding //secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30))); //30 degrees good for shooter far near outpost - secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10))); + secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10))); //10 degrees good for shooting ~33in away from hub + + hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter)); - hood.setDefaultCommand(hood.trackToAngle(() -> { + /*hood.setDefaultCommand(hood.trackToAngle(() -> { Pose2d drivetrainPose = drivetrain.getPose(); Pose2d hubPose = Utilities.getHubPose(); @@ -345,7 +348,7 @@ public class RobotContainer { } else { return 0; } - })); + }));*/ new Trigger(() -> MathUtil.isNear( shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(), @@ -399,7 +402,8 @@ public class RobotContainer { // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn()); new EventTrigger("Intake Start") - .onTrue(intakeRoller.runIn()); + .onTrue( + intakeRoller.runIn()); NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop()); @@ -415,6 +419,8 @@ public class RobotContainer { hood.trackToAngle(() -> Units.degreesToRadians(10))) ).withTimeout(3).andThen(spindexer.instantaneousStop())); + + NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index 17c713d..af2833b 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -71,22 +71,22 @@ public class HoodConstants { kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( Double.valueOf(Units.inchesToMeters(22.2 + 40)), - Double.valueOf(Units.degreesToRadians(10))); + Double.valueOf(Units.degreesToRadians(9.5))); kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( Double.valueOf(Units.inchesToMeters(22.2 + 60)), - Double.valueOf(Units.degreesToRadians(13))); + Double.valueOf(Units.degreesToRadians(12.5))); kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( Double.valueOf(Units.inchesToMeters(22.2 + 80)), - Double.valueOf(Units.degreesToRadians(17))); + Double.valueOf(Units.degreesToRadians(16.25))); kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( Double.valueOf(Units.inchesToMeters(22.2 + 100)), - Double.valueOf(Units.degreesToRadians(21))); + Double.valueOf(Units.degreesToRadians(20.5))); kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put( Double.valueOf(Units.inchesToMeters(22.2 + 120)), - Double.valueOf(Units.degreesToRadians(24))); + Double.valueOf(Units.degreesToRadians(23.5))); } } diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 3b32a86..c549d41 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -5,8 +5,10 @@ import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; + public class ShooterConstants { public enum ShooterSpeeds { kHubSpeed(3000.0), @@ -41,16 +43,16 @@ public class ShooterConstants { public static final boolean kLeftShooterMotorInverted = true; public static final boolean kRightShooterMotorInverted = false; - public static final double kLeftP = 0.04;//0.01;//0.001; + public static final double kLeftP = 0.1;//0.01;//0.001; public static final double kLeftI = 0; - public static final double kLeftD = 0;//1.8; + public static final double kLeftD = 0;//0.1;//1.8; public static final double kLeftS = 0; public static final double kLeftV = 0.00129; public static final double kLeftA = 0; - public static final double kRightP = 0.04;//0.001;//0.001; + public static final double kRightP = 0.1;//0.001;//0.001; public static final double kRightI = 0; - public static final double kRightD = 0; + public static final double kRightD = 0;//0.1; public static final double kRightS = 0; public static final double kRightV = 0.00125; public static final double kRightA = 0; @@ -78,12 +80,12 @@ public class ShooterConstants { kLeftMotorConfig.absoluteEncoder .positionConversionFactor(1) .velocityConversionFactor(60) - .averageDepth(16); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER + .averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER kLeftMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0) .outputRange(-1, 1) - //.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0) + .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0) .feedForward .sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0); @@ -94,13 +96,13 @@ public class ShooterConstants { kRightMotorConfig.absoluteEncoder .positionConversionFactor(1) .velocityConversionFactor(60) - .averageDepth(16)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER + .averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER .inverted(true); kRightMotorConfig.closedLoop .feedbackSensor(FeedbackSensor.kAbsoluteEncoder) .pid(kRightP, kRightI, kRightD) .outputRange(-1, 1) - //.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0) + .allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0) .feedForward .sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0); } diff --git a/src/main/java/frc/robot/subsystems/Hood.java b/src/main/java/frc/robot/subsystems/Hood.java index 236c516..057a0f0 100644 --- a/src/main/java/frc/robot/subsystems/Hood.java +++ b/src/main/java/frc/robot/subsystems/Hood.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import java.util.Optional; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -13,6 +14,8 @@ import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -20,6 +23,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.HoodConstants; +import frc.robot.constants.ShooterConstants.ShooterSpeeds; +import frc.robot.utilities.Utilities; public class Hood extends SubsystemBase { private SparkMax motor; @@ -92,6 +97,32 @@ public class Hood extends SubsystemBase { Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage()); } + public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) { + return trackToAngle(() -> { + Pose2d drivetrainPose = drivetrain.getPose(); + Pose2d hubPose = Utilities.getHubPose(); + + double distance = drivetrainPose.getTranslation() + .getDistance(hubPose.getTranslation()); + + Logger.recordOutput("Hood/DistanceToHub", distance); + + Optional currentSpeeds = shooter.getTargetSpeeds(); + + if(currentSpeeds.isPresent()) { + InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get()); + + if(map != null) { + return MathUtil.clamp(map.get(distance), 0, 40); + } else { + return 0; + } + } else { + return 0; + } + }); + } + public Command trackToAngle(DoubleSupplier degreeAngleSupplier) { return run(() -> { currentTargetDegrees = degreeAngleSupplier.getAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0481d9e..3e44e10 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -9,11 +9,13 @@ import com.revrobotics.AbsoluteEncoder; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,6 +35,8 @@ public class Shooter extends SubsystemBase { private SparkClosedLoopController rightClosedLoopController; private ShooterSpeeds targetSpeeds; + private SimpleMotorFeedforward shooterFFLeft; + private SimpleMotorFeedforward shooterFFRight; public Shooter() { leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless); @@ -60,6 +64,9 @@ public class Shooter extends SubsystemBase { targetSpeeds = null; rightRelative = rightMotor.getEncoder(); + + shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA); + shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA); } @Override @@ -116,12 +123,16 @@ public class Shooter extends SubsystemBase { } else { leftClosedLoopController.setSetpoint( targetSpeeds.getSpeedRPM(), - ControlType.kVelocity + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + shooterFFLeft.calculate(targetSpeeds.getSpeedRPM()) ); rightClosedLoopController.setSetpoint( targetSpeeds.getSpeedRPM(), - ControlType.kVelocity + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + shooterFFRight.calculate(targetSpeeds.getSpeedRPM()) ); } });