From a89f7810d7e085a22d9fc1747390efe855d05e01 Mon Sep 17 00:00:00 2001 From: BenG49 Date: Tue, 26 Mar 2024 16:52:32 -0400 Subject: [PATCH 01/10] Add shoot while follow path and tracking --- .../auton/FollowPathTrackingShoot.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java new file mode 100644 index 00000000..ba3cf4f9 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java @@ -0,0 +1,31 @@ +package com.stuypulse.robot.commands.auton; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.stuypulse.robot.commands.conveyor.ConveyorShoot; +import com.stuypulse.robot.commands.conveyor.ConveyorStop; +import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.streams.booleans.BStream; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class FollowPathTrackingShoot extends ParallelCommandGroup { + + public FollowPathTrackingShoot(PathPlannerPath path, BStream shouldShoot) { + addCommands( + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), + new SequentialCommandGroup( + new WaitUntilCommand(shouldShoot), + new ConveyorShoot(), + new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), + new ConveyorStop(), + new IntakeStop() + ) + ); + } + +} From 8fccf8ef47352a9d072ecb57180cbd41f70fb23f Mon Sep 17 00:00:00 2001 From: BenG49 Date: Wed, 27 Mar 2024 16:37:49 -0400 Subject: [PATCH 02/10] Add SwerveDriveToShootMoving and make FollowPathTrackingShoot shoot when projected pose is aligned Co-authored-by: Zixi52 Co-authored-by: Baiulus Co-authored-by: mabdullah32 Co-authored-by: Rahuldeb75 Co-authored-by: k4limul Co-authored-by: Zixi Feng Co-authored-by: BrianZ60 --- .../auton/FollowPathTrackingShoot.java | 54 ++++++- .../swerve/SwerveDriveToShootMoving.java | 152 ++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 3 + 3 files changed, 206 insertions(+), 3 deletions(-) create mode 100644 src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java index ba3cf4f9..deb6440d 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java @@ -4,9 +4,15 @@ import com.stuypulse.robot.commands.conveyor.ConveyorShoot; import com.stuypulse.robot.commands.conveyor.ConveyorStop; import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; -import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; @@ -14,12 +20,44 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; public class FollowPathTrackingShoot extends ParallelCommandGroup { + + private final Odometry odometry; + + private final VStream robotPose; + private final VStream robotVelocity; + private final VStream projectedRobotPose; + + private double targetDistance; + private double distanceTolerance; + + private boolean shouldShoot() { + double distance = Field.getAllianceSpeakerPose().getTranslation() + .minus(projectedRobotPose.get().getTranslation2d()) + .getNorm(); + + return Math.abs(targetDistance - distance) < distanceTolerance; + } - public FollowPathTrackingShoot(PathPlannerPath path, BStream shouldShoot) { + public FollowPathTrackingShoot(PathPlannerPath path) { + odometry = Odometry.getInstance(); + + robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); + + robotVelocity = robotPose + .filtered(new VDerivative()) + .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)); + + projectedRobotPose = robotVelocity + .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) + .add(robotPose); + + distanceTolerance = 0.033; + targetDistance = Alignment.PODIUM_SHOT_DISTANCE.get(); + addCommands( SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), new SequentialCommandGroup( - new WaitUntilCommand(shouldShoot), + new WaitUntilCommand(this::shouldShoot), new ConveyorShoot(), new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), new ConveyorStop(), @@ -28,4 +66,14 @@ public FollowPathTrackingShoot(PathPlannerPath path, BStream shouldShoot) { ); } + public FollowPathTrackingShoot withTolerance(double distanceTolerance) { + this.distanceTolerance = distanceTolerance; + return this; + } + + public FollowPathTrackingShoot withTargetDistance(double target) { + this.targetDistance = target; + return this; + } + } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java new file mode 100644 index 00000000..75e8fafc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -0,0 +1,152 @@ +/************************ PROJECT IZZI *************************/ +/* Copyright (c) 2024 StuyPulse Robotics. All rights reserved. */ +/* Use of this source code is governed by an MIT-style license */ +/* that can be found in the repository LICENSE file. */ +/***************************************************************/ + +package com.stuypulse.robot.commands.swerve; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Swerve; +import com.stuypulse.robot.constants.Settings.Alignment.Shoot; +import com.stuypulse.robot.subsystems.odometry.Odometry; +import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; +import com.stuypulse.stuylib.control.feedback.PIDController; +import com.stuypulse.stuylib.math.Angle; +import com.stuypulse.stuylib.math.SLMath; +import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.booleans.BStream; +import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.vectors.VStream; +import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; +import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SwerveDriveToShootMoving extends Command { + + public static SwerveDriveToShootMoving withHigherDebounce() { + return new SwerveDriveToShootMoving(Alignment.PODIUM_SHOT_DISTANCE, 0.75); + } + + private final SwerveDrive swerve; + private final Odometry odometry; + + private final PIDController distanceController; + private final AnglePIDController angleController; + + private final BStream isAligned; + + private final VStream robotPose; + private final VStream robotVelocity; + private final VStream projectedRobotPose; + + private final Number targetDistance; + + private double distanceTolerance; + private double angleTolerance; + + public SwerveDriveToShootMoving() { + this(Alignment.PODIUM_SHOT_DISTANCE); + } + + public SwerveDriveToShootMoving(Number targetDistance) { + this(targetDistance, Alignment.DEBOUNCE_TIME); + } + + public SwerveDriveToShootMoving(Number targetDistance, double debounce) { + this.targetDistance = targetDistance; + + swerve = SwerveDrive.getInstance(); + odometry = Odometry.getInstance(); + + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD); + + angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); + + robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); + + robotVelocity = robotPose + .filtered(new VDerivative()) + .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)); + + projectedRobotPose = robotVelocity + .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) + .add(robotPose); + + isAligned = BStream.create(this::isAligned) + .filtered(new BDebounceRC.Rising(debounce)); + + distanceTolerance = 0.033; + angleTolerance = Alignment.ANGLE_TOLERANCE.get(); + + addRequirements(swerve); + } + + private double getTargetDistance() { + return SLMath.clamp(targetDistance.doubleValue(), 1, 5); + } + + public SwerveDriveToShootMoving withTolerance(double distanceTolerance, double angleTolerance) { + this.distanceTolerance = distanceTolerance; + this.angleTolerance = angleTolerance; + return this; + } + + public SwerveDriveToShootMoving withDistanceConstants(double p, double i, double d) { + distanceController.setPID(p, i, d); + return this; + } + + public SwerveDriveToShootMoving withRotationConstants(double p, double i, double d) { + angleController.setPID(p, i, d); + return this; + } + + private boolean isAligned() { + return distanceController.isDone(distanceTolerance) + && angleController.isDoneDegrees(angleTolerance); + } + + @Override + public void execute() { + Translation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() + .minus(projectedRobotPose.get().getTranslation2d()); + + double speed = -distanceController.update(getTargetDistance(), toSpeaker.getNorm()); + double rotation = angleController.update( + Angle.fromRotation2d(toSpeaker.getAngle()).add(Angle.k180deg), + Angle.fromRotation2d(odometry.getPose().getRotation())); + + Translation2d speeds = new Translation2d( + speed, + toSpeaker.getAngle()); + + if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) + rotation = 0; + + swerve.setFieldRelativeSpeeds( + new ChassisSpeeds( + speeds.getX(), + speeds.getY(), + rotation)); + + SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.getAngle().plus(Rotation2d.fromDegrees(180)).getDegrees()); + } + + @Override + public boolean isFinished() { + return isAligned.get(); + } + + @Override + public void end(boolean interrupted) { + swerve.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 135c6ddd..564058c4 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -406,6 +406,9 @@ public interface Alignment { SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); + double PROJECTED_POSE_RC = 0.05; + double NOTE_TO_GOAL_TIME = 0.5; + public interface Translation { SmartNumber kP = new SmartNumber("Alignment/Translation/kP", 5.0); SmartNumber kI = new SmartNumber("Alignment/Translation/kI", 0.0); From de85f101390a99ba559e5b3b28cfec258437a25e Mon Sep 17 00:00:00 2001 From: BenG49 Date: Wed, 27 Mar 2024 18:08:16 -0400 Subject: [PATCH 03/10] Shoot in SwerveDriveToShootMoving --- .../swerve/SwerveDriveToShootMoving.java | 21 ++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java index 75e8fafc..bdab892e 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -10,6 +10,8 @@ import com.stuypulse.robot.constants.Settings.Alignment; import com.stuypulse.robot.constants.Settings.Swerve; import com.stuypulse.robot.constants.Settings.Alignment.Shoot; +import com.stuypulse.robot.subsystems.conveyor.Conveyor; +import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; @@ -37,6 +39,8 @@ public static SwerveDriveToShootMoving withHigherDebounce() { private final SwerveDrive swerve; private final Odometry odometry; + private final Conveyor conveyor; + private final Intake intake; private final PIDController distanceController; private final AnglePIDController angleController; @@ -65,6 +69,8 @@ public SwerveDriveToShootMoving(Number targetDistance, double debounce) { swerve = SwerveDrive.getInstance(); odometry = Odometry.getInstance(); + conveyor = Conveyor.getInstance(); + intake = Intake.getInstance(); distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD); @@ -137,16 +143,21 @@ public void execute() { speeds.getY(), rotation)); - SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.getAngle().plus(Rotation2d.fromDegrees(180)).getDegrees()); - } + if (isAligned.get()) { + conveyor.toShooter(); + intake.acquire(); + } else { + conveyor.stop(); + intake.stop(); + } - @Override - public boolean isFinished() { - return isAligned.get(); + SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.getAngle().plus(Rotation2d.fromDegrees(180)).getDegrees()); } @Override public void end(boolean interrupted) { swerve.stop(); + intake.stop(); + conveyor.stop(); } } \ No newline at end of file From 2697ee383c9c5178e1ed777087fc93cd0b8693c5 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 09:54:56 -0400 Subject: [PATCH 04/10] Tune 6 piece --- src/main/deploy/pathplanner/paths/A to D.path | 8 ++++---- src/main/deploy/pathplanner/paths/A to E.path | 12 +++++------ src/main/deploy/pathplanner/paths/B to A.path | 20 +++++++++---------- .../pathplanner/paths/C to B Close.path | 8 ++++---- src/main/deploy/pathplanner/paths/C to B.path | 16 +++++++-------- .../pathplanner/paths/CShoot to H (CHGF).path | 4 ++-- .../deploy/pathplanner/paths/E to Shoot.path | 12 ++++++++--- .../pathplanner/paths/Preload to C.path | 12 +++++------ .../pathplanner/paths/Shoot to D (CBAED).path | 8 ++++---- .../commands/auton/CBADE/SixPieceCBAED.java | 2 +- .../robot/commands/leds/LEDDefaultMode.java | 6 ++++++ .../commands/swerve/SwerveDriveToShoot.java | 6 ++++++ .../robot/constants/LEDInstructions.java | 2 +- .../stuypulse/robot/constants/Settings.java | 8 ++++---- 14 files changed, 71 insertions(+), 53 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path index eb91cb13..ca53b80e 100644 --- a/src/main/deploy/pathplanner/paths/A to D.path +++ b/src/main/deploy/pathplanner/paths/A to D.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 3.7902680687415704, - "y": 7.079173100960065 + "x": 3.6941070946532406, + "y": 7.027030067557189 }, "isLocked": false, "linkedName": "A" diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path index 3cf34949..bf4678ec 100644 --- a/src/main/deploy/pathplanner/paths/A to E.path +++ b/src/main/deploy/pathplanner/paths/A to E.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": null, "nextControl": { - "x": 4.91984991243836, - "y": 7.023157661821328 + "x": 4.82368893835003, + "y": 6.971014628418452 }, "isLocked": false, "linkedName": "A" @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 6.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index ffa08e70..3fbd922d 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": null, "nextControl": { - "x": 2.6183483616967234, - "y": 5.861535033255252 + "x": 2.756722253230932, + "y": 5.931533495077972 }, "isLocked": false, "linkedName": "B" }, { "anchor": { - "x": 2.8, - "y": 6.94 + "x": 2.70383902591167, + "y": 6.887856966597124 }, "prevControl": { - "x": 2.646912247528199, - "y": 6.50431154103049 + "x": 2.5507512734398694, + "y": 6.452168507627614 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 60.0, + "rotationDegrees": 52.37639263407269, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 51.76617482255292, + "rotation": 50.90614111377048, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index 01b578f6..e2d6156f 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": { - "x": 2.4857700960953526, - "y": 5.063860187032356 + "x": 2.6241439876295614, + "y": 5.133858648855076 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 4d2cbcf0..1125ee7f 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.5, - "y": 4.878834526352327 + "x": 2.62197994481459, + "y": 4.794603321400375 }, "isLocked": false, "linkedName": "C" }, { "anchor": { - "x": 2.78765406878382, - "y": 5.239147655045014 + "x": 2.9260279603180286, + "y": 5.309146116867734 }, "prevControl": { - "x": 2.5539374447669423, - "y": 4.8009289850133685 + "x": 2.692311336301151, + "y": 4.870927446836089 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.5, - "rotationDegrees": 60.0, + "rotationDegrees": 34.799293349232, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 60.71172956223985, + "rotation": 43.876697285924436, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path index 6e19b1f6..07571c6a 100644 --- a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path +++ b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.55, - "maxAcceleration": 5.5, + "maxVelocity": 5.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/E to Shoot.path b/src/main/deploy/pathplanner/paths/E to Shoot.path index 5213bed9..135d3fe9 100644 --- a/src/main/deploy/pathplanner/paths/E to Shoot.path +++ b/src/main/deploy/pathplanner/paths/E to Shoot.path @@ -28,12 +28,18 @@ "linkedName": "Top Side Shoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": 15.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 5.0, + "maxVelocity": 6.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Preload to C.path b/src/main/deploy/pathplanner/paths/Preload to C.path index 0ca02525..9ccfa9b6 100644 --- a/src/main/deploy/pathplanner/paths/Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Preload to C.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.52, - "y": 4.1 + "x": 2.493344245947751, + "y": 4.163118981508619 }, "prevControl": { - "x": 2.4857700960953526, - "y": 4.323757544312242 + "x": 2.4591143420431036, + "y": 4.3868765258208615 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.6, - "rotationDegrees": -40.0, + "rotationDegrees": -35.316048663452285, "rotateFast": false } ], @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -20.0, + "rotation": -32.93139922764586, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path b/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path index 56b962cf..2150ffeb 100644 --- a/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path +++ b/src/main/deploy/pathplanner/paths/Shoot to D (CBAED).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.2, - "y": 7.43 + "x": 8.118232532761363, + "y": 7.729836086452799 }, "prevControl": { - "x": 6.242200000323567, - "y": 7.081144238920643 + "x": 6.160432533084931, + "y": 7.380980325373443 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java index 3cf5b4b7..e36ff549 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java @@ -27,7 +27,7 @@ public SixPieceCBAED(PathPlannerPath... paths) { .withTolerance(0.1, 0.1, 3) ), - new ConveyorShootRoutine(), + new ConveyorShootRoutine(0.35), new FollowPathAndIntake(paths[0]), new SwerveDriveToShoot(), diff --git a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java index d89a4d70..d8d25463 100644 --- a/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java +++ b/src/main/java/com/stuypulse/robot/commands/leds/LEDDefaultMode.java @@ -14,6 +14,8 @@ import com.stuypulse.robot.subsystems.leds.instructions.LEDInstruction; import com.stuypulse.stuylib.util.StopWatch; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; /** @@ -44,6 +46,10 @@ private LEDInstruction getInstruction() { && amper.isAtTargetHeight(0.15)) return LEDInstructions.GREEN; + if (DriverStation.isAutonomousEnabled() && SmartDashboard.getBoolean("A", false)) { + return LEDInstructions.GREEN; + } + if (intake.hasNote()) { return LEDInstructions.CONTAINS_NOTE; } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index 1f54051f..19704e61 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -110,6 +110,11 @@ private boolean isAligned() { && velocityError.get() < velocityTolerance; } + @Override + public void initialize() { + SmartDashboard.putBoolean("A", true); + } + @Override public void execute() { Translation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() @@ -145,5 +150,6 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { swerve.stop(); + SmartDashboard.putBoolean("A", false); } } \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java index 9b473ef0..1fa0d331 100644 --- a/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java +++ b/src/main/java/com/stuypulse/robot/constants/LEDInstructions.java @@ -68,7 +68,7 @@ public interface LEDInstructions { LEDInstruction DEFAULT = LEDInstructions.OFF; - LEDInstruction INTAKE = new LEDPulseColor(SLColor.RED); + LEDInstruction INTAKE = new LEDPulseColor(SLColor.BLUE); LEDInstruction PICKUP = new LEDPulseColor(SLColor.RED); diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index dab99c73..b75f69a2 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -332,7 +332,7 @@ public interface Shooter { double AT_RPM_EPSILON = 125; SmartNumber RPM_CHANGE_RC = new SmartNumber("Shooter/RPM Change RC", 0.2); - double RPM_CHANGE_DIP_THRESHOLD = 250; + double RPM_CHANGE_DIP_THRESHOLD = 300; public interface Feedforward { double kS = 0.11873; @@ -352,7 +352,7 @@ public interface Feeder { public interface Feedforward { double kS = 0.71611; - double kV = 0.0032; + double kV = 0.0035; double kA = 0.00040287; } @@ -383,7 +383,7 @@ public interface Conveyor { SmartNumber DEBOUNCE_TIME = new SmartNumber("Conveyor/Debounce Time", 0.0); SmartNumber RECALL_DEBOUNCE = new SmartNumber("Conveyor/Recall Delay", 1.0); - SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.35); + SmartNumber SHOOT_WAIT_DELAY = new SmartNumber("Conveyor/Shoot Wait Delay", 0.45); SmartNumber AT_FEEDER_WAIT_DELAY = new SmartNumber("Conveyor/At Feeder Wait Delay", 0.5); } @@ -413,7 +413,7 @@ public interface Translation { } public interface Rotation { - SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 6.0); + SmartNumber kP = new SmartNumber("Alignment/Rotation/kP", 4.0); SmartNumber kI = new SmartNumber("Alignment/Rotation/kI", 0.0); SmartNumber kD = new SmartNumber("Alignment/Rotation/kD", 0.0); } From 333a02dc41ce37685820c4b6478de4e8dc171588 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 11:41:39 -0400 Subject: [PATCH 05/10] Tune 6 piece, other changes --- .../deploy/pathplanner/autos/6 CBAED.auto | 8 ++- .../pathplanner/paths/Close Preload to C.path | 2 +- .../deploy/pathplanner/paths/Close Shoot.path | 52 +++++++++++++++++++ .../pathplanner/paths/Preload to C Close.path | 52 +++++++++++++++++++ .../com/stuypulse/robot/RobotContainer.java | 4 +- .../commands/auton/CBADE/SixPieceCBAED.java | 34 +++++++----- .../auton/FollowPathTrackingShoot.java | 34 +++++++----- .../conveyor/ConveyorShootNoIntake.java | 22 ++++++++ .../swerve/SwerveDriveToShootMoving.java | 21 +++++--- .../stuypulse/robot/constants/Settings.java | 6 +-- .../robot/subsystems/amper/AmperImpl.java | 4 ++ 11 files changed, 202 insertions(+), 37 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Close Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/Preload to C Close.path create mode 100644 src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java diff --git a/src/main/deploy/pathplanner/autos/6 CBAED.auto b/src/main/deploy/pathplanner/autos/6 CBAED.auto index 37eed8f9..f1abb8a7 100644 --- a/src/main/deploy/pathplanner/autos/6 CBAED.auto +++ b/src/main/deploy/pathplanner/autos/6 CBAED.auto @@ -8,7 +8,13 @@ { "type": "path", "data": { - "pathName": "Preload to C" + "pathName": "Preload to C Close" + } + }, + { + "type": "path", + "data": { + "pathName": "Close Preload to C" } }, { diff --git a/src/main/deploy/pathplanner/paths/Close Preload to C.path b/src/main/deploy/pathplanner/paths/Close Preload to C.path index 35835595..e4ccc948 100644 --- a/src/main/deploy/pathplanner/paths/Close Preload to C.path +++ b/src/main/deploy/pathplanner/paths/Close Preload to C.path @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": -29.392354726969238, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Close Shoot.path b/src/main/deploy/pathplanner/paths/Close Shoot.path new file mode 100644 index 00000000..4a64e859 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Close Shoot.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3239288017037572, + "y": 5.531335051274091 + }, + "prevControl": null, + "nextControl": { + "x": 2.323928801703759, + "y": 5.531335051274091 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6978421535190646, + "y": 5.531335051274091 + }, + "prevControl": { + "x": 2.6978421535190646, + "y": 5.531335051274091 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Tests", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Preload to C Close.path b/src/main/deploy/pathplanner/paths/Preload to C Close.path new file mode 100644 index 00000000..9ac2206d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Preload to C Close.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.3473171105886368, + "y": 5.402699352407253 + }, + "prevControl": null, + "nextControl": { + "x": 1.8501657516135543, + "y": 4.9934039469218545 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3764027015233515, + "y": 4.432084533684738 + }, + "prevControl": { + "x": 1.7800008249589148, + "y": 5.122039645788694 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -32.969403903462045, + "rotateFast": false + }, + "reversed": false, + "folder": "ABCDE", + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index 0f84573c..c12264eb 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -305,8 +305,8 @@ public void configureAutons() { AutonConfig TrackingCBAE = new AutonConfig("Tracking 5 CBAE Podium", FivePieceTrackingCBAE::new, "Preload to C", "C to B", "B to A", "A to E", "E to Shoot"); - AutonConfig CBAED = new AutonConfig("6 CBAED", SixPieceCBAED::new, - "Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); + AutonConfig CBAED = new AutonConfig("5 CBAE", SixPieceCBAED::new, + "Preload to C Close", "Close Preload to C", "C to B", "B to A","A to E", "E to Shoot", "Shoot to D (CBAED)", "D to Shoot"); AutonConfig CHGF = new AutonConfig("4.5 Piece CHGF", FivePieceCHGF::new, "Preload to C", "CShoot To H (CHGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)"); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java index e36ff549..185d5857 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/CBADE/SixPieceCBAED.java @@ -3,13 +3,16 @@ import com.pathplanner.lib.path.PathPlannerPath; import com.stuypulse.robot.commands.auton.FollowPathAlignAndShoot; import com.stuypulse.robot.commands.auton.FollowPathAndIntake; +import com.stuypulse.robot.commands.conveyor.ConveyorShootNoIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; +import com.stuypulse.robot.commands.conveyor.ConveyorStop; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; import com.stuypulse.robot.constants.Settings.Auton; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -23,29 +26,36 @@ public SixPieceCBAED(PathPlannerPath... paths) { new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) .andThen(new ShooterPodiumShot()), - SwerveDriveToPose.speakerRelative(-15) - .withTolerance(0.1, 0.1, 3) + SwerveDrive.getInstance().followPathCommand(paths[0]) ), - new ConveyorShootRoutine(0.35), - - new FollowPathAndIntake(paths[0]), - new SwerveDriveToShoot(), - new ConveyorShootRoutine(), + new InstantCommand(SwerveDrive.getInstance()::stop, SwerveDrive.getInstance()), + new ConveyorShootNoIntake(), + new WaitCommand(0.55), + new ConveyorStop(), new FollowPathAndIntake(paths[1]), - new SwerveDriveToShoot(), + new SwerveDriveToShoot() + .withTolerance(0.05, 7), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[2]), - new SwerveDriveToShoot(), + new SwerveDriveToShoot() + .withTolerance(0.05, 7), new ConveyorShootRoutine(), new FollowPathAndIntake(paths[3]), - new FollowPathAlignAndShoot(paths[4], new SwerveDriveToShoot()), + new SwerveDriveToShoot() + .withTolerance(0.05, 5), + new ConveyorShootRoutine(), + + new FollowPathAndIntake(paths[4]), + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot() + .withTolerance(0.033, 7)), - new FollowPathAndIntake(paths[5]), - new FollowPathAlignAndShoot(paths[6], new SwerveDriveToShoot()) + new FollowPathAndIntake(paths[6]), + new FollowPathAlignAndShoot(paths[7], new SwerveDriveToShoot() + .withTolerance(0.033, 7)) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java index deb6440d..5a654629 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java @@ -4,6 +4,8 @@ import com.stuypulse.robot.commands.conveyor.ConveyorShoot; import com.stuypulse.robot.commands.conveyor.ConveyorStop; import com.stuypulse.robot.commands.intake.IntakeStop; +import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; import com.stuypulse.robot.constants.Field; import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.constants.Settings.Alignment; @@ -14,12 +16,13 @@ import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -public class FollowPathTrackingShoot extends ParallelCommandGroup { +public class FollowPathTrackingShoot extends SequentialCommandGroup { private final Odometry odometry; @@ -30,12 +33,15 @@ public class FollowPathTrackingShoot extends ParallelCommandGroup { private double targetDistance; private double distanceTolerance; - private boolean shouldShoot() { - double distance = Field.getAllianceSpeakerPose().getTranslation() - .minus(projectedRobotPose.get().getTranslation2d()) + public static boolean shouldShoot() { + Translation2d speaker = Field.getAllianceSpeakerPose().getTranslation(); + double distance = speaker + // .minus(projectedRobotPose.get().getTranslation2d()) + .minus(Odometry.getInstance().getPose().getTranslation()) .getNorm(); - return Math.abs(targetDistance - distance) < distanceTolerance; + // return Math.abs(targetDistance - distance) < distanceTolerance; + return distance > 2.25; } public FollowPathTrackingShoot(PathPlannerPath path) { @@ -55,13 +61,17 @@ public FollowPathTrackingShoot(PathPlannerPath path) { targetDistance = Alignment.PODIUM_SHOT_DISTANCE.get(); addCommands( - SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), - new SequentialCommandGroup( - new WaitUntilCommand(this::shouldShoot), - new ConveyorShoot(), - new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), - new ConveyorStop(), - new IntakeStop() + new ShooterPodiumShot(), + new ShooterWaitForTarget(), + new ParallelCommandGroup( + SwerveDrive.getInstance().followPathWithSpeakerAlignCommand(path), + new SequentialCommandGroup( + new WaitUntilCommand(FollowPathTrackingShoot::shouldShoot), + new ConveyorShoot(), + new WaitCommand(Settings.Conveyor.SHOOT_WAIT_DELAY.get()), + new ConveyorStop(), + new IntakeStop() + ) ) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java new file mode 100644 index 00000000..4b4495ee --- /dev/null +++ b/src/main/java/com/stuypulse/robot/commands/conveyor/ConveyorShootNoIntake.java @@ -0,0 +1,22 @@ +package com.stuypulse.robot.commands.conveyor; + +import com.stuypulse.robot.subsystems.conveyor.Conveyor; + +import edu.wpi.first.wpilibj2.command.InstantCommand; + +public class ConveyorShootNoIntake extends InstantCommand { + + private final Conveyor conveyor; + + public ConveyorShootNoIntake() { + conveyor = Conveyor.getInstance(); + + addRequirements(conveyor); + } + + @Override + public void initialize() { + conveyor.toShooter(); + } + +} diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java index bdab892e..5d6998df 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -21,6 +21,8 @@ import com.stuypulse.stuylib.math.Vector2D; import com.stuypulse.stuylib.streams.booleans.BStream; import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC; +import com.stuypulse.stuylib.streams.numbers.IStream; +import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; @@ -45,6 +47,7 @@ public static SwerveDriveToShootMoving withHigherDebounce() { private final PIDController distanceController; private final AnglePIDController angleController; + private final IStream distanceToSpeaker; private final BStream isAligned; private final VStream robotPose; @@ -86,6 +89,9 @@ public SwerveDriveToShootMoving(Number targetDistance, double debounce) { .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) .add(robotPose); + distanceToSpeaker = IStream.create(() -> Field.getAllianceSpeakerPose().getTranslation().minus(projectedRobotPose.get().getTranslation2d()).getNorm()) + .filtered(new LowPassFilter(0.05)); + isAligned = BStream.create(this::isAligned) .filtered(new BDebounceRC.Rising(debounce)); @@ -122,17 +128,18 @@ private boolean isAligned() { @Override public void execute() { - Translation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() - .minus(projectedRobotPose.get().getTranslation2d()); + Rotation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() + .minus(projectedRobotPose.get().getTranslation2d()) + .getAngle(); - double speed = -distanceController.update(getTargetDistance(), toSpeaker.getNorm()); + double speed = -distanceController.update(getTargetDistance(), distanceToSpeaker.get()); double rotation = angleController.update( - Angle.fromRotation2d(toSpeaker.getAngle()).add(Angle.k180deg), + Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), Angle.fromRotation2d(odometry.getPose().getRotation())); Translation2d speeds = new Translation2d( speed, - toSpeaker.getAngle()); + toSpeaker); if (Math.abs(rotation) < Swerve.ALIGN_OMEGA_DEADBAND.get()) rotation = 0; @@ -151,7 +158,9 @@ public void execute() { intake.stop(); } - SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.getAngle().plus(Rotation2d.fromDegrees(180)).getDegrees()); + SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees()); + SmartDashboard.putNumber("Alignment/Velocity X", robotVelocity.get().x); + SmartDashboard.putNumber("Alignment/Velocity Y", robotVelocity.get().y); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 70b79518..edab759b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -354,7 +354,7 @@ public interface Feeder { public interface Feedforward { double kS = 0.71611; - double kV = 0.0032; + double kV = 0.0035; double kA = 0.076981; } @@ -425,13 +425,13 @@ public interface Rotation { public interface Shoot { public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 4.0); + SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 5.0); SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.05); } public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 8.0); + SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 5.0); SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.0); } diff --git a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java index 164bca11..507dc03c 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/amper/AmperImpl.java @@ -166,6 +166,10 @@ public void periodic() { voltage = 0; } + if (getTargetHeight() == Settings.Amper.Lift.MIN_HEIGHT && voltage > 0) { + voltage = 0; + } + if (getTargetHeight() == Settings.Amper.Lift.TRAP_SCORE_HEIGHT && voltage < 0.75) { voltage = 0.75; } From 7d1411671358446ef77f0dcc73fd21a60babe23c Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 12:29:27 -0400 Subject: [PATCH 06/10] Tune 4 piece --- src/main/deploy/pathplanner/paths/B to A.path | 4 +-- .../pathplanner/paths/C to B Close.path | 4 +-- src/main/deploy/pathplanner/paths/C to B.path | 4 +-- .../pathplanner/paths/CShoot to H (CHGF).path | 12 ++++----- .../deploy/pathplanner/paths/Curved Line.path | 4 +-- .../deploy/pathplanner/paths/D to Shoot.path | 4 +-- .../pathplanner/paths/F to Shoot (HGF).path | 14 +++++----- .../pathplanner/paths/Ferry Shot to E.path | 4 +-- .../pathplanner/paths/G to Shoot (HGF).path | 12 ++++----- .../pathplanner/paths/H to HShoot (HGF).path | 24 ++++++++++------- .../pathplanner/paths/HShoot to G (HGF).path | 8 +++--- .../deploy/pathplanner/paths/Mobility.path | 4 +-- .../pathplanner/paths/Preload to A.path | 4 +-- .../pathplanner/paths/Preload to C Close.path | 4 +-- .../deploy/pathplanner/paths/SPEED A.path | 4 +-- .../deploy/pathplanner/paths/SPEED B.path | 4 +-- .../pathplanner/paths/Sharp Curved Line.path | 4 +-- .../deploy/pathplanner/paths/Simple Note.path | 4 +-- .../pathplanner/paths/Start to H (HGF).path | 16 ++++++------ .../paths/Straight Line Turning.path | 4 +-- .../pathplanner/paths/Straight Line.path | 4 +-- .../commands/auton/HGF/FourPieceHGF.java | 26 ++++++++++++++----- .../swerve/SwerveDriveToShootMoving.java | 3 ++- 23 files changed, 98 insertions(+), 77 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path index 3fbd922d..c0bf1429 100644 --- a/src/main/deploy/pathplanner/paths/B to A.path +++ b/src/main/deploy/pathplanner/paths/B to A.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path index e2d6156f..e52e881d 100644 --- a/src/main/deploy/pathplanner/paths/C to B Close.path +++ b/src/main/deploy/pathplanner/paths/C to B Close.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path index 1125ee7f..c509df3a 100644 --- a/src/main/deploy/pathplanner/paths/C to B.path +++ b/src/main/deploy/pathplanner/paths/C to B.path @@ -38,8 +38,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path index 07571c6a..95a1284e 100644 --- a/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path +++ b/src/main/deploy/pathplanner/paths/CShoot to H (CHGF).path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": { - "x": 5.120084675408034, - "y": -0.14032985330927936 + "x": 5.268463833761794, + "y": 0.39999999999999997 }, "nextControl": null, "isLocked": false, @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Curved Line.path b/src/main/deploy/pathplanner/paths/Curved Line.path index a7bdaed6..ffa21a81 100644 --- a/src/main/deploy/pathplanner/paths/Curved Line.path +++ b/src/main/deploy/pathplanner/paths/Curved Line.path @@ -80,8 +80,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/D to Shoot.path b/src/main/deploy/pathplanner/paths/D to Shoot.path index ba36f36e..357a1275 100644 --- a/src/main/deploy/pathplanner/paths/D to Shoot.path +++ b/src/main/deploy/pathplanner/paths/D to Shoot.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path index c31efa7e..82964740 100644 --- a/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/F to Shoot (HGF).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.508704809076285, - "y": 4.189449101120425 + "x": 5.559068829433955, + "y": 4.954498043255346 }, "isLocked": false, "linkedName": "F" }, { "anchor": { - "x": 3.338562098420493, - "y": 5.052304113867487 + "x": 2.18, + "y": 3.1400677569873663 }, "prevControl": { - "x": 4.81527342684315, - "y": 4.724383578396787 + "x": 3.3717256982662116, + "y": 1.8074229296606745 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -20.559668324391616, + "rotation": -52.04098183775284, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path index 3dee2c2f..180624fe 100644 --- a/src/main/deploy/pathplanner/paths/Ferry Shot to E.path +++ b/src/main/deploy/pathplanner/paths/Ferry Shot to E.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path index 874a9f14..0101b601 100644 --- a/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/G to Shoot (HGF).path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.917428261138573, - "y": 1.0335790082691276 + "x": 5.229776385478697, + "y": 0.5379311043522382 }, "isLocked": false, "linkedName": "G" }, { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": { - "x": 3.4288766013429455, - "y": 1.4617693053049925 + "x": 2.271155311541392, + "y": 2.03478287298455 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path index 6cce4637..93675792 100644 --- a/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path +++ b/src/main/deploy/pathplanner/paths/H to HShoot (HGF).path @@ -3,32 +3,38 @@ "waypoints": [ { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": null, "nextControl": { - "x": 5.932118299133126, - "y": 0.24833522794883411 + "x": 6.080497457486887, + "y": 0.7886650812581135 }, "isLocked": false, "linkedName": "H" }, { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": { - "x": 3.188773241245417, - "y": 2.2398182778317244 + "x": 2.873031071299537, + "y": 2.473701366680523 }, "nextControl": null, "isLocked": false, "linkedName": "HShoot" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.35, + "rotationDegrees": -39.8209274955494, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path b/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path index 83ed8fd4..1d433c2a 100644 --- a/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path +++ b/src/main/deploy/pathplanner/paths/HShoot to G (HGF).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.943718987153074, - "y": 3.4965521782895426 + "x": 1.6279768172071953, + "y": 3.730435267138341 }, "prevControl": null, "nextControl": { - "x": 3.8409597536149493, - "y": 0.7380457221309411 + "x": 3.525217583669071, + "y": 0.9719288109797397 }, "isLocked": false, "linkedName": "HShoot" diff --git a/src/main/deploy/pathplanner/paths/Mobility.path b/src/main/deploy/pathplanner/paths/Mobility.path index 7da2abe1..375f5e97 100644 --- a/src/main/deploy/pathplanner/paths/Mobility.path +++ b/src/main/deploy/pathplanner/paths/Mobility.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Preload to A.path b/src/main/deploy/pathplanner/paths/Preload to A.path index 89aec544..e57b613f 100644 --- a/src/main/deploy/pathplanner/paths/Preload to A.path +++ b/src/main/deploy/pathplanner/paths/Preload to A.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Preload to C Close.path b/src/main/deploy/pathplanner/paths/Preload to C Close.path index 9ac2206d..e9294dd9 100644 --- a/src/main/deploy/pathplanner/paths/Preload to C Close.path +++ b/src/main/deploy/pathplanner/paths/Preload to C Close.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/SPEED A.path b/src/main/deploy/pathplanner/paths/SPEED A.path index 76848e8e..5472ca69 100644 --- a/src/main/deploy/pathplanner/paths/SPEED A.path +++ b/src/main/deploy/pathplanner/paths/SPEED A.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/SPEED B.path b/src/main/deploy/pathplanner/paths/SPEED B.path index f5963551..3806d63d 100644 --- a/src/main/deploy/pathplanner/paths/SPEED B.path +++ b/src/main/deploy/pathplanner/paths/SPEED B.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sharp Curved Line.path b/src/main/deploy/pathplanner/paths/Sharp Curved Line.path index e9c74b2d..a9535b05 100644 --- a/src/main/deploy/pathplanner/paths/Sharp Curved Line.path +++ b/src/main/deploy/pathplanner/paths/Sharp Curved Line.path @@ -80,8 +80,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Simple Note.path b/src/main/deploy/pathplanner/paths/Simple Note.path index 649b587f..a96b7a1f 100644 --- a/src/main/deploy/pathplanner/paths/Simple Note.path +++ b/src/main/deploy/pathplanner/paths/Simple Note.path @@ -64,8 +64,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Start to H (HGF).path b/src/main/deploy/pathplanner/paths/Start to H (HGF).path index 15eec615..2bbdf4a1 100644 --- a/src/main/deploy/pathplanner/paths/Start to H (HGF).path +++ b/src/main/deploy/pathplanner/paths/Start to H (HGF).path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.76, - "y": 3.262 + "x": 1.87, + "y": 3.264181107065918 }, "prevControl": null, "nextControl": { - "x": 2.639123454924323, - "y": 1.1446271693862373 + "x": 3.661009849273592, + "y": 1.8329545447689473 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.14162084164624, - "y": -0.14032985330927936 + "x": 8.290000000000001, + "y": 0.4 }, "prevControl": { - "x": 5.120084675408034, - "y": -0.14032985330927938 + "x": 5.268463833761794, + "y": 0.4 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Straight Line Turning.path b/src/main/deploy/pathplanner/paths/Straight Line Turning.path index 650f6895..16a158be 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line Turning.path +++ b/src/main/deploy/pathplanner/paths/Straight Line Turning.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Straight Line.path b/src/main/deploy/pathplanner/paths/Straight Line.path index 68db48a3..3b116f7c 100644 --- a/src/main/deploy/pathplanner/paths/Straight Line.path +++ b/src/main/deploy/pathplanner/paths/Straight Line.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 4.0, + "maxVelocity": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java index dd01cce4..700bafbc 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/HGF/FourPieceHGF.java @@ -5,27 +5,41 @@ import com.stuypulse.robot.commands.auton.FollowPathAndIntake; import com.stuypulse.robot.commands.conveyor.ConveyorShootRoutine; import com.stuypulse.robot.commands.shooter.ShooterPodiumShot; +import com.stuypulse.robot.commands.shooter.ShooterStop; import com.stuypulse.robot.commands.shooter.ShooterWaitForTarget; +import com.stuypulse.robot.commands.swerve.SwerveDriveToPose; import com.stuypulse.robot.commands.swerve.SwerveDriveToShoot; +import com.stuypulse.robot.constants.Settings.Alignment; +import com.stuypulse.robot.constants.Settings.Auton; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; public class FourPieceHGF extends SequentialCommandGroup { public FourPieceHGF(PathPlannerPath... paths) { addCommands( - new ShooterPodiumShot(), + new ParallelCommandGroup( + new WaitCommand(Auton.SHOOTER_STARTUP_DELAY) + .andThen(new ShooterPodiumShot()), - new ShooterWaitForTarget() - .withTimeout(1.0), + SwerveDriveToPose.speakerRelative(-45) + .withTolerance(0.1, 0.1, 2) + ), + + new ShooterWaitForTarget(), new ConveyorShootRoutine(), + // new ShooterStop(), new FollowPathAndIntake(paths[0]), - new FollowPathAlignAndShoot(paths[1], SwerveDriveToShoot.withHigherDebounce()), + new FollowPathAlignAndShoot(paths[1], new SwerveDriveToShoot() + .withTolerance(0.06, 5)), new FollowPathAndIntake(paths[2]), - new FollowPathAlignAndShoot(paths[3], SwerveDriveToShoot.withHigherDebounce()), + new FollowPathAlignAndShoot(paths[3], new SwerveDriveToShoot() + .withTolerance(0.05, 5)), new FollowPathAndIntake(paths[4]), - new FollowPathAlignAndShoot(paths[5], SwerveDriveToShoot.withHigherDebounce()) + new FollowPathAlignAndShoot(paths[5], new SwerveDriveToShoot()) ); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java index 5d6998df..dd1d35f0 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -83,7 +83,8 @@ public SwerveDriveToShootMoving(Number targetDistance, double debounce) { robotVelocity = robotPose .filtered(new VDerivative()) - .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)); + .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)) + .polling(0.02); projectedRobotPose = robotVelocity .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) From 03dfbdc42b900921afb5aaad0365129ca4021d22 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 15:06:31 -0400 Subject: [PATCH 07/10] Alignment + shooter kI testing --- .../robot/commands/AmpScoreRoutine.java | 2 +- .../auton/FollowPathTrackingShoot.java | 3 +- .../swerve/SwerveDriveDriveDirection.java | 9 ++-- .../commands/swerve/SwerveDriveToShoot.java | 17 ++++--- .../swerve/SwerveDriveToShootMoving.java | 50 ++++++++++--------- .../stuypulse/robot/constants/Settings.java | 15 +++--- .../robot/subsystems/odometry/Odometry.java | 16 ++++++ .../robot/subsystems/shooter/ShooterImpl.java | 9 ++-- 8 files changed, 72 insertions(+), 49 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java index 50d66b63..9ba14b32 100644 --- a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java +++ b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java @@ -82,7 +82,7 @@ public AmpScoreRoutine() { new AmperToHeight(Settings.Amper.Lift.MIN_HEIGHT), new SwerveDriveDriveDirection( - new Vector2D(new Translation2d( + () -> new Vector2D(new Translation2d( Score.DRIVE_AWAY_SPEED, Field.getAllianceAmpTag().getLocation().toPose2d().getRotation()))) ); diff --git a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java index 5a654629..6b2eb089 100644 --- a/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/auton/FollowPathTrackingShoot.java @@ -50,8 +50,7 @@ public FollowPathTrackingShoot(PathPlannerPath path) { robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); robotVelocity = robotPose - .filtered(new VDerivative()) - .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)); + .filtered(new VDerivative()); projectedRobotPose = robotVelocity .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java index c2d0e612..c8d0eabe 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDriveDirection.java @@ -2,6 +2,7 @@ import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.stuylib.math.Vector2D; +import com.stuypulse.stuylib.streams.vectors.VStream; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.Command; @@ -10,9 +11,9 @@ public class SwerveDriveDriveDirection extends Command { private final SwerveDrive swerve; - private final Vector2D direction; + private final VStream direction; - public SwerveDriveDriveDirection(Vector2D direction) { + public SwerveDriveDriveDirection(VStream direction) { swerve = SwerveDrive.getInstance(); this.direction = direction; @@ -21,8 +22,8 @@ public SwerveDriveDriveDirection(Vector2D direction) { @Override public void execute() { swerve.setFieldRelativeSpeeds(new ChassisSpeeds( - direction.x, - direction.y, + direction.get().x, + direction.get().y, 0)); } diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index 509faf65..7d4e7ba1 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; +import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Angle; @@ -24,6 +25,7 @@ import com.stuypulse.stuylib.streams.numbers.filters.Derivative; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -39,7 +41,7 @@ public static SwerveDriveToShoot withHigherDebounce() { private final SwerveDrive swerve; private final Odometry odometry; - private final PIDController distanceController; + private final Controller distanceController; private final AnglePIDController angleController; private final IStream velocityError; @@ -66,7 +68,11 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) { swerve = SwerveDrive.getInstance(); odometry = Odometry.getInstance(); - distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD); + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); @@ -98,11 +104,6 @@ public SwerveDriveToShoot withTolerance(double distanceTolerance, double angleTo return this; } - public SwerveDriveToShoot withDistanceConstants(double p, double i, double d) { - distanceController.setPID(p, i, d); - return this; - } - public SwerveDriveToShoot withRotationConstants(double p, double i, double d) { angleController.setPID(p, i, d); return this; @@ -127,7 +128,7 @@ public void initialize() { public void execute() { Rotation2d toSpeaker = getTranslationToSpeaker().getAngle(); - double speed = -distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); double rotation = angleController.update( Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), Angle.fromRotation2d(odometry.getPose().getRotation())); diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java index dd1d35f0..ca65dfb7 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShootMoving.java @@ -14,6 +14,7 @@ import com.stuypulse.robot.subsystems.intake.Intake; import com.stuypulse.robot.subsystems.odometry.Odometry; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; +import com.stuypulse.stuylib.control.Controller; import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController; import com.stuypulse.stuylib.control.feedback.PIDController; import com.stuypulse.stuylib.math.Angle; @@ -24,9 +25,11 @@ import com.stuypulse.stuylib.streams.numbers.IStream; import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter; import com.stuypulse.stuylib.streams.vectors.VStream; -import com.stuypulse.stuylib.streams.vectors.filters.VDerivative; import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter; +import com.stuypulse.stuylib.streams.vectors.filters.VTimedMovingAverage; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -44,14 +47,13 @@ public static SwerveDriveToShootMoving withHigherDebounce() { private final Conveyor conveyor; private final Intake intake; - private final PIDController distanceController; + private final Controller distanceController; private final AnglePIDController angleController; private final IStream distanceToSpeaker; private final BStream isAligned; private final VStream robotPose; - private final VStream robotVelocity; private final VStream projectedRobotPose; private final Number targetDistance; @@ -64,7 +66,7 @@ public SwerveDriveToShootMoving() { } public SwerveDriveToShootMoving(Number targetDistance) { - this(targetDistance, Alignment.DEBOUNCE_TIME); + this(targetDistance, 0.1); } public SwerveDriveToShootMoving(Number targetDistance, double debounce) { @@ -75,28 +77,30 @@ public SwerveDriveToShootMoving(Number targetDistance, double debounce) { conveyor = Conveyor.getInstance(); intake = Intake.getInstance(); - distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD); + distanceController = new PIDController(Shoot.Translation.kP, Shoot.Translation.kI, Shoot.Translation.kD) + .setOutputFilter( + x -> -x, + x -> MathUtil.clamp(x, -Alignment.MAX_ALIGNMENT_SPEED, +Alignment.MAX_ALIGNMENT_SPEED) + ); angleController = new AnglePIDController(Shoot.Rotation.kP, Shoot.Rotation.kI, Shoot.Rotation.kD); robotPose = VStream.create(() -> new Vector2D(odometry.getPose().getTranslation())); - robotVelocity = robotPose - .filtered(new VDerivative()) - .filtered(new VLowPassFilter(Alignment.PROJECTED_POSE_RC)) - .polling(0.02); - - projectedRobotPose = robotVelocity + projectedRobotPose = VStream.create(() -> new Vector2D(odometry.getRobotVelocity())) .filtered(v -> v.mul(Alignment.NOTE_TO_GOAL_TIME)) - .add(robotPose); + .add(robotPose) + .filtered( + new VTimedMovingAverage(0.1), + new VLowPassFilter(0.1) + ); - distanceToSpeaker = IStream.create(() -> Field.getAllianceSpeakerPose().getTranslation().minus(projectedRobotPose.get().getTranslation2d()).getNorm()) - .filtered(new LowPassFilter(0.05)); + distanceToSpeaker = IStream.create(() -> Field.getAllianceSpeakerPose().getTranslation().minus(projectedRobotPose.get().getTranslation2d()).getNorm()); isAligned = BStream.create(this::isAligned) - .filtered(new BDebounceRC.Rising(debounce)); + .filtered(new BDebounceRC.Both(debounce)); - distanceTolerance = 0.033; + distanceTolerance = 0.08; angleTolerance = Alignment.ANGLE_TOLERANCE.get(); addRequirements(swerve); @@ -112,11 +116,6 @@ public SwerveDriveToShootMoving withTolerance(double distanceTolerance, double a return this; } - public SwerveDriveToShootMoving withDistanceConstants(double p, double i, double d) { - distanceController.setPID(p, i, d); - return this; - } - public SwerveDriveToShootMoving withRotationConstants(double p, double i, double d) { angleController.setPID(p, i, d); return this; @@ -129,11 +128,14 @@ private boolean isAligned() { @Override public void execute() { + Odometry.getInstance().getField().getObject("Projected Pose") + .setPose(new Pose2d(projectedRobotPose.get().getTranslation2d(), odometry.getPose().getRotation())); + Rotation2d toSpeaker = Field.getAllianceSpeakerPose().getTranslation() .minus(projectedRobotPose.get().getTranslation2d()) .getAngle(); - double speed = -distanceController.update(getTargetDistance(), distanceToSpeaker.get()); + double speed = distanceController.update(getTargetDistance(), distanceToSpeaker.get()); double rotation = angleController.update( Angle.fromRotation2d(toSpeaker).add(Angle.k180deg), Angle.fromRotation2d(odometry.getPose().getRotation())); @@ -160,8 +162,8 @@ public void execute() { } SmartDashboard.putNumber("Alignment/To Shoot Target Angle", toSpeaker.plus(Rotation2d.fromDegrees(180)).getDegrees()); - SmartDashboard.putNumber("Alignment/Velocity X", robotVelocity.get().x); - SmartDashboard.putNumber("Alignment/Velocity Y", robotVelocity.get().y); + SmartDashboard.putNumber("Alignment/Distance Error", distanceController.getError()); + SmartDashboard.putNumber("Alignment/Distance Output", distanceController.getOutput()); } @Override diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index edab759b..b4f7470d 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -329,7 +329,7 @@ public interface Shooter { 500, new SmartNumber("Shooter/Ferry Feeder RPM", 3000)); - double AT_RPM_EPSILON = 125; + double AT_RPM_EPSILON = 200; SmartNumber RPM_CHANGE_RC = new SmartNumber("Shooter/RPM Change RC", 0.2); double RPM_CHANGE_DIP_THRESHOLD = 300; @@ -342,7 +342,7 @@ public interface Feedforward { public interface PID { double kP = 0.00034711; - double kI = 0.0; + double kI = kP * 20; double kD = 0.0; } } @@ -354,13 +354,13 @@ public interface Feeder { public interface Feedforward { double kS = 0.71611; - double kV = 0.0035; + double kV = 0.0032; double kA = 0.076981; } public interface PID { double kP = 0.00020863; - double kI = 0.0; + double kI = kP * 20; double kD = 0.0; } } @@ -408,8 +408,9 @@ public interface Alignment { SmartNumber INTO_CHAIN_SPEED = new SmartNumber("Alignment/Trap/Into Chain Speed", 0.25); - double PROJECTED_POSE_RC = 0.05; - double NOTE_TO_GOAL_TIME = 0.5; + double NOTE_TO_GOAL_TIME = 0.4; + + double MAX_ALIGNMENT_SPEED = 1.5; public interface Translation { SmartNumber kP = new SmartNumber("Alignment/Translation/kP", 5.0); @@ -427,7 +428,7 @@ public interface Shoot { public interface Translation { SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 5.0); SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.05); + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.1); // try 0.1 } public interface Rotation { diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java index 6f9ea045..300f2972 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -9,6 +9,7 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.robot.constants.Cameras; import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.constants.Settings; import com.stuypulse.robot.subsystems.swerve.SwerveDrive; import com.stuypulse.robot.subsystems.vision.AprilTagVision; import com.stuypulse.robot.util.LinearRegression; @@ -57,6 +58,9 @@ public static Odometry getInstance() { private Pose2d lastGoodPose; + private Translation2d robotVelocity; + private Translation2d lastPose; + protected Odometry() { SwerveDrive swerve = SwerveDrive.getInstance(); odometry = new SwerveDriveOdometry( @@ -89,6 +93,9 @@ protected Odometry() { thetaRegression = new LinearRegression(Cameras.thetaStdDevs); lastGoodPose = new Pose2d(); + + robotVelocity = new Translation2d(); + lastPose = new Translation2d(); SmartDashboard.putData("Field", field); } @@ -194,9 +201,16 @@ public void periodic() { lastGoodPose = getPose(); } + robotVelocity = getPose().getTranslation().minus(lastPose).div(Settings.DT); + lastPose = getPose().getTranslation(); + updateTelemetry(); } + public Translation2d getRobotVelocity() { + return robotVelocity; + } + private void updateTelemetry() { SmartDashboard.putNumber("Odometry/Odometry/X", odometry.getPoseMeters().getTranslation().getX()); SmartDashboard.putNumber("Odometry/Odometry/Y", odometry.getPoseMeters().getTranslation().getY()); @@ -204,6 +218,8 @@ private void updateTelemetry() { SmartDashboard.putNumber("Odometry/Estimator/X", estimator.getEstimatedPosition().getTranslation().getX()); SmartDashboard.putNumber("Odometry/Estimator/Y", estimator.getEstimatedPosition().getTranslation().getY()); + SmartDashboard.putNumber("Odometry/Estimator/VX", robotVelocity.getX()); + SmartDashboard.putNumber("Odometry/Estimator/VY", robotVelocity.getY()); SmartDashboard.putNumber("Odometry/Estimator/Rotation", estimator.getEstimatedPosition().getRotation().getDegrees()); odometryPose2D.setPose(odometry.getPoseMeters()); diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 97eff0aa..2e07bd6a 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -60,11 +60,14 @@ protected ShooterImpl() { feederEncoder.setVelocityConversionFactor(Feeder.GEARING); leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); + .add(new PIDController(PID.kP, PID.kI, PID.kD) + .setIntegratorFilter(600, 5.0 / PID.kI)); rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() - .add(new PIDController(PID.kP, PID.kI, PID.kD)); + .add(new PIDController(PID.kP, PID.kI, PID.kD) + .setIntegratorFilter(600, 5.0 / PID.kI)); feederController = new MotorFeedforward(Feeder.Feedforward.kS, Feeder.Feedforward.kV, Feeder.Feedforward.kA).velocity() - .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD)); + .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD) + .setIntegratorFilter(600, 5.0 / PID.kI)); rpmChange = IStream.create(this::getAverageShooterRPM) .filtered(new HighPassFilter(Settings.Shooter.RPM_CHANGE_RC)); From 455128ac01d1e442546b7ce26c479802e50faade Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 15:06:40 -0400 Subject: [PATCH 08/10] Add driver camera to shuffleboard --- shuffleboard.json | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/shuffleboard.json b/shuffleboard.json index 789c920e..9c7ffe39 100644 --- a/shuffleboard.json +++ b/shuffleboard.json @@ -159,6 +159,27 @@ "_glyph": 148, "_showGlyph": false } + }, + "1,0": { + "size": [ + 5, + 4 + ], + "content": { + "_type": "Camera Stream", + "_source0": "camera_server://USB Camera 0", + "_title": "USB Camera 0", + "_glyph": 148, + "_showGlyph": false, + "Crosshair/Show crosshair": true, + "Crosshair/Crosshair color": "#FFFFFFFF", + "Controls/Show controls": true, + "Controls/Rotation": "HALF", + "compression": -1.0, + "fps": -1, + "imageWidth": 0, + "imageHeight": 0 + } } } } From 4aef60d106ba2053f9d30d5b3b5ed3d72bff34b8 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 15:06:55 -0400 Subject: [PATCH 09/10] Increase path constraints --- .pathplanner/settings.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 5303a099..7d74ae9d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -8,8 +8,8 @@ "Tests" ], "autoFolders": [], - "defaultMaxVel": 5.0, - "defaultMaxAccel": 4.0, + "defaultMaxVel": 6.0, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 5.0 From 4a170066c4a2a5f78ccff33bc6bb43a68f2f55b3 Mon Sep 17 00:00:00 2001 From: Prog694 Date: Sat, 30 Mar 2024 15:49:33 -0400 Subject: [PATCH 10/10] Alignment --- .../com/stuypulse/robot/RobotContainer.java | 5 +++-- .../commands/swerve/SwerveDriveToShoot.java | 2 +- .../stuypulse/robot/constants/Settings.java | 18 +++++++++--------- .../robot/subsystems/shooter/ShooterImpl.java | 6 +++--- 4 files changed, 16 insertions(+), 15 deletions(-) diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java index c12264eb..f085899c 100644 --- a/src/main/java/com/stuypulse/robot/RobotContainer.java +++ b/src/main/java/com/stuypulse/robot/RobotContainer.java @@ -207,6 +207,7 @@ private void configureDriverBindings() { driver.getTopButton() .whileTrue(new SwerveDriveAutoFerry(driver)); + // .whileTrue(new SwerveDriveToShootMoving()); // climb driver.getRightButton() @@ -323,11 +324,11 @@ public void configureAutons() { // AutonConfig PodiumCloseCBAE = new AutonConfig("Podium Close 5 Piece CBAE", FivePiecePodiumForwardCBAE::new, // "Forward First Piece to C", "C to B 2", "B to A","A to E", "E to Shoot"); - HGF.registerBlue(autonChooser) + HGF.registerDefaultBlue(autonChooser) .registerRed(autonChooser); CBAED - .registerDefaultBlue(autonChooser) + .registerBlue(autonChooser) .registerRed(autonChooser); CHGF diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java index 7d4e7ba1..c542c1fd 100644 --- a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java +++ b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java @@ -87,7 +87,7 @@ public SwerveDriveToShoot(Number targetDistance, double debounce) { isAligned = BStream.create(this::isAligned) .filtered(new BDebounceRC.Rising(debounce)); - distanceTolerance = 0.033; + distanceTolerance = 0.05; angleTolerance = Alignment.ANGLE_TOLERANCE.get(); velocityTolerance = 0.1; diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index b4f7470d..188aed3f 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -342,7 +342,7 @@ public interface Feedforward { public interface PID { double kP = 0.00034711; - double kI = kP * 20; + double kI = 0; double kD = 0.0; } } @@ -354,13 +354,13 @@ public interface Feeder { public interface Feedforward { double kS = 0.71611; - double kV = 0.0032; + double kV = 0.00335; double kA = 0.076981; } public interface PID { double kP = 0.00020863; - double kI = kP * 20; + double kI = 0.0; double kD = 0.0; } } @@ -391,7 +391,7 @@ public interface Conveyor { } public interface Alignment { - double DEBOUNCE_TIME = 0.2; + double DEBOUNCE_TIME = 0.05; SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1); SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1); @@ -410,7 +410,7 @@ public interface Alignment { double NOTE_TO_GOAL_TIME = 0.4; - double MAX_ALIGNMENT_SPEED = 1.5; + double MAX_ALIGNMENT_SPEED = 2.5; public interface Translation { SmartNumber kP = new SmartNumber("Alignment/Translation/kP", 5.0); @@ -426,15 +426,15 @@ public interface Rotation { public interface Shoot { public interface Translation { - SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 5.0); + SmartNumber kP = new SmartNumber("ShootAlign/Translation/kP", 7.5); SmartNumber kI = new SmartNumber("ShootAlign/Translation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.1); // try 0.1 + SmartNumber kD = new SmartNumber("ShootAlign/Translation/kD", 0.8); } public interface Rotation { - SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 5.0); + SmartNumber kP = new SmartNumber("ShootAlign/Rotation/kP", 6.0); SmartNumber kI = new SmartNumber("ShootAlign/Rotation/kI", 0.0); - SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.0); + SmartNumber kD = new SmartNumber("ShootAlign/Rotation/kD", 0.4); } } } diff --git a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java index 2e07bd6a..0908e09b 100644 --- a/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java +++ b/src/main/java/com/stuypulse/robot/subsystems/shooter/ShooterImpl.java @@ -61,13 +61,13 @@ protected ShooterImpl() { leftController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() .add(new PIDController(PID.kP, PID.kI, PID.kD) - .setIntegratorFilter(600, 5.0 / PID.kI)); + .setIntegratorFilter(1000, 0.5 / PID.kI)); rightController = new MotorFeedforward(Feedforward.kS, Feedforward.kV, Feedforward.kA).velocity() .add(new PIDController(PID.kP, PID.kI, PID.kD) - .setIntegratorFilter(600, 5.0 / PID.kI)); + .setIntegratorFilter(1000, 0.5 / PID.kI)); feederController = new MotorFeedforward(Feeder.Feedforward.kS, Feeder.Feedforward.kV, Feeder.Feedforward.kA).velocity() .add(new PIDController(Feeder.PID.kP, Feeder.PID.kI, Feeder.PID.kD) - .setIntegratorFilter(600, 5.0 / PID.kI)); + .setIntegratorFilter(1000, 1.0 / PID.kI)); rpmChange = IStream.create(this::getAverageShooterRPM) .filtered(new HighPassFilter(Settings.Shooter.RPM_CHANGE_RC));