From 23eb066864a1ca5c2f39cf2bc2946e2db613fcf1 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Fri, 22 Mar 2024 20:07:49 -0500 Subject: [PATCH 01/82] Added spin to shooter (NEEDS TESTING) --- .../java/frc/robot/commands/AimShoot.java | 86 +------------------ .../java/frc/robot/subsystems/Shooter.java | 3 + 2 files changed, 6 insertions(+), 83 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index d78c022..5bf7eea 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -130,17 +130,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i addRequirements(eyes, shooterPivot, shooter); // instantiate objects - shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolation = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolation = new InterpolatingDoubleTreeMap(); - - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - - shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); + shooterAngleInterpolation = new InterpolatingDoubleTreeMap();; // create points in angle linear interpolation line // TODO tune these values @@ -150,21 +140,6 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolation.put(subWooferDistance, subWooferLeftShooterSpeed); - //shooterLeftSpeedInterpolation.put(d2Distance, d2LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(podiumDistance, podiumLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d3Distance, d3LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolation.put(subWooferDistance, subWooferRightShooterSpeed); - //shooterRightSpeedInterpolation.put(d2Distance, d2RightShooterSpeed); - shooterRightSpeedInterpolation.put(podiumDistance, podiumRightShooterSpeed); - shooterRightSpeedInterpolation.put(d3Distance, d3RightShooterSpeed); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeed); - - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); @@ -172,23 +147,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d2DistanceAuto, d2LeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(podiumDistanceAuto, podiumLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d3DistanceAuto, d3LeftShooterSpeedAuto); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d2DistanceAuto, d2RightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(podiumDistanceAuto, podiumRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d3DistanceAuto, d3RightShooterSpeedAuto); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeedAuto); - shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); - shooterLeftSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotLeftShooterSpeed); - shooterRightSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotRightShooterSpeed); } @@ -202,16 +161,10 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolation = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolation = new InterpolatingDoubleTreeMap(); shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationAuto = new InterpolatingDoubleTreeMap(); shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterLeftSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); - shooterRightSpeedInterpolationElevator = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values @@ -221,46 +174,13 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolation.put(subWooferDistance, subWooferLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d2Distance, d2LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(podiumDistance, podiumLeftShooterSpeed); - shooterLeftSpeedInterpolation.put(d3Distance, d3LeftShooterSpeed); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolation.put(subWooferDistance, subWooferRightShooterSpeed); - shooterRightSpeedInterpolation.put(d2Distance, d2RightShooterSpeed); - shooterRightSpeedInterpolation.put(podiumDistance, podiumRightShooterSpeed); - shooterRightSpeedInterpolation.put(d3Distance, d3RightShooterSpeed); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeed); - - - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); shooterAngleInterpolationAuto.put(podiumDistanceAuto, podiumAngleAuto); shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); - // create points in shooter power linear interpolation line - // TODO tune these values - shooterLeftSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d2DistanceAuto, d2LeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(podiumDistanceAuto, podiumLeftShooterSpeedAuto); - shooterLeftSpeedInterpolationAuto.put(d3DistanceAuto, d3LeftShooterSpeedAuto); - shooterLeftSpeedInterpolation.put(xSpotDistance, xSpotLeftShooterSpeed); - - shooterRightSpeedInterpolationAuto.put(subWooferDistanceAuto, subWooferRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d2DistanceAuto, d2RightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(podiumDistanceAuto, podiumRightShooterSpeedAuto); - shooterRightSpeedInterpolationAuto.put(d3DistanceAuto, d3RightShooterSpeedAuto); - shooterRightSpeedInterpolation.put(xSpotDistance, xSpotRightShooterSpeedAuto); - shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); - shooterLeftSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotLeftShooterSpeed); - shooterRightSpeedInterpolationElevator.put(elevatorShotDistance, elevatorShotRightShooterSpeed); - } @Override @@ -288,8 +208,8 @@ public void execute() { // get desired shooter power using the linear interpolation // x (input) = distance // y (output) = shooter power - leftShooterSpeed = shooterLeftSpeedInterpolation.get(distance); - rightShooterSpeed = shooterRightSpeedInterpolation.get(distance); + leftShooterSpeed = shooter.shotSpeedRPS; + rightShooterSpeed = shooter.shotSpeedRPS * shooter.shooterSpinReduction; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 11e1173..028bd8c 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -38,6 +38,9 @@ public class Shooter extends SubsystemBase { public final double stopShooterVoltage = 0.0; public final double shooterSpeedToleranceRPS = 100.0/60.0; //100 RPM + public final double shotSpeedRPS = 90; + public final double shooterSpinReduction = 0.95; + // left shooter motor PID private final double lShooterMotorPGains = 0.05; From 21dac06967fe78ab6ff65ff45eafec0d83c19745 Mon Sep 17 00:00:00 2001 From: leo Date: Sat, 23 Mar 2024 14:07:52 -0500 Subject: [PATCH 02/82] Source side auto proto for missed note --- .../Prototype Sourceside Smart Auto.auto | 131 ++++++++++++++++++ .../paths/Sourceside missed note 1.path | 58 ++++++++ src/main/java/frc/robot/RobotContainer.java | 11 ++ .../java/frc/robot/subsystems/Shooter.java | 7 + 4 files changed, 207 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Sourceside missed note 1.path diff --git a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto new file mode 100644 index 0000000..6cd8961 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto @@ -0,0 +1,131 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7036808379666589, + "y": 4.401663085651201 + }, + "rotation": -59.47029410006589 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 2" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sourceside missed note 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 4" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path new file mode 100644 index 0000000..85f4391 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.036539916496203, + "y": 0.7887936060569642 + }, + "prevControl": null, + "nextControl": { + "x": 6.3713089703759485, + "y": 1.3828233587665275 + }, + "isLocked": false, + "linkedName": "Loadside Auto Path 1 Endpoint" + }, + { + "anchor": { + "x": 8.163136421167144, + "y": 2.395595396177869 + }, + "prevControl": { + "x": 6.721883906401265, + "y": 2.181355157490862 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Sourceside Auto path 3 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.55, + "rotationDegrees": 32.61473904001839, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "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 f9493a2..7a60cb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -211,6 +211,17 @@ public RobotContainer() { NamedCommands.registerCommand("Score Far", AimThenShootFar); NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)); NamedCommands.registerCommand("Fire", new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage))); + NamedCommands.registerCommand("Check Note", new InstantCommand(() -> s_Shooter.checkNote())); + NamedCommands.registerCommand("Got Note", new ConditionalCommand( + new WaitCommand (15), + new InstantCommand (), + () -> s_Shooter.gotNote + )); + NamedCommands.registerCommand("Not Got Note", new ConditionalCommand( + new InstantCommand (), + new WaitCommand (15), + () -> s_Shooter.gotNote + )); autoChooser = AutoBuilder.buildAutoChooser(); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 028bd8c..9fb6c03 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -74,6 +74,8 @@ public class Shooter extends SubsystemBase { private TalonFXConfigurator configF; private double m_setSpeed = 0.0; + public boolean gotNote = true; + // constructor public Shooter() { // motors @@ -154,6 +156,11 @@ public void setLoaderVoltage(double loaderSpeedVoltage) { */ public boolean getBreakBeamOutput() { return breakBeam.get(); + + } + + public void checkNote() { + gotNote = getBreakBeamOutput(); } public Trigger getBreakBeamTrigger() { From 229dfdea211de00f246cad7084efd3b9b48521de Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 23 Mar 2024 14:08:03 -0500 Subject: [PATCH 03/82] 930 tuning distances and new intake positions --- .../java/frc/robot/commands/AimShoot.java | 24 +++++++------------ .../java/frc/robot/subsystems/Intake.java | 4 ++-- .../java/frc/robot/subsystems/Shooter.java | 2 +- 3 files changed, 11 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 5bf7eea..c30afa6 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -41,16 +41,8 @@ public class AimShoot extends Command { // required WPILib class objects private InterpolatingDoubleTreeMap shooterAngleInterpolation; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterAngleInterpolationAuto; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterAngleInterpolationElevator; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationElevator; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationElevator; // local variables private double shooterAngle; @@ -62,7 +54,7 @@ public class AimShoot extends Command { // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.25; + private final double subWooferDistance = 1.21; //1.21 at 930, 1.25 at comp private final double subWooferAngle = 115.0; private final double subWooferLeftShooterSpeed = 90.0; private final double subWooferRightShooterSpeed = subWooferLeftShooterSpeed; @@ -72,13 +64,13 @@ public class AimShoot extends Command { private final double d2LeftShooterSpeed = 90.0; private final double d2RightShooterSpeed = d2LeftShooterSpeed; - private final double podiumDistance = 3.17; - private final double podiumAngle = 137; + private final double podiumDistance = 2.98; //2.98 at 930, 3.17 at comp + private final double podiumAngle = 138; private final double podiumLeftShooterSpeed = 90.0; private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; - private final double d3Distance = 4.0; - private final double d3Angle = 137.0; + private final double d3Distance = 4.1; + private final double d3Angle = 140.0; private final double d3LeftShooterSpeed = 95.0; private final double d3RightShooterSpeed = d3LeftShooterSpeed; @@ -130,7 +122,9 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i addRequirements(eyes, shooterPivot, shooter); // instantiate objects - shooterAngleInterpolation = new InterpolatingDoubleTreeMap();; + shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); + shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); + shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values @@ -161,9 +155,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0886038..8494743 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -27,8 +27,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 28.0; - public final double intakeGroundPosition = -74.0; + public final double intakeSafePosition = 41.7; + public final double intakeGroundPosition = -63.1; public final double intakeSourcePosition = intakeSafePosition; // PID values diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 028bd8c..417e0b2 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -39,7 +39,7 @@ public class Shooter extends SubsystemBase { public final double shooterSpeedToleranceRPS = 100.0/60.0; //100 RPM public final double shotSpeedRPS = 90; - public final double shooterSpinReduction = 0.95; + public final double shooterSpinReduction = 1.0; // left shooter motor PID From c79f3c3d1f7a7f86c3e770c10004ef638da9025b Mon Sep 17 00:00:00 2001 From: leo Date: Sat, 23 Mar 2024 14:12:30 -0500 Subject: [PATCH 04/82] Intake positions changed --- src/main/java/frc/robot/subsystems/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0886038..8494743 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -27,8 +27,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 28.0; - public final double intakeGroundPosition = -74.0; + public final double intakeSafePosition = 41.7; + public final double intakeGroundPosition = -63.1; public final double intakeSourcePosition = intakeSafePosition; // PID values From 4647482b66360d5a8c5326abd817576e769119ad Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 23 Mar 2024 17:20:53 -0500 Subject: [PATCH 05/82] Smart sourceside auto done --- .../Prototype Sourceside Smart Auto.auto | 157 +++++++++++++++++- .../deploy/pathplanner/autos/Stationary.auto | 25 +++ .../Ampside Auto (Inside Center) Note 2.path | 11 +- .../deploy/pathplanner/paths/New Path.path | 52 ++++++ .../paths/Sourceside Auto Path 1.path | 14 +- .../paths/Sourceside Auto Path 2.path | 10 +- .../paths/Sourceside Auto Path 3.path | 12 +- .../paths/Sourceside Auto Path 4.path | 20 +-- .../paths/Sourceside Auto Path 5.path | 10 +- .../Sourceside missed Note 2 Path 2.path | 58 +++++++ .../paths/Sourceside missed Note 2.path | 76 +++++++++ .../paths/Sourceside missed note 1.path | 42 +++-- src/main/java/frc/robot/Robot.java | 2 + src/main/java/frc/robot/RobotContainer.java | 43 +++-- .../java/frc/robot/commands/AimShoot.java | 24 +-- .../java/frc/robot/subsystems/Shooter.java | 6 +- 16 files changed, 474 insertions(+), 88 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Stationary.auto create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path create mode 100644 src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path diff --git a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto index 6cd8961..2a4c4b5 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto @@ -44,9 +44,22 @@ "data": { "commands": [ { - "type": "path", + "type": "race", "data": { - "pathName": "Sourceside Auto Path 2" + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 2" + } + } + ] } }, { @@ -112,15 +125,149 @@ } }, { - "type": "path", + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside Auto Path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", "data": { - "pathName": "Sourceside Auto Path 4" + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Sourceside missed Note 2" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] } }, { "type": "named", "data": { - "name": "AutoScore" + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Sourceside missed Note 2 Path 2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Stationary.auto b/src/main/deploy/pathplanner/autos/Stationary.auto new file mode 100644 index 0000000..ee6c837 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Stationary.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Check Note" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path index 7c1ca18..01e9ab8 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path @@ -33,11 +33,18 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.35, + "waypointRelativePos": 0.65, "command": { "type": "parallel", "data": { - "commands": [] + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } } diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..fc9a85e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.018336848061597, + "y": 7.069927876510557 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0, + "y": 7.0 + }, + "prevControl": { + "x": 1.93, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path index 5b32b63..fb2c0cb 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.036539916496203, - "y": 0.7887936060569642 + "x": 8.260518347838573, + "y": 0.6427207160475723 }, "prevControl": { - "x": 7.036539916496203, - "y": 0.7887936060569642 + "x": 7.260518347838573, + "y": 0.6427207160475723 }, "nextControl": null, "isLocked": false, @@ -30,9 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.65, + "waypointRelativePos": 0.55, "rotationDegrees": 0, - "rotateFast": false + "rotateFast": true } ], "constraintZones": [], @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path index 119da31..9495150 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.036539916496203, - "y": 0.7887936060569642 + "x": 8.260518347838573, + "y": 0.6427207160475723 }, "prevControl": null, "nextControl": { - "x": 8.075492687165683, - "y": 0.7887936060569642 + "x": 8.299471118508054, + "y": 0.6427207160475723 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path index edce25d..2c845e5 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.163136421167144, - "y": 2.395595396177869 + "x": 8.241041962510971, + "y": 2.4150717815077387 }, "prevControl": { - "x": 6.8566562791000525, - "y": 1.924590227639699 + "x": 6.93456182044388, + "y": 1.9440666129695687 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.7, + "waypointRelativePos": 0.6, "command": { "type": "parallel", "data": { @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path index 96be0d7..296cdfb 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.163136421167144, - "y": 2.395595396177869 + "x": 8.241041962510971, + "y": 2.4150717815077387 }, "prevControl": null, "nextControl": { - "x": 4.721797688395219, - "y": 1.5161824972439915 + "x": 4.608696097586862, + "y": 0.3310985506857081 }, "isLocked": false, "linkedName": "Sourceside Auto path 3 Endpoint" }, { "anchor": { - "x": 2.3576264436479493, - "y": 3.909726337971194 + "x": 2.261791664745845, + "y": 3.953706222952186 }, "prevControl": { - "x": 2.4597283762468765, - "y": 2.4246073183504393 + "x": 3.157705390143877, + "y": 2.960410570880455 }, "nextControl": null, "isLocked": false, @@ -33,13 +33,13 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -26.56505117707797, + "rotation": -39.3693172423648, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path index 932eb17..f1be36d 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.3576264436479493, - "y": 3.909726337971194 + "x": 2.261791664745845, + "y": 3.953706222952186 }, "prevControl": null, "nextControl": { - "x": 4.540528748910404, - "y": 0.15581108267791854 + "x": 4.4446939700083, + "y": 0.19979096765891036 }, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 4.0, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path new file mode 100644 index 0000000..5b01fd2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.241041962510971, + "y": 4.119255498297474 + }, + "prevControl": null, + "nextControl": { + "x": 3.2356109314828365, + "y": 4.5574741683291204 + }, + "isLocked": false, + "linkedName": "Sourceside Missed Note 2 Endpoint" + }, + { + "anchor": { + "x": 2.057289618731077, + "y": 3.5933930942594987 + }, + "prevControl": { + "x": 4.248382968889307, + "y": 1.9963294968108325 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.55, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.75, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -55.124671655397854, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path new file mode 100644 index 0000000..726dd69 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path @@ -0,0 +1,76 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.241041962510971, + "y": 2.4150717815077387 + }, + "prevControl": null, + "nextControl": { + "x": 6.14733053902644, + "y": 3.40836743357947 + }, + "isLocked": false, + "linkedName": "Sourceside Auto path 3 Endpoint" + }, + { + "anchor": { + "x": 8.241041962510971, + "y": 4.119255498297474 + }, + "prevControl": { + "x": 6.770574869738114, + "y": 3.8368479109437468 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Sourceside Missed Note 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": 25.624492662715717, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.57, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 20.28255908891649, + "rotateFast": false + }, + "reversed": false, + "folder": "Source Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path index 85f4391..dac3497 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 8.036539916496203, - "y": 0.7887936060569642 + "x": 8.260518347838573, + "y": 0.6427207160475723 }, "prevControl": null, "nextControl": { - "x": 6.3713089703759485, - "y": 1.3828233587665275 + "x": 6.595287401718319, + "y": 1.2367504687571356 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" }, { "anchor": { - "x": 8.163136421167144, - "y": 2.395595396177869 + "x": 8.241041962510971, + "y": 2.4150717815077387 }, "prevControl": { - "x": 6.721883906401265, - "y": 2.181355157490862 + "x": 6.799789447745093, + "y": 2.200831542820732 }, "nextControl": null, "isLocked": false, @@ -36,10 +36,28 @@ } ], "constraintZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -54,5 +72,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4126cb3..532e5c8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -76,6 +76,7 @@ public void disabledPeriodic() {} @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + @@ -100,6 +101,7 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7a60cb4..3a090c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -168,14 +168,7 @@ public RobotContainer() { } - //spin up shooter when we have a note in the indexer - //NOTE: IF REMOVED NEED TO ADD SHOOTER SPINUP FOR AMP SCORE - s_Shooter.setDefaultCommand( - new ConditionalCommand( - new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(40, 40), s_Shooter), - () -> s_Shooter.getBreakBeamOutput()) - ); + // Configure the button bindings configureButtonBindings(); @@ -183,11 +176,19 @@ public RobotContainer() { //Command ElevatorAtPosition = new s_Elevator.ElevatorAtPosition(); Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false), + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false).alongWith(new TeleopSwerve( + s_Swerve, + () -> 0, + () -> 0, + () -> 0, + () -> driverDpadUp.getAsBoolean(), + () -> s_Eyes.getTargetRotation(), + () -> false, + rotationSpeed, + true + )), new SequentialCommandGroup( - new PrintCommand("pre release:" + Timer.getFPGATimestamp()), new WaitCommand(1.0).until(() -> prepareShot()), - new PrintCommand("post release:" + Timer.getFPGATimestamp()), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), new WaitCommand(1.0)).until(() -> s_Shooter.getBreakBeamOutput()) ); @@ -205,7 +206,7 @@ public RobotContainer() { ); NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) .until(() -> !s_Shooter.getBreakBeamOutput()) - .withTimeout(2) + .withTimeout(.5) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); NamedCommands.registerCommand("Score Far", AimThenShootFar); @@ -258,7 +259,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> false, //driverLeftTrigger.getAsBoolean(), rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) @@ -272,7 +273,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> false,//driverLeftTrigger.getAsBoolean(), rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) @@ -292,7 +293,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getTargetRotation(), - () -> driverB.getAsBoolean(), + () -> false,//driverB.getAsBoolean(), rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), @@ -314,7 +315,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getTargetRotation(), - () -> driverB.getAsBoolean(), + () -> false,//driverB.getAsBoolean(), rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), @@ -328,7 +329,14 @@ private void configureButtonBindings() { ); } - + //spin up shooter when we have a note in the indexer + //NOTE: IF REMOVED NEED TO ADD SHOOTER SPINUP FOR AMP SCORE + s_Shooter.setDefaultCommand( + new ConditionalCommand( + new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), + new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(40, 40), s_Shooter), + () -> s_Shooter.getBreakBeamOutput()) + ); // shoot speaker driverRightTrigger.onTrue( @@ -470,6 +478,7 @@ public void rumbleControllers() { } } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 5bf7eea..c30afa6 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -41,16 +41,8 @@ public class AimShoot extends Command { // required WPILib class objects private InterpolatingDoubleTreeMap shooterAngleInterpolation; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolation; - private InterpolatingDoubleTreeMap shooterAngleInterpolationAuto; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationAuto; - private InterpolatingDoubleTreeMap shooterAngleInterpolationElevator; - private InterpolatingDoubleTreeMap shooterLeftSpeedInterpolationElevator; - private InterpolatingDoubleTreeMap shooterRightSpeedInterpolationElevator; // local variables private double shooterAngle; @@ -62,7 +54,7 @@ public class AimShoot extends Command { // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.25; + private final double subWooferDistance = 1.21; //1.21 at 930, 1.25 at comp private final double subWooferAngle = 115.0; private final double subWooferLeftShooterSpeed = 90.0; private final double subWooferRightShooterSpeed = subWooferLeftShooterSpeed; @@ -72,13 +64,13 @@ public class AimShoot extends Command { private final double d2LeftShooterSpeed = 90.0; private final double d2RightShooterSpeed = d2LeftShooterSpeed; - private final double podiumDistance = 3.17; - private final double podiumAngle = 137; + private final double podiumDistance = 2.98; //2.98 at 930, 3.17 at comp + private final double podiumAngle = 138; private final double podiumLeftShooterSpeed = 90.0; private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; - private final double d3Distance = 4.0; - private final double d3Angle = 137.0; + private final double d3Distance = 4.1; + private final double d3Angle = 140.0; private final double d3LeftShooterSpeed = 95.0; private final double d3RightShooterSpeed = d3LeftShooterSpeed; @@ -130,7 +122,9 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i addRequirements(eyes, shooterPivot, shooter); // instantiate objects - shooterAngleInterpolation = new InterpolatingDoubleTreeMap();; + shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); + shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); + shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values @@ -161,9 +155,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 9fb6c03..b799467 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -39,7 +39,7 @@ public class Shooter extends SubsystemBase { public final double shooterSpeedToleranceRPS = 100.0/60.0; //100 RPM public final double shotSpeedRPS = 90; - public final double shooterSpinReduction = 0.95; + public final double shooterSpinReduction = 1.0; // left shooter motor PID @@ -74,7 +74,7 @@ public class Shooter extends SubsystemBase { private TalonFXConfigurator configF; private double m_setSpeed = 0.0; - public boolean gotNote = true; + public boolean gotNote = false; // constructor public Shooter() { @@ -160,7 +160,7 @@ public boolean getBreakBeamOutput() { } public void checkNote() { - gotNote = getBreakBeamOutput(); + gotNote = !getBreakBeamOutput(); } public Trigger getBreakBeamTrigger() { From 5178c55b9daa7c75294b8b3810b2e5b4cd4b1500 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sun, 24 Mar 2024 23:18:26 -0500 Subject: [PATCH 06/82] started shoot on move --- src/main/java/frc/robot/RobotContainer.java | 23 +++--- .../java/frc/robot/commands/AimShoot.java | 15 ++-- src/main/java/frc/robot/subsystems/Eyes.java | 71 ++++++++++++++++++- .../java/frc/robot/subsystems/Shooter.java | 2 +- 4 files changed, 90 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9493a2..cba9f18 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,7 +123,7 @@ public class RobotContainer { private final Elevator s_Elevator = new Elevator(); private final Intake s_Intake = new Intake(); private final Shooter s_Shooter = new Shooter(); - private final Eyes s_Eyes = new Eyes(s_Swerve); + private final Eyes s_Eyes = new Eyes(s_Swerve, s_Shooter); @@ -183,7 +183,7 @@ public RobotContainer() { //Command ElevatorAtPosition = new s_Elevator.ElevatorAtPosition(); Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false), + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false), new SequentialCommandGroup( new PrintCommand("pre release:" + Timer.getFPGATimestamp()), new WaitCommand(1.0).until(() -> prepareShot()), @@ -192,13 +192,7 @@ public RobotContainer() { new WaitCommand(1.0)).until(() -> s_Shooter.getBreakBeamOutput()) ); - Command AimThenShootFar = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false), - new SequentialCommandGroup( - new WaitCommand(1.5), - new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), - new WaitCommand(1.0)) - ); + NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) .until(() -> !s_Shooter.getBreakBeamOutput()) @@ -208,8 +202,7 @@ public RobotContainer() { .withTimeout(2) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); - NamedCommands.registerCommand("Score Far", AimThenShootFar); - NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)); + NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false)); NamedCommands.registerCommand("Fire", new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage))); autoChooser = AutoBuilder.buildAutoChooser(); @@ -250,7 +243,7 @@ private void configureButtonBindings() { () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true)) ); } else { @@ -264,7 +257,7 @@ private void configureButtonBindings() { () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true)) ); } @@ -284,7 +277,7 @@ private void configureButtonBindings() { () -> driverB.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( @@ -306,7 +299,7 @@ private void configureButtonBindings() { () -> driverB.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true)), + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index d78c022..8bd6e92 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -58,6 +58,7 @@ public class AimShoot extends Command { private double rightShooterSpeed; public boolean isElevatorShot = false; + public boolean onMove = false; // positions @@ -119,13 +120,13 @@ public class AimShoot extends Command { private final double elevatorShotLeftShooterSpeed = 90; private final double elevatorShotRightShooterSpeed = elevatorShotLeftShooterSpeed; - // constructor - public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot) { + public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot, boolean onMove) { this.eyes = eyes; this.shooterPivot = shooterPivot; this.shooter = shooter; this.isElevatorShot = isElevatorShot; + this.onMove = onMove; addRequirements(eyes, shooterPivot, shooter); @@ -272,8 +273,14 @@ public void execute() { distance = elevatorShotDistance; shooterAngle = elevatorShotAngle; } else if(manualDistance == 0) { - distance = eyes.getDistanceFromTarget(); - shooterAngle = shooterAngleInterpolation.get(distance); + if (onMove == true) { + distance = eyes.getDistanceFromMovingTarget(); + shooterAngle = shooterAngleInterpolation.get(distance); + } else { + distance = eyes.getDistanceFromTarget(); + shooterAngle = shooterAngleInterpolation.get(distance); + } + } else { distance = manualDistance; shooterAngle = shooterAngleInterpolation.get(distance); diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index cbdb9cf..0bf1304 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -25,6 +25,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.robot.subsystems.Swerve; @@ -34,6 +35,7 @@ public class Eyes extends SubsystemBase { // Swerve subsystem for pose estimator Swerve s_Swerve; + Shooter s_Shooter; // create objects and variables public LimelightHelpers limelight; @@ -41,13 +43,15 @@ public class Eyes extends SubsystemBase { public double ty; public double ta; public double tID; + private double accelerationCompensation = 0.1; public boolean controllerRumble = false; // constuctor - public Eyes(Swerve swerve) { + public Eyes(Swerve swerve, Shooter shooter) { s_Swerve = swerve; + s_Shooter = shooter; } @@ -196,6 +200,12 @@ public boolean swerveAtPosition() { } } + public double getShotTime(double distance) { + + double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1.333) * 0.0254; //TODO: change shooter gear ratio + return (distance / linearSpeed) + 0.1; + } + public double getDistanceFromTarget() { double distance; @@ -218,6 +228,65 @@ public double getDistanceFromTarget() { } + public Pose2d getMovingTarget() { + double shotTime = getShotTime(getDistanceFromTarget()); + + Translation2d movingGoal = new Translation2d(); + Translation2d movingGoalLocation = new Translation2d(); + + //TODO MAKE FIELD RELATIVE? + double robotVelX = s_Swerve.getChassisSpeed().vxMetersPerSecond; + double robotVelY = s_Swerve.getChassisSpeed().vyMetersPerSecond; + + //TODO calculate accelerations + double robotAccelX = 0; + double robotAccelY = 0; + + for(int i=0;i<5;i++){ + + double virtualGoalX = getTargetPose().getX() - shotTime * (robotVelX + robotAccelX * accelerationCompensation); + double virtualGoalY = getTargetPose().getY() - shotTime * (robotVelY + robotAccelY * accelerationCompensation); + + SmartDashboard.putNumber("Goal X", virtualGoalX); + SmartDashboard.putNumber("Goal Y", virtualGoalY); + + Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); + + Translation2d toTestGoal = testGoalLocation.minus(s_Swerve.getEstimatedPose().getTranslation()); + + double newShotTime = getShotTime(toTestGoal.getDistance(new Translation2d())); + + + + if(Math.abs(newShotTime-shotTime) <= 0.010){ + i=4; + } + + if(i == 4){ + movingGoalLocation = testGoalLocation; + SmartDashboard.putNumber("NewShotTime", newShotTime); + } + else{ + shotTime = newShotTime; + } + + } + return new Pose2d(movingGoalLocation, getTargetPose().getRotation().toRotation2d()); + + } + + public double getDistanceFromMovingTarget() { + + double distance; + + double xDistanceToSpeaker = getMovingTarget().getX() - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); + double yDistanceToSpeaker = getMovingTarget().getY() - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); + distance = Math.sqrt(Math.pow(xDistanceToSpeaker, 2) + Math.pow(yDistanceToSpeaker, 2)); + + return distance; + + } + public double getDistanceFromTargetBlind() { double distance; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 11e1173..4fd8f97 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -69,7 +69,7 @@ public class Shooter extends SubsystemBase { public DigitalInput breakBeam; private TalonFXConfigurator configF; - private double m_setSpeed = 0.0; + public double m_setSpeed = 0.0; // constructor public Shooter() { From 30e49656da21b60a4373e4350988ebe7836acdfe Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Mon, 25 Mar 2024 09:14:07 -0500 Subject: [PATCH 07/82] Calc and aim for moving target --- src/main/java/frc/robot/RobotContainer.java | 4 +-- src/main/java/frc/robot/subsystems/Eyes.java | 26 ++++++++++++++++++++ 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cba9f18..17970da 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -239,7 +239,7 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), + () -> s_Eyes.getMovingTargetRotation(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, true @@ -253,7 +253,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), + () -> s_Eyes.getMovingTargetRotation(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, true diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 0bf1304..5705c3b 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -183,6 +183,32 @@ public double getTargetRotation() { return -angle + 180; } + public double getMovingTargetRotation() { + + Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); + Pose2d targetPose = getMovingTarget(); + + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + + double targetX = targetPose.getX(); + double targetY = targetPose.getY(); + + double angle = (Math.atan((targetY - robotY) / (targetX - robotX)) * (180 / Math.PI)); + + if (robotX > targetX) { + + angle = angle + 180; + + } + + SmartDashboard.putNumber("angle", angle); + SmartDashboard.putNumber(" inverted angle", -angle); + + return -angle + 180; + } + + public boolean swerveAtPosition() { From ce40f8469e9a66ea032fa4aa892ac4c5dad1909c Mon Sep 17 00:00:00 2001 From: Driver Station Date: Mon, 25 Mar 2024 18:48:27 -0500 Subject: [PATCH 08/82] Manual climb on operator controls --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/subsystems/Elevator.java | 8 ++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f9493a2..f308e7c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -234,7 +234,8 @@ private boolean prepareShot() { */ private void configureButtonBindings() { - + //Manual climb + operatorLB.onTrue(new InstantCommand(() -> s_Elevator.climb())); // zero gyro driverY.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 762bc61..3c6b086 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -63,6 +64,7 @@ public class Elevator extends SubsystemBase { private double heightlimit = 16; public double elevatorspeed = 0.1; public double restingposition = 0; + public double climbingPosition = -2.0; public double shootingPosition = 12.0; private double elevatorP = 1.5; //4.0 @@ -165,6 +167,12 @@ public double getTargetElevatorPosition(){ return targetElevatorPosition; } + public void climb() { + m_elevator1.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(climbingPosition)); + m_elevator2.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(climbingPosition)); + SetElevatorPosition(climbingPosition); + } + private double motorRotationsToInches(double rotations) { return rotations * Constants.ELEVATOR_ROTATIONS_TO_IN; From 75b28704dee09ce00fdfffbf662c840b81a4b85c Mon Sep 17 00:00:00 2001 From: Driver Station Date: Mon, 25 Mar 2024 19:06:25 -0500 Subject: [PATCH 09/82] fixed side effect of manual climb --- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/subsystems/Elevator.java | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f308e7c..3e50f43 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -369,6 +369,7 @@ private void configureButtonBindings() { // climb reach driverLB.onTrue( new SequentialCommandGroup( + new InstantCommand(() -> s_Elevator.resetElevatorReverseSoftlimit()), new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), s_Elevator.ElevatorAtPosition(), @@ -378,6 +379,7 @@ private void configureButtonBindings() { new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ) + ); diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 3c6b086..23ddaf1 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -173,6 +173,10 @@ public void climb() { SetElevatorPosition(climbingPosition); } + public void resetElevatorReverseSoftlimit(){ + m_elevator1.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(restingposition)); + m_elevator2.setSoftLimit(SoftLimitDirection.kReverse,(float)inchesToMotorRotations(restingposition)); + } private double motorRotationsToInches(double rotations) { return rotations * Constants.ELEVATOR_ROTATIONS_TO_IN; From ca47b1e10c7c56c8aa9853306659a36c8eaf68f4 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Mon, 25 Mar 2024 19:56:34 -0500 Subject: [PATCH 10/82] started source intake from shooter --- src/main/java/frc/robot/RobotContainer.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e50f43..6294fc0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -234,8 +234,7 @@ private boolean prepareShot() { */ private void configureButtonBindings() { - //Manual climb - operatorLB.onTrue(new InstantCommand(() -> s_Elevator.climb())); + // zero gyro driverY.onTrue(new InstantCommand(() -> s_Swerve.zeroHeading())); @@ -413,6 +412,8 @@ private void configureButtonBindings() { /* Operator Buttons */ + //Manual climb + operatorLB.onTrue(new InstantCommand(() -> s_Elevator.climb())); // aim amp operatorLeftTrigger.whileTrue( @@ -432,7 +433,12 @@ private void configureButtonBindings() { new AmpElevatorRetract(s_Elevator) ) ); - + operatorY.onTrue( + new ParallelCommandGroup( + new InstantCommand(() -> s_Elevator.SetElevatorPosition()), + new InstantCommand(() -> s_ShooterPivot.Set), + ) + ); operatorRightTrigger.onTrue( new ParallelCommandGroup( From b5dc0466b71e1e9f05e93c981f1d1d0830a31531 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Tue, 26 Mar 2024 08:55:57 -0500 Subject: [PATCH 11/82] added field centric velocity and acceleration calculations to shooter. --- .../java/frc/lib/util/FieldRelativeAccel.java | 36 +++++++++++++++++++ .../java/frc/lib/util/FieldRelativeSpeed.java | 29 +++++++++++++++ src/main/java/frc/robot/subsystems/Eyes.java | 6 ++-- .../java/frc/robot/subsystems/Swerve.java | 13 +++++++ 4 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/lib/util/FieldRelativeAccel.java create mode 100644 src/main/java/frc/lib/util/FieldRelativeSpeed.java diff --git a/src/main/java/frc/lib/util/FieldRelativeAccel.java b/src/main/java/frc/lib/util/FieldRelativeAccel.java new file mode 100644 index 0000000..17d5103 --- /dev/null +++ b/src/main/java/frc/lib/util/FieldRelativeAccel.java @@ -0,0 +1,36 @@ +package frc.lib.util; + +public class FieldRelativeAccel { + public double ax; + public double ay; + public double alpha; + + public FieldRelativeAccel(double ax, double ay, double alpha) { + this.ax = ax; + this.ay = ay; + this.alpha = alpha; + } + + public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpeed, double time) { + this.ax = (newSpeed.vx - oldSpeed.vx) / time; + this.ay = (newSpeed.vy - oldSpeed.vy) / time; + this.alpha = (newSpeed.omega - oldSpeed.omega) / time; + + if (Math.abs(this.ax) > 6.0) { + this.ax = 6.0 * Math.signum(this.ax); + } + if (Math.abs(this.ay) > 6.0) { + this.ay = 6.0 * Math.signum(this.ay); + } + if (Math.abs(this.alpha) > 4 * Math.PI) { + this.alpha = 4 * Math.PI * Math.signum(this.alpha); + } + } + + public FieldRelativeAccel() { + this.ax = 0.0; + this.ay = 0.0; + this.alpha = 0.0; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/lib/util/FieldRelativeSpeed.java b/src/main/java/frc/lib/util/FieldRelativeSpeed.java new file mode 100644 index 0000000..273e995 --- /dev/null +++ b/src/main/java/frc/lib/util/FieldRelativeSpeed.java @@ -0,0 +1,29 @@ +package frc.lib.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class FieldRelativeSpeed { + public double vx; + public double vy; + public double omega; + + public FieldRelativeSpeed(double vx, double vy, double omega) { + this.vx = vx; + this.vy = vy; + this.omega = omega; + } + + public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) { + this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(), + chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(), + chassisSpeed.omegaRadiansPerSecond); + } + + public FieldRelativeSpeed() { + this.vx = 0.0; + this.vy = 0.0; + this.omega = 0.0; + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 5705c3b..c789af8 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -12,6 +12,8 @@ package frc.robot.subsystems; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeSpeed; import frc.robot.Constants; import java.util.List; import edu.wpi.first.wpilibj.DriverStation; @@ -265,8 +267,8 @@ public Pose2d getMovingTarget() { double robotVelY = s_Swerve.getChassisSpeed().vyMetersPerSecond; //TODO calculate accelerations - double robotAccelX = 0; - double robotAccelY = 0; + double robotAccelX = s_Swerve.fieldRelativeAccel.ax; + double robotAccelY = s_Swerve.fieldRelativeAccel.ay; for(int i=0;i<5;i++){ diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ee19e4..da195ea 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -6,6 +6,9 @@ import frc.robot.SwerveModule; import frc.robot.LimelightHelpers.LimelightTarget_Detector; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeAccel; +import frc.lib.util.FieldRelativeSpeed; import frc.robot.Constants; import frc.robot.LimelightHelpers; import edu.wpi.first.math.kinematics.ChassisSpeeds; @@ -70,6 +73,9 @@ public class Swerve extends SubsystemBase { public StructArrayPublisher swerveKinematicsPublisher; public StructPublisher estimatedRobotPosePublisher; public SwerveDrivePoseEstimator m_poseEstimator; + public FieldRelativeSpeed fieldRelativeVelocity; + public FieldRelativeSpeed lastFieldRelativeVelocity; + public FieldRelativeAccel fieldRelativeAccel; // constructor @@ -78,6 +84,9 @@ public Swerve() { // instantiate objects gyro = new Pigeon2(Constants.Swerve.pigeonID); m_field = new Field2d(); + fieldRelativeVelocity = new FieldRelativeSpeed(); + lastFieldRelativeVelocity = new FieldRelativeSpeed(); + fieldRelativeAccel = new FieldRelativeAccel(); // set gyro gyro.getConfigurator().apply(new Pigeon2Configuration()); @@ -291,6 +300,10 @@ public void resetModulesToAbsolute(){ public void periodic(){ swerveOdometry.update(getGyroYaw(), getModulePositions()); + fieldRelativeVelocity = new FieldRelativeSpeed(getChassisSpeed(), getGyroYaw()); + fieldRelativeAccel = new FieldRelativeAccel(fieldRelativeVelocity, lastFieldRelativeVelocity, 0.02); + lastFieldRelativeVelocity = fieldRelativeVelocity; + SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); From 6c760927e010c9b874017e5ffde478d7db41bb5e Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Tue, 26 Mar 2024 11:28:32 -0500 Subject: [PATCH 12/82] changed shoot on move velocity to field centric --- src/main/java/frc/robot/subsystems/Eyes.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index c789af8..81e70c8 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -263,8 +263,8 @@ public Pose2d getMovingTarget() { Translation2d movingGoalLocation = new Translation2d(); //TODO MAKE FIELD RELATIVE? - double robotVelX = s_Swerve.getChassisSpeed().vxMetersPerSecond; - double robotVelY = s_Swerve.getChassisSpeed().vyMetersPerSecond; + double robotVelX = s_Swerve.fieldRelativeVelocity.vx; //s_Swerve.getChassisSpeed().vxMetersPerSecond + double robotVelY = s_Swerve.fieldRelativeVelocity.vy; //s_Swerve.getChassisSpeed().vyMetersPerSecond //TODO calculate accelerations double robotAccelX = s_Swerve.fieldRelativeAccel.ax; From f373cd660a0ee59571e00911cec0beaafd957858 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Tue, 26 Mar 2024 15:12:11 -0500 Subject: [PATCH 13/82] Remove slow mode while shooting on the move --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17970da..dbe4dca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -240,7 +240,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> false, rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true)) @@ -254,7 +254,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), - () -> driverLeftTrigger.getAsBoolean(), + () -> false, rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true)) From 2b70d341f8da181181743942440c81b67419b663 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 26 Mar 2024 17:45:35 -0500 Subject: [PATCH 14/82] worked on source intake --- src/main/java/frc/robot/RobotContainer.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6294fc0..976ac1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,12 +433,21 @@ private void configureButtonBindings() { new AmpElevatorRetract(s_Elevator) ) ); + //source intake 1 operatorY.onTrue( new ParallelCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition()), new InstantCommand(() -> s_ShooterPivot.Set), ) ); + //source intake 2 + operatorB.onTrue( + new ParallelCommandGroup( + new InstantCommand(() -> s_Elevator.SetElevatorPosition()), + new InstantCommand(() -> s_ShooterPivot.Set), + new InstantCommand(() -> ) + ) + ); operatorRightTrigger.onTrue( new ParallelCommandGroup( From 2066848146718cb6ef5c2f7ce40647572b53c272 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 26 Mar 2024 18:57:07 -0500 Subject: [PATCH 15/82] added comments and changed values for source intake --- src/main/java/frc/robot/RobotContainer.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 976ac1e..21b7f2d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,19 +433,20 @@ private void configureButtonBindings() { new AmpElevatorRetract(s_Elevator) ) ); - //source intake 1 + //source intake 1: shooter pivot will flip and the note will go in on the intake side. operatorY.onTrue( new ParallelCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition()), - new InstantCommand(() -> s_ShooterPivot.Set), - ) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(/*TODO height of elevator for source intake 1 */)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(/*TODO pivot position for source intake 1 */)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) + ) ); - //source intake 2 + //source intake 2: note will come in through the shooter side. operatorB.onTrue( new ParallelCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition()), - new InstantCommand(() -> s_ShooterPivot.Set), - new InstantCommand(() -> ) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(/*TODO height of elevator for source intake 2 */)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(/*TODO pivot position for source intake 2 */)), + new InstantCommand(() -> s_Shooter.setShooterVoltage(/*TODO value to make wheels go oppisite way*/),)//wheels will spin the oppisite way ) ); From e800e7505c305f969f848c6ad878d15f14a20766 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 26 Mar 2024 19:05:37 -0500 Subject: [PATCH 16/82] changed button for manual climb --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21b7f2d..94137c1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -407,13 +407,13 @@ private void configureButtonBindings() { ) ); - + //Manual climb + driverDpadDown.onTrue(new InstantCommand(() -> s_Elevator.climb())); /* Operator Buttons */ - //Manual climb - operatorLB.onTrue(new InstantCommand(() -> s_Elevator.climb())); + // aim amp operatorLeftTrigger.whileTrue( From 140227128cf2807aa08da1996cd5b1972c145d21 Mon Sep 17 00:00:00 2001 From: SubzeroHero76 Date: Tue, 26 Mar 2024 19:13:51 -0500 Subject: [PATCH 17/82] Fixed elevator not going down when intake commanded --- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/commands/RunIntake.java | 9 +++++++-- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e50f43..e918107 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -200,10 +200,10 @@ public RobotContainer() { new WaitCommand(1.0)) ); - NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator) .until(() -> !s_Shooter.getBreakBeamOutput()) ); - NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator) .until(() -> !s_Shooter.getBreakBeamOutput()) .withTimeout(2) ); @@ -335,7 +335,7 @@ private void configureButtonBindings() { // intake driverX.whileTrue( - new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index 8efa7eb..c8c8efc 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -6,6 +6,7 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Eyes; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Elevator; public class RunIntake extends Command { @@ -15,6 +16,7 @@ public class RunIntake extends Command { ShooterPivot s_ShooterPivot; Shooter s_Shooter; Eyes s_Eyes; + Elevator s_Elevator; // required WPILib class objects @@ -25,14 +27,16 @@ public class RunIntake extends Command { // constructor - public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) { + public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes, + Elevator s_Elevator) { this.s_Intake = s_Intake; this.s_ShooterPivot = s_ShooterPivot; this.s_Shooter = s_Shooter; this.s_Eyes = s_Eyes; + this.s_Elevator = s_Elevator; - addRequirements(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes); + addRequirements(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes, s_Elevator); } @@ -42,6 +46,7 @@ public void execute() { s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition); s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage); + s_Elevator.SetElevatorPosition(0); s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition); s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); From faaffedffd837337ecd97e1b0aa7c507479a6ae3 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 26 Mar 2024 20:25:47 -0500 Subject: [PATCH 18/82] Red works, blue probably doesnt --- .../java/frc/lib/util/FieldRelativeAccel.java | 5 +++++ src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/Eyes.java | 21 ++++++++++++------- .../java/frc/robot/subsystems/Swerve.java | 1 + 4 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/lib/util/FieldRelativeAccel.java b/src/main/java/frc/lib/util/FieldRelativeAccel.java index 17d5103..9929ed0 100644 --- a/src/main/java/frc/lib/util/FieldRelativeAccel.java +++ b/src/main/java/frc/lib/util/FieldRelativeAccel.java @@ -1,5 +1,7 @@ package frc.lib.util; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + public class FieldRelativeAccel { public double ax; public double ay; @@ -25,6 +27,9 @@ public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpe if (Math.abs(this.alpha) > 4 * Math.PI) { this.alpha = 4 * Math.PI * Math.signum(this.alpha); } + + SmartDashboard.putNumber("fieldRelativeAccelX", this.ax); + SmartDashboard.putNumber("fieldRelativeAccelY", this.ay); } public FieldRelativeAccel() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dbe4dca..8626d00 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -240,7 +240,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), - () -> false, + () -> true, rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true)) @@ -254,7 +254,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), - () -> false, + () -> true, rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true)) diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 81e70c8..0042707 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -29,6 +29,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import frc.robot.subsystems.Swerve; @@ -45,13 +47,16 @@ public class Eyes extends SubsystemBase { public double ty; public double ta; public double tID; - private double accelerationCompensation = 0.1; + private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times + private StructPublisher posePublisher; public boolean controllerRumble = false; // constuctor public Eyes(Swerve swerve, Shooter shooter) { + posePublisher = NetworkTableInstance.getDefault().getStructTopic("/moving Goal x", Pose2d.struct).publish(); + s_Swerve = swerve; s_Shooter = shooter; } @@ -231,7 +236,7 @@ public boolean swerveAtPosition() { public double getShotTime(double distance) { double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1.333) * 0.0254; //TODO: change shooter gear ratio - return (distance / linearSpeed) + 0.1; + return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time } public double getDistanceFromTarget() { @@ -259,12 +264,11 @@ public double getDistanceFromTarget() { public Pose2d getMovingTarget() { double shotTime = getShotTime(getDistanceFromTarget()); - Translation2d movingGoal = new Translation2d(); Translation2d movingGoalLocation = new Translation2d(); //TODO MAKE FIELD RELATIVE? - double robotVelX = s_Swerve.fieldRelativeVelocity.vx; //s_Swerve.getChassisSpeed().vxMetersPerSecond - double robotVelY = s_Swerve.fieldRelativeVelocity.vy; //s_Swerve.getChassisSpeed().vyMetersPerSecond + double robotVelX = s_Swerve.fieldRelativeVelocity.vx; + double robotVelY = s_Swerve.fieldRelativeVelocity.vy; //TODO calculate accelerations double robotAccelX = s_Swerve.fieldRelativeAccel.ax; @@ -272,8 +276,8 @@ public Pose2d getMovingTarget() { for(int i=0;i<5;i++){ - double virtualGoalX = getTargetPose().getX() - shotTime * (robotVelX + robotAccelX * accelerationCompensation); - double virtualGoalY = getTargetPose().getY() - shotTime * (robotVelY + robotAccelY * accelerationCompensation); + double virtualGoalX = getTargetPose().getX() + shotTime * (robotVelX + robotAccelX * accelerationCompensation); //TODO: Test on blue + double virtualGoalY = getTargetPose().getY() + shotTime * (robotVelY + robotAccelY * accelerationCompensation); //TODO: Test on blue SmartDashboard.putNumber("Goal X", virtualGoalX); SmartDashboard.putNumber("Goal Y", virtualGoalY); @@ -417,5 +421,8 @@ public void periodic() { SmartDashboard.putNumber("target Y", getTargetPose().getY()); SmartDashboard.putNumber("Distance to Target", getDistanceFromTarget()); + + posePublisher.set(getMovingTarget()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index da195ea..63256fe 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -307,6 +307,7 @@ public void periodic(){ SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); + From 7c24d7b83f788d30f9c9ac90a9facbda71a1087d Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Wed, 27 Mar 2024 00:43:26 -0500 Subject: [PATCH 19/82] Calculate looptimes for acceleration --- src/main/java/frc/robot/subsystems/Swerve.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 63256fe..085e1e0 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -76,6 +76,8 @@ public class Swerve extends SubsystemBase { public FieldRelativeSpeed fieldRelativeVelocity; public FieldRelativeSpeed lastFieldRelativeVelocity; public FieldRelativeAccel fieldRelativeAccel; + private double time; + private double lastTime = 0.0; // constructor @@ -300,9 +302,11 @@ public void resetModulesToAbsolute(){ public void periodic(){ swerveOdometry.update(getGyroYaw(), getModulePositions()); + time = Timer.getFPGATimestamp(); fieldRelativeVelocity = new FieldRelativeSpeed(getChassisSpeed(), getGyroYaw()); - fieldRelativeAccel = new FieldRelativeAccel(fieldRelativeVelocity, lastFieldRelativeVelocity, 0.02); + fieldRelativeAccel = new FieldRelativeAccel(fieldRelativeVelocity, lastFieldRelativeVelocity, time-lastTime); //time was 0.02 for standard loop time attempting to calculate it to increase accuracy. May crash with /0 error if startup is too quick lastFieldRelativeVelocity = fieldRelativeVelocity; + lastTime = time; SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); From 295a71da4ee627591fc14ddff799d8c54807bdff Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Wed, 27 Mar 2024 01:37:08 -0500 Subject: [PATCH 20/82] logging moving target --- src/main/java/frc/robot/subsystems/Eyes.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 0042707..5a9ecf3 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -49,14 +49,14 @@ public class Eyes extends SubsystemBase { public double tID; private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times private StructPublisher posePublisher; - + private StructPublisher translationPublisher; public boolean controllerRumble = false; // constuctor public Eyes(Swerve swerve, Shooter shooter) { - posePublisher = NetworkTableInstance.getDefault().getStructTopic("/moving Goal x", Pose2d.struct).publish(); - + posePublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal pose", Pose2d.struct).publish(); + translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal translation", Translation2d.struct).publish(); s_Swerve = swerve; s_Shooter = shooter; } @@ -285,6 +285,7 @@ public Pose2d getMovingTarget() { Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); Translation2d toTestGoal = testGoalLocation.minus(s_Swerve.getEstimatedPose().getTranslation()); + translationPublisher.set(toTestGoal); double newShotTime = getShotTime(toTestGoal.getDistance(new Translation2d())); @@ -303,7 +304,7 @@ public Pose2d getMovingTarget() { } } - return new Pose2d(movingGoalLocation, getTargetPose().getRotation().toRotation2d()); + return new Pose2d(movingGoalLocation, getTargetPose().getRotation().toRotation2d()); //TODO: validate rotation--probably not needed since only get x and y } @@ -407,6 +408,7 @@ else if (limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); + if (LimelightHelpers.getTV("") == true) { s_Swerve.m_poseEstimator.addVisionMeasurement( getRobotPose(), From 6dcc10ec335d7914e289394595a420232f2d8418 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Wed, 27 Mar 2024 08:54:17 -0500 Subject: [PATCH 21/82] LL Update and Filtering --- src/main/java/frc/robot/LimelightHelpers.java | 181 +++++++++++++++++- src/main/java/frc/robot/subsystems/Eyes.java | 28 ++- 2 files changed, 188 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 21c4d8b..4d5c6d3 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.4.0 (March 21, 2024) package frc.robot; @@ -303,6 +303,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -362,9 +374,60 @@ public Results() { public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + + public String error; public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -385,7 +448,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -397,7 +460,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -405,6 +468,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -544,8 +687,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -604,6 +747,17 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -618,6 +772,16 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -643,6 +807,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -765,7 +934,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 5a9ecf3..270542a 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -28,6 +28,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.StructPublisher; @@ -364,30 +365,33 @@ public double getDistanceFromTargetAuto() { } - /*public void updatePoseEstimatorWithVisionBotPose() { + public void updatePoseEstimatorWithVisionBotPose() { //invalid limelight if (LimelightHelpers.getTV("") == false) { return; } // distance from current pose to vision estimated pose - double poseDifference = s_Swerve.m_poseEstimator.getEstimatedPosition().getTranslation() - .getDistance(getRobotPose().getTranslation()); + LimelightHelpers.PoseEstimate lastResult = LimelightHelpers.getBotPoseEstimate_wpiBlue(""); + double fpgaTimestamp = Timer.getFPGATimestamp(); + + Translation2d translation = s_Swerve.m_poseEstimator.getEstimatedPosition().getTranslation(); + double poseDifference = translation.getDistance(lastResult.pose.getTranslation()); double xyStds; double degStds; // multiple targets detected - if (limelight.getNumberOfTargetsVisible() >= 2) { + if (lastResult.tagCount >= 2) { xyStds = 0.5; degStds = 6; } // 1 target with large area and close to estimated pose - else if (LimelightHelpers.getTA() > 0.8 && poseDifference < 0.5) { + else if (lastResult.avgTagArea > 0.8 && poseDifference < 0.5) { xyStds = 1.0; degStds = 12; } // 1 target farther away and estimated pose is close - else if (limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { + else if (lastResult.avgTagArea > 0.1 && poseDifference < 0.3) { xyStds = 2.0; degStds = 30; } @@ -399,22 +403,16 @@ else if (limelight.getBestTargetArea() > 0.1 && poseDifference < 0.3) { s_Swerve.m_poseEstimator.setVisionMeasurementStdDevs( VecBuilder.fill(xyStds, xyStds, Units.degreesToRadians(degStds))); s_Swerve.m_poseEstimator.addVisionMeasurement(getRobotPose(), - Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0)); + fpgaTimestamp - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0)); } - } -/* */ + @Override public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); - if (LimelightHelpers.getTV("") == true) { - s_Swerve.m_poseEstimator.addVisionMeasurement( - getRobotPose(), - Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0) - ); - } + updatePoseEstimatorWithVisionBotPose(); SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); From c6bd52f3ab2de356bc209b352c050b2b78f344a2 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Wed, 27 Mar 2024 13:32:35 -0500 Subject: [PATCH 22/82] fix rumbling (untested) --- src/main/java/frc/robot/commands/AimShoot.java | 2 +- src/main/java/frc/robot/commands/PrepareShot.java | 6 ++++-- src/main/java/frc/robot/subsystems/Eyes.java | 15 +++++++++------ 3 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 8bd6e92..3130db3 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -306,7 +306,7 @@ public void execute() { // set shooter speed power to calculated value shooter.shootingMotorsSetControl(rightShooterSpeed, leftShooterSpeed); - if(shooterPivot.atPosition() == true && eyes.swerveAtPosition() == true && shooter.isUpToSpeed() == true) { + if(shooterPivot.atPosition() == true && eyes.swerveAtPosition(onMove) == true && shooter.isUpToSpeed() == true) { eyes.controllerRumble = true; } else { eyes.controllerRumble = false; diff --git a/src/main/java/frc/robot/commands/PrepareShot.java b/src/main/java/frc/robot/commands/PrepareShot.java index 68ac102..49be0a9 100644 --- a/src/main/java/frc/robot/commands/PrepareShot.java +++ b/src/main/java/frc/robot/commands/PrepareShot.java @@ -20,15 +20,17 @@ public class PrepareShot extends Command { // local variables boolean shotReady = false; + boolean onMove = false; // constructor - public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) { + public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes, boolean onMove) { this.s_ShooterPivot = s_ShooterPivot; this.s_Shooter = s_Shooter; this.s_Eyes = s_Eyes; + this.onMove = onMove; addRequirements(s_ShooterPivot, s_Shooter, s_Eyes); @@ -37,7 +39,7 @@ public PrepareShot(ShooterPivot s_ShooterPivot, Shooter s_Shooter, Eyes s_Eyes) @Override public void execute() { - if (s_ShooterPivot.atPosition() == true && s_Eyes.swerveAtPosition() == true && s_Shooter.isUpToSpeed() == true) { + if (s_ShooterPivot.atPosition() == true && s_Eyes.swerveAtPosition(onMove) == true && s_Shooter.isUpToSpeed() == true) { shotReady = true; } else { shotReady = false; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 270542a..ed7e89a 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -217,11 +217,13 @@ public double getMovingTargetRotation() { } - public boolean swerveAtPosition() { - - - - double error = Math.abs(getTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + public boolean swerveAtPosition(boolean onMove) { + double error; + if(onMove){ + error = Math.abs(getMovingTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + } else { + error = Math.abs(getTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + } SmartDashboard.putNumber("getTargetRotation", getTargetRotation()); SmartDashboard.putNumber("estimated rotation", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); @@ -234,6 +236,8 @@ public boolean swerveAtPosition() { } } + + public double getShotTime(double distance) { double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1.333) * 0.0254; //TODO: change shooter gear ratio @@ -267,7 +271,6 @@ public Pose2d getMovingTarget() { Translation2d movingGoalLocation = new Translation2d(); - //TODO MAKE FIELD RELATIVE? double robotVelX = s_Swerve.fieldRelativeVelocity.vx; double robotVelY = s_Swerve.fieldRelativeVelocity.vy; From 32417918c9ed10b9c2b27229d73e6384675b2f03 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 28 Mar 2024 20:02:27 -0500 Subject: [PATCH 23/82] Intake rollers run after intake down --- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++------ .../java/frc/robot/commands/AimShoot.java | 1 + .../java/frc/robot/commands/RunIntake.java | 3 +-- src/main/java/frc/robot/subsystems/Eyes.java | 19 +++++++++----- .../java/frc/robot/subsystems/Intake.java | 25 +++++++++++++++++++ .../frc/robot/subsystems/ShooterPivot.java | 1 + .../java/frc/robot/subsystems/Swerve.java | 1 + 7 files changed, 56 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8626d00..31ff7e0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -327,13 +327,20 @@ private void configureButtonBindings() { // intake driverX.whileTrue( - new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) - .andThen(new ParallelCommandGroup( - new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) - ))) - .onFalse(new ParallelCommandGroup( + new SequentialCommandGroup( + new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes), + new ConditionalCommand( + new ParallelCommandGroup( + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage))), + new ParallelCommandGroup( + new InstantCommand(() -> s_Shooter.setLoaderVoltage(0)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(0))), + () -> s_Intake.atPosition())).andThen( + new ParallelCommandGroup( + new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1))) + )).onFalse(new ParallelCommandGroup( new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) ) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 3130db3..c000e96 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -311,6 +311,7 @@ public void execute() { } else { eyes.controllerRumble = false; } + } diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index 8efa7eb..c9c10e2 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -41,9 +41,8 @@ public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter public void execute() { s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition); - s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage); s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition); - s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); + } diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index ed7e89a..1055ad4 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -220,14 +220,15 @@ public double getMovingTargetRotation() { public boolean swerveAtPosition(boolean onMove) { double error; if(onMove){ - error = Math.abs(getMovingTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + error = Math.abs(getMovingTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360) % 360; } else { error = Math.abs(getTargetRotation() + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); } - SmartDashboard.putNumber("getTargetRotation", getTargetRotation()); + SmartDashboard.putNumber("getMovingTargetRotation", getMovingTargetRotation()); SmartDashboard.putNumber("estimated rotation", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); SmartDashboard.putNumber("rotationError", error); + //SmartDashboard.putBoolean("swerveAtPosition", error <= Constants.Swerve.atPositionTolerance); if (error <= Constants.Swerve.atPositionTolerance) { return true; @@ -283,13 +284,13 @@ public Pose2d getMovingTarget() { double virtualGoalX = getTargetPose().getX() + shotTime * (robotVelX + robotAccelX * accelerationCompensation); //TODO: Test on blue double virtualGoalY = getTargetPose().getY() + shotTime * (robotVelY + robotAccelY * accelerationCompensation); //TODO: Test on blue - SmartDashboard.putNumber("Goal X", virtualGoalX); - SmartDashboard.putNumber("Goal Y", virtualGoalY); Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); + translationPublisher.set(testGoalLocation); Translation2d toTestGoal = testGoalLocation.minus(s_Swerve.getEstimatedPose().getTranslation()); - translationPublisher.set(toTestGoal); + + double newShotTime = getShotTime(toTestGoal.getDistance(new Translation2d())); @@ -415,7 +416,13 @@ public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); - updatePoseEstimatorWithVisionBotPose(); + //updatePoseEstimatorWithVisionBotPose(); + if (LimelightHelpers.getTV("") == true) { + s_Swerve.m_poseEstimator.addVisionMeasurement( + getRobotPose(), + Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0) + ); + } SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0886038..9991cb6 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -14,7 +14,10 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; public class Intake extends SubsystemBase { @@ -136,6 +139,28 @@ public void setIntakeVoltage(double intakeSpeedVoltage) { public double cancoderInDegrees() { return e_intakePivot.getPosition().getValue() * 360; } + + double getTargetIntakePosition() { + return intakeGroundPosition; + } + + + public boolean atPosition() { + + double error = Math.abs(e_intakePivot.getPosition().getValueAsDouble() - getTargetIntakePosition()); + + if (Constants.ELEVATOR_TOLERANCE >= error) { + return true; + + } else { + return false; + } + + } + + public Command IntakeAtPosition(){ + return Commands.waitUntil(() -> atPosition()); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index 4d71c79..e884bbf 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -117,6 +117,7 @@ public boolean atPosition() { double error = Math.abs(cancoderInDegrees() - m_setPoint); + //SmartDashboard.putBoolean("at pivot position", pivotTolerance >= error); if (pivotTolerance >= error) { return true; diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 085e1e0..993128f 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -307,6 +307,7 @@ public void periodic(){ fieldRelativeAccel = new FieldRelativeAccel(fieldRelativeVelocity, lastFieldRelativeVelocity, time-lastTime); //time was 0.02 for standard loop time attempting to calculate it to increase accuracy. May crash with /0 error if startup is too quick lastFieldRelativeVelocity = fieldRelativeVelocity; lastTime = time; + SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); From af1d75c9eea75c11fa0fc7f6fa7e8da09ca444fb Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 28 Mar 2024 21:52:12 -0500 Subject: [PATCH 24/82] Restore intake command Previous intake command would not start the intake rollers. --- src/main/java/frc/robot/RobotContainer.java | 27 +++++++------------ .../java/frc/robot/commands/RunIntake.java | 2 ++ 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 31ff7e0..c40df8b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -327,24 +327,15 @@ private void configureButtonBindings() { // intake driverX.whileTrue( - new SequentialCommandGroup( - new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes), - new ConditionalCommand( - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), - new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage))), - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setLoaderVoltage(0)), - new InstantCommand(() -> s_Intake.setIntakeVoltage(0))), - () -> s_Intake.atPosition())).andThen( - new ParallelCommandGroup( - new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1))) - )).onFalse(new ParallelCommandGroup( - new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) - ) - ); + new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + .until(() -> !s_Shooter.getBreakBeamOutput()) + .andThen(new ParallelCommandGroup( + new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) + ))) + .onFalse(new ParallelCommandGroup( new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) + )); // outake driverA.whileTrue(new SequentialCommandGroup( diff --git a/src/main/java/frc/robot/commands/RunIntake.java b/src/main/java/frc/robot/commands/RunIntake.java index c9c10e2..7a02c65 100644 --- a/src/main/java/frc/robot/commands/RunIntake.java +++ b/src/main/java/frc/robot/commands/RunIntake.java @@ -41,7 +41,9 @@ public RunIntake(Intake s_Intake, ShooterPivot s_ShooterPivot, Shooter s_Shooter public void execute() { s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition); + s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage); s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition); + s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); } From 22d3516e447f4a626c581c7e4251fc0971311af9 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Fri, 29 Mar 2024 11:22:20 -0500 Subject: [PATCH 25/82] New tele intake command --- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++++++++------- 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c40df8b..13e0982 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,9 +125,9 @@ public class RobotContainer { private final Shooter s_Shooter = new Shooter(); private final Eyes s_Eyes = new Eyes(s_Swerve, s_Shooter); - - + /* Commands */ private final SendableChooser autoChooser; + @@ -135,6 +135,8 @@ public class RobotContainer { public RobotContainer() { + //Default Commands + //Swerve Drive if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { s_Swerve.setDefaultCommand( new TeleopSwerve( @@ -180,7 +182,8 @@ public RobotContainer() { // Configure the button bindings configureButtonBindings(); - //Command ElevatorAtPosition = new s_Elevator.ElevatorAtPosition(); + + //Auto Commands Command AimThenShootAuto = new ParallelRaceGroup( new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false), @@ -326,16 +329,26 @@ private void configureButtonBindings() { // intake - driverX.whileTrue( - new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) + driverX.whileTrue(new SequentialCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeGroundPosition)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) + ), + s_Intake.IntakeAtPosition().withTimeout(0.5), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage))) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeSafePosition)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.stopIntakeVoltage)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage)), new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink("")), new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) ))) - .onFalse(new ParallelCommandGroup( new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) - )); + .onFalse(new ParallelCommandGroup( + new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)))); // outake driverA.whileTrue(new SequentialCommandGroup( @@ -404,7 +417,6 @@ private void configureButtonBindings() { // aim amp operatorLeftTrigger.whileTrue( - new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), s_Elevator.ElevatorAtPosition(), From 2d2f1985644908cc19a32c290e48258f19c5f145 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 30 Mar 2024 09:36:37 -0500 Subject: [PATCH 26/82] Intake Tele Command Correction --- src/main/java/frc/robot/RobotContainer.java | 8 ++++++-- src/main/java/frc/robot/subsystems/Intake.java | 4 ++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 13e0982..1d72ed2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -335,8 +335,8 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition)), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) ), - s_Intake.IntakeAtPosition().withTimeout(0.5), - new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage))) + s_Intake.IntakeAtPosition().withTimeout(0.25), //TODO Make at position work + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage)).repeatedly()) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeSafePosition)), @@ -347,6 +347,10 @@ private void configureButtonBindings() { new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) ))) .onFalse(new ParallelCommandGroup( + new InstantCommand(() -> s_Intake.setIntakePivotPosition(s_Intake.intakeSafePosition)), + new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.stopIntakeVoltage)), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage)), new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)))); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 9991cb6..276be33 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -31,7 +31,7 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. public final double intakeSafePosition = 28.0; - public final double intakeGroundPosition = -74.0; + public final double intakeGroundPosition = -70.0; public final double intakeSourcePosition = intakeSafePosition; // PID values @@ -149,7 +149,7 @@ public boolean atPosition() { double error = Math.abs(e_intakePivot.getPosition().getValueAsDouble() - getTargetIntakePosition()); - if (Constants.ELEVATOR_TOLERANCE >= error) { + if (5.0 >= error) { return true; } else { From 6aa07c76bad330b19938273a78581d4684e512b9 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 30 Mar 2024 10:36:34 -0500 Subject: [PATCH 27/82] Adjusted shots --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/commands/AimShoot.java | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1d72ed2..ae367d2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -175,7 +175,7 @@ public RobotContainer() { s_Shooter.setDefaultCommand( new ConditionalCommand( new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(40, 40), s_Shooter), + new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(20, 20), s_Shooter), () -> s_Shooter.getBreakBeamOutput()) ); diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index c000e96..35af49f 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -65,22 +65,22 @@ public class AimShoot extends Command { //Higher note shot is lower angle!!! private final double subWooferDistance = 1.25; private final double subWooferAngle = 115.0; - private final double subWooferLeftShooterSpeed = 90.0; + private final double subWooferLeftShooterSpeed = 50.0; private final double subWooferRightShooterSpeed = subWooferLeftShooterSpeed; private final double d2Distance = 2.15; private final double d2Angle = 128.0; - private final double d2LeftShooterSpeed = 90.0; + private final double d2LeftShooterSpeed = 50.0; private final double d2RightShooterSpeed = d2LeftShooterSpeed; private final double podiumDistance = 3.17; private final double podiumAngle = 137; - private final double podiumLeftShooterSpeed = 90.0; + private final double podiumLeftShooterSpeed = 60.0; private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; private final double d3Distance = 4.0; - private final double d3Angle = 137.0; - private final double d3LeftShooterSpeed = 95.0; + private final double d3Angle = 138.0; + private final double d3LeftShooterSpeed = 70.0; private final double d3RightShooterSpeed = d3LeftShooterSpeed; From 65d082813644717f2b18311a7f44b83c90e561a5 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 30 Mar 2024 12:09:18 -0500 Subject: [PATCH 28/82] Shot tuning, blue alliance works now, for shoot on move --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/AimShoot.java | 125 +++++++----------- src/main/java/frc/robot/subsystems/Eyes.java | 13 +- 4 files changed, 60 insertions(+), 82 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 850e8fa..0e43804 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,7 +33,7 @@ public final class Constants { /* slow mode */ public static final double SLOW_MODE_PERCENT_TRANSLATION = 0.5; public static final double SLOW_MODE_PERCENT_STRAFE = 0.5; - public static final double SLOW_MODE_PERCENT_ROTATION = 0.5; + public static final double SLOW_MODE_PERCENT_ROTATION = 1.0; public static final double AUTO_ROTATE_DEADBAND = 1.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17ccd21..a82d0a6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -186,7 +186,7 @@ public RobotContainer() { //Auto Commands Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false).alongWith(new TeleopSwerve( + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true).alongWith(new TeleopSwerve( s_Swerve, () -> 0, () -> 0, diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index f7902a5..fa3edc3 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -20,6 +20,7 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.ShooterPivot; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Timer; @@ -41,13 +42,12 @@ public class AimShoot extends Command { // required WPILib class objects private InterpolatingDoubleTreeMap shooterAngleInterpolation; - private InterpolatingDoubleTreeMap shooterAngleInterpolationAuto; private InterpolatingDoubleTreeMap shooterAngleInterpolationElevator; + private InterpolatingDoubleTreeMap shooterSpeedInterpolation; // local variables private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; + private double shooterSpeed; public boolean isElevatorShot = false; public boolean onMove = false; @@ -57,60 +57,31 @@ public class AimShoot extends Command { //Higher note shot is lower angle!!! private final double subWooferDistance = 1.21; //1.21 at 930, 1.25 at comp private final double subWooferAngle = 115.0; - private final double subWooferLeftShooterSpeed = 50.0; - private final double subWooferRightShooterSpeed = subWooferLeftShooterSpeed; - - private final double d2Distance = 2.15; - private final double d2Angle = 128.0; - private final double d2LeftShooterSpeed = 50.0; - private final double d2RightShooterSpeed = d2LeftShooterSpeed; + private final double subWooferSpeed = 50.0; + + private final double xSpotDistance = 2.4; + private final double xSpotAngle = 131.0; + private final double xSpotSpeed = 50.0; private final double podiumDistance = 3.17; private final double podiumAngle = 137; - private final double podiumLeftShooterSpeed = 60.0; - private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; + private final double podiumSpeed = 60.0; private final double d3Distance = 4.0; - private final double d3Angle = 138.0; - private final double d3LeftShooterSpeed = 70.0; - private final double d3RightShooterSpeed = d3LeftShooterSpeed; - - - private final double subWooferDistanceAuto = 1.25; - private final double subWooferAngleAuto = 115.0; - private final double subWooferLeftShooterSpeedAuto = 90.0; - private final double subWooferRightShooterSpeedAuto = subWooferLeftShooterSpeed; - - private final double d2DistanceAuto = 2.15; - private final double d2AngleAuto = 128.0; - private final double d2LeftShooterSpeedAuto = 90.0; - private final double d2RightShooterSpeedAuto = d2LeftShooterSpeed; - - private final double xSpotDistance = 2.4; - private final double xSpotAngle = 131.0; - private final double xSpotLeftShooterSpeed = 92.5; - private final double xSpotRightShooterSpeed = xSpotLeftShooterSpeed; + private final double d3Angle = 140.0; + private final double d3Speed = 70.0; - private final double podiumDistanceAuto = 3.17; - private final double podiumAngleAuto = 138; - private final double podiumLeftShooterSpeedAuto = 90.0; - private final double podiumRightShooterSpeedAuto = podiumLeftShooterSpeed; + private final double wingerDistance = 5.44; + private final double wingerAngle = 145; + private final double wingerSpeed = 85.0; + - private final double d3DistanceAuto = 5.27; - private final double d3AngleAuto = 136.0; - private final double d3LeftShooterSpeedAuto = 95.0; - private final double d3RightShooterSpeedAuto = d3LeftShooterSpeed; - - private final double xSpotDistanceAuto = 2.4; - private final double xSpotAngleAuto = 130.0; - private final double xSpotLeftShooterSpeedAuto = 90.0; - private final double xSpotRightShooterSpeedAuto = xSpotLeftShooterSpeed; private final double elevatorShotDistance = 2.65; - private final double elevatorShotAngle = 144; - private final double elevatorShotLeftShooterSpeed = 90; - private final double elevatorShotRightShooterSpeed = elevatorShotLeftShooterSpeed; + private final double elevatorShotAngle = 146; + private final double elevatorShotShooterSpeed = 60; + // constructor public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot, boolean onMove) { @@ -124,26 +95,28 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); + shooterSpeedInterpolation = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); - //shooterAngleInterpolation.put(d2Distance, d2Angle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); + shooterAngleInterpolation.put(d3Distance, d3Angle); + shooterAngleInterpolation.put(wingerDistance, wingerAngle); - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); - shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); - shooterAngleInterpolationAuto.put(podiumDistanceAuto, podiumAngleAuto); - shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); - shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); - shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); + // create points in shooter linear interpolation line + // TODO tune these values + shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); + shooterSpeedInterpolation.put(d3Distance, d3Speed); + shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); + shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); + shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); + } public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double manualDistance) { @@ -156,22 +129,24 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma // instantiate objects shooterAngleInterpolation = new InterpolatingDoubleTreeMap(); - shooterAngleInterpolationAuto = new InterpolatingDoubleTreeMap(); shooterAngleInterpolationElevator = new InterpolatingDoubleTreeMap(); + shooterSpeedInterpolation = new InterpolatingDoubleTreeMap(); // create points in angle linear interpolation line // TODO tune these values shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); - shooterAngleInterpolation.put(d2Distance, d2Angle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); + shooterAngleInterpolation.put(d3Distance, d3Angle); + shooterAngleInterpolation.put(wingerDistance, wingerAngle); - shooterAngleInterpolationAuto.put(subWooferDistanceAuto, subWooferAngleAuto); - shooterAngleInterpolationAuto.put(d2DistanceAuto, d2AngleAuto); - shooterAngleInterpolationAuto.put(podiumDistanceAuto, podiumAngleAuto); - shooterAngleInterpolationAuto.put(d3DistanceAuto, d3AngleAuto); - shooterAngleInterpolationAuto.put(xSpotDistanceAuto, xSpotAngleAuto); + // create points in angle linear interpolation line + // TODO tune these values + shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); + shooterSpeedInterpolation.put(d3Distance, d3Speed); + shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); + shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); + shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); } @@ -198,25 +173,21 @@ public void execute() { shooterAngle = shooterAngleInterpolation.get(distance); } - // get desired shooter angle using the linear interpolation - // x (input) = distance - // y (output) = shooter angle - - + // move shooter to calculated angle + shooterPivot.moveShooterPivot(shooterAngle); + // get desired shooter power using the linear interpolation // x (input) = distance // y (output) = shooter power - leftShooterSpeed = shooter.shotSpeedRPS; - rightShooterSpeed = shooter.shotSpeedRPS * shooter.shooterSpinReduction; + if(isElevatorShot) { + shooterSpeed = elevatorShotShooterSpeed; + } else { + shooterSpeed = shooterSpeedInterpolation.get(distance); + } - - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(shooterAngle); - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(rightShooterSpeed, leftShooterSpeed); + shooter.shootingMotorsSetControl(shooterSpeed, shooterSpeed); if(shooterPivot.atPosition() == true && eyes.swerveAtPosition(onMove) == true && shooter.isUpToSpeed() == true) { eyes.controllerRumble = true; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 1055ad4..6ff1eeb 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -241,7 +241,7 @@ public boolean swerveAtPosition(boolean onMove) { public double getShotTime(double distance) { - double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1.333) * 0.0254; //TODO: change shooter gear ratio + double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time } @@ -281,9 +281,16 @@ public Pose2d getMovingTarget() { for(int i=0;i<5;i++){ - double virtualGoalX = getTargetPose().getX() + shotTime * (robotVelX + robotAccelX * accelerationCompensation); //TODO: Test on blue - double virtualGoalY = getTargetPose().getY() + shotTime * (robotVelY + robotAccelY * accelerationCompensation); //TODO: Test on blue + double virtualGoalX; + double virtualGoalY; + if(DriverStation.getAlliance().get() == Alliance.Blue) { + virtualGoalX = getTargetPose().getX() - shotTime * (robotVelX + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() - shotTime * (robotVelY + robotAccelY * accelerationCompensation); + } else { + virtualGoalX = getTargetPose().getX() + shotTime * (robotVelX + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() + shotTime * (robotVelY + robotAccelY * accelerationCompensation); + } Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); translationPublisher.set(testGoalLocation); From 541d1e85e31e8f97ddef3de245946c7097b3fd86 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 30 Mar 2024 16:01:22 -0500 Subject: [PATCH 29/82] Feeding, current limiting and new climb angle --- src/main/java/frc/robot/RobotContainer.java | 61 +++++++++++++------ .../java/frc/robot/commands/AimShoot.java | 16 ++++- src/main/java/frc/robot/subsystems/Eyes.java | 58 +++++++++++++++++- .../java/frc/robot/subsystems/Intake.java | 2 +- .../java/frc/robot/subsystems/Shooter.java | 19 +++++- .../frc/robot/subsystems/ShooterPivot.java | 2 +- 6 files changed, 130 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a82d0a6..05e04ce 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -186,7 +186,7 @@ public RobotContainer() { //Auto Commands Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true).alongWith(new TeleopSwerve( + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false).alongWith(new TeleopSwerve( s_Swerve, () -> 0, () -> 0, @@ -213,7 +213,7 @@ public RobotContainer() { .withTimeout(.5) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); - NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false)); + NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, false)); NamedCommands.registerCommand("Fire", new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage))); NamedCommands.registerCommand("Check Note", new InstantCommand(() -> s_Shooter.checkNote())); NamedCommands.registerCommand("Got Note", new ConditionalCommand( @@ -265,7 +265,7 @@ private void configureButtonBindings() { () -> true, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false)) ); } else { @@ -279,7 +279,7 @@ private void configureButtonBindings() { () -> true, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false ,true, false)) ); } @@ -299,7 +299,7 @@ private void configureButtonBindings() { () -> false,//driverB.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false)), + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( @@ -321,7 +321,7 @@ private void configureButtonBindings() { () -> false,//driverB.getAsBoolean(), rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false)), + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) ).onFalse( @@ -418,7 +418,7 @@ private void configureButtonBindings() { driverRB.onTrue( new ParallelCommandGroup( new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(2)), new InstantCommand(() -> SmartDashboard.putString("ClimbCommand", "up")) ) @@ -444,8 +444,40 @@ private void configureButtonBindings() { /* Operator Buttons */ - // aim amp - + + // aim speaker + if (DriverStation.getAlliance().get() == Alliance.Blue) { + operatorRightTrigger.whileTrue(new TeleopSwerve( + s_Swerve, + () -> -driver.getRawAxis(leftY), + () -> -driver.getRawAxis(leftX), + () -> 0, + () -> driverDpadUp.getAsBoolean(), + () -> s_Eyes.getFeedRotation(), + () -> false, + rotationSpeed, + true + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) + + ); + } else { + operatorRightTrigger.whileTrue(new TeleopSwerve( + s_Swerve, + () -> driver.getRawAxis(leftY), + () -> driver.getRawAxis(leftX), + () -> 0, + () -> driverDpadUp.getAsBoolean(), + () -> s_Eyes.getFeedRotation(), + () -> false, + rotationSpeed, + true + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4)) + + ); + } + + + //reach amp operatorLeftTrigger.whileTrue( new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), @@ -464,17 +496,6 @@ private void configureButtonBindings() { ); - operatorRightTrigger.onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setLoaderVoltage(6)), - new InstantCommand(() -> s_Shooter.setShooterVoltage(6, -6)) - ) - ).onFalse( - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setLoaderVoltage(0)), - new InstantCommand(() -> s_Shooter.setShooterVoltage(0, 0)) - ) - ); // dummy shoot commands operatorDpadDown.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25))); diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index fa3edc3..a2e8d8e 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -51,6 +51,7 @@ public class AimShoot extends Command { public boolean isElevatorShot = false; public boolean onMove = false; + public boolean feed = false; // positions @@ -74,9 +75,12 @@ public class AimShoot extends Command { private final double wingerDistance = 5.44; private final double wingerAngle = 145; private final double wingerSpeed = 85.0; - + + private final double feedDistance = 2.4; + private final double feedAngle = 131.0; + private final double feedSpeed = 60.0; private final double elevatorShotDistance = 2.65; private final double elevatorShotAngle = 146; @@ -84,12 +88,13 @@ public class AimShoot extends Command { // constructor - public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot, boolean onMove) { + public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean isElevatorShot, boolean onMove, boolean feed) { this.eyes = eyes; this.shooterPivot = shooterPivot; this.shooter = shooter; this.isElevatorShot = isElevatorShot; this.onMove = onMove; + this.feed = feed; addRequirements(eyes, shooterPivot, shooter); @@ -156,9 +161,12 @@ public void execute() { // assign target distance as variable - if (isElevatorShot == true) { + if (isElevatorShot) { distance = elevatorShotDistance; shooterAngle = elevatorShotAngle; + } else if(feed) { + distance = feedDistance; + shooterAngle = feedAngle; } else if(manualDistance == 0) { if (onMove == true) { distance = eyes.getDistanceFromMovingTarget(); @@ -182,6 +190,8 @@ public void execute() { // y (output) = shooter power if(isElevatorShot) { shooterSpeed = elevatorShotShooterSpeed; + } else if (feed){ + shooterSpeed = feedSpeed; } else { shooterSpeed = shooterSpeedInterpolation.get(distance); } diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 6ff1eeb..26247ca 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -166,6 +166,37 @@ public Pose3d getTargetPose() { } + /* + * This method will return the known pose of the desired target. + * + * parameters: + * none + * + * returns: + * target pose (Pose2d) + */ + public Pose3d getFeedPose() { + + Pose3d pose; + + // if robot is on blue alliance + if(DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue) { + + // get pose of blue speaker + pose = new Pose3d(Constants.Positions.ampBlueX, Constants.Positions.ampBlueY, 0, new Rotation3d(0,0,Constants.Positions.ampBlueR)); + + // if robot is on red alliance + } else { + + // get pose of red speaker + pose = new Pose3d(Constants.Positions.ampRedX, Constants.Positions.ampRedY, 0, new Rotation3d(0,0,Constants.Positions.ampRedR)); + + } + + return pose; + + } + public double getTargetRotation() { Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); @@ -191,6 +222,31 @@ public double getTargetRotation() { return -angle + 180; } + public double getFeedRotation() { + + Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); + Pose3d targetPose = getFeedPose(); + + double robotX = robotPose.getX(); + double robotY = robotPose.getY(); + + double targetX = targetPose.getX(); + double targetY = targetPose.getY(); + + double angle = (Math.atan((targetY - robotY) / (targetX - robotX)) * (180 / Math.PI)); + + if (robotX > targetX) { + + angle = angle + 180; + + } + + SmartDashboard.putNumber("angle", angle); + SmartDashboard.putNumber(" inverted angle", -angle); + + return -angle + 180; + } + public double getMovingTargetRotation() { Pose2d robotPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); @@ -242,7 +298,7 @@ public boolean swerveAtPosition(boolean onMove) { public double getShotTime(double distance) { double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio - return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time + return (distance / linearSpeed) + 0.2; //TODO: tune constant for shot accel/feeding time } public double getDistanceFromTarget() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 276be33..59db86b 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -44,7 +44,7 @@ public class Intake extends SubsystemBase { private final double magnetOffSet = 0.0; // Kermit Limits - private final int intakeCurrentLimit = 120; + private final int intakeCurrentLimit = 60; private final int intakePivotCurrentLimit = 60; // local variables diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 50204f5..f576a24 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -26,11 +26,13 @@ public class Shooter extends SubsystemBase { private final int rightShooterMotorID = 14; private final int loaderMotorID = 16; private final int breakBeamID = 0; + // loader speeds public final double runLoaderVoltage = 12.0; public final double reverseLoaderVoltage = -3.0; public final double stopLoaderVoltage = 0.0; + private final int loaderCurrentLimit = 40; // shooter speeds public final double runShooterVoltage = 6.0; @@ -40,6 +42,7 @@ public class Shooter extends SubsystemBase { public final double shotSpeedRPS = 90; public final double shooterSpinReduction = 1.0; + private final int currentLimit = 80; // left shooter motor PID @@ -53,7 +56,9 @@ public class Shooter extends SubsystemBase { private final double rShooterMotorPGains = 0.05; private final double rShooterMotorIGains = 0.0; private final double rShooterMotorDGains = 0.0; - private final int m_CurrentLimit = 40; + + + private final double rShooterMotorSGains = 0.0; private final double rShooterMotorVGains = 0.12; @@ -91,7 +96,7 @@ public Shooter() { configF.apply( new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(m_CurrentLimit) + .withSupplyCurrentLimit(loaderCurrentLimit) ); // right shooter motor configuration @@ -184,9 +189,19 @@ public void shootingMotorsConfig() { slotConfigsL.kP = SmartDashboard.getNumber("LShooter kP", 0); slotConfigsL.kV = SmartDashboard.getNumber("LShooter kV", 0); + + // set configurations to motors m_rightShooter.getConfigurator().apply(slotConfigsR); m_leftShooter.getConfigurator().apply(slotConfigsL); + + m_rightShooter.getConfigurator().apply(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(currentLimit)); + + m_leftShooter.getConfigurator().apply(new CurrentLimitsConfigs() + .withSupplyCurrentLimitEnable(true) + .withSupplyCurrentLimit(currentLimit)); } /* diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index e884bbf..f152038 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -30,7 +30,7 @@ public class ShooterPivot extends SubsystemBase { public final double shooterPivotStowPosition = 115.0; public final double shooterPivotIntakePosition = 136.75; public final double shooterPivotAmpPosition = 200; - public final double shooterPivotClimbPosition = 364.30; + public final double shooterPivotClimbPosition = 309; // pivot motor PID private final double shooterPivotPGains = 0.3; //0.5 From 33ab4193d2066946eeb1009c108fff84090ab16e Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sat, 30 Mar 2024 21:43:24 -0500 Subject: [PATCH 30/82] Command cleaning, pivot stows --- src/main/java/frc/robot/RobotContainer.java | 40 +++++----- .../java/frc/robot/commands/AmpElevator.java | 43 ----------- .../robot/commands/AmpElevatorRetract.java | 43 ----------- .../frc/robot/commands/AmpShooterPivot.java | 44 ----------- .../commands/AmpShooterPivotRetract.java | 42 ---------- .../java/frc/robot/commands/ShootPodium.java | 74 ------------------ .../frc/robot/commands/ShootSubwoofer.java | 76 ------------------- .../java/frc/robot/commands/ShootXSpot.java | 76 ------------------- .../java/frc/robot/subsystems/Elevator.java | 17 +++++ 9 files changed, 38 insertions(+), 417 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AmpElevator.java delete mode 100644 src/main/java/frc/robot/commands/AmpElevatorRetract.java delete mode 100644 src/main/java/frc/robot/commands/AmpShooterPivot.java delete mode 100644 src/main/java/frc/robot/commands/AmpShooterPivotRetract.java delete mode 100644 src/main/java/frc/robot/commands/ShootPodium.java delete mode 100644 src/main/java/frc/robot/commands/ShootSubwoofer.java delete mode 100644 src/main/java/frc/robot/commands/ShootXSpot.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 05e04ce..07fe49e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -175,7 +175,7 @@ public RobotContainer() { s_Shooter.setDefaultCommand( new ConditionalCommand( new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(20, 20), s_Shooter), + new InstantCommand(() -> s_Shooter.setShooterVoltage(5, 5), s_Shooter), //s_Shooter.shootingMotorsSetControl(20, 20) () -> s_Shooter.getBreakBeamOutput()) ); @@ -206,10 +206,10 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) + .until(() -> !s_Shooter.getBreakBeamOutput()) //Make rollers spin after at position/0.25s ); NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) + .until(() -> !s_Shooter.getBreakBeamOutput()) //Make rollers spin after at position/0.25s .withTimeout(.5) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); @@ -226,6 +226,8 @@ public RobotContainer() { new WaitCommand (15), () -> s_Shooter.gotNote )); + + //TODO Shoot on the Move in Auto? autoChooser = AutoBuilder.buildAutoChooser(); @@ -361,7 +363,7 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition)), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) ), - s_Intake.IntakeAtPosition().withTimeout(0.25), //TODO Make at position work + s_Intake.IntakeAtPosition().repeatedly().withTimeout(0.25), //TODO Test if this works now instead of just being time based new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage)).repeatedly()) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( @@ -445,7 +447,7 @@ private void configureButtonBindings() { /* Operator Buttons */ - // aim speaker + //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { operatorRightTrigger.whileTrue(new TeleopSwerve( s_Swerve, @@ -458,8 +460,7 @@ private void configureButtonBindings() { rotationSpeed, true ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) - - ); + ).onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); } else { operatorRightTrigger.whileTrue(new TeleopSwerve( s_Swerve, @@ -471,7 +472,8 @@ private void configureButtonBindings() { () -> false, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4)) + ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4))) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)) ); } @@ -480,27 +482,27 @@ private void configureButtonBindings() { //reach amp operatorLeftTrigger.whileTrue( new SequentialCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), - s_Elevator.ElevatorAtPosition(), - new ParallelCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), - new AmpShooterPivot(s_ShooterPivot) - ) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) ) ).onFalse( new SequentialCommandGroup( - new AmpShooterPivotRetract(s_ShooterPivot), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), s_ShooterPivot.ShooterPivotAtPosition(), - new AmpElevatorRetract(s_Elevator) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)) ) ); // dummy shoot commands - operatorDpadDown.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25))); - operatorDpadLeft.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4))); - operatorDpadUp.whileTrue((new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 3.17))); + operatorDpadDown.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + operatorDpadLeft.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + operatorDpadUp.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 3.17)) + .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); } diff --git a/src/main/java/frc/robot/commands/AmpElevator.java b/src/main/java/frc/robot/commands/AmpElevator.java deleted file mode 100644 index 259ec8f..0000000 --- a/src/main/java/frc/robot/commands/AmpElevator.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.subsystems.Elevator; - -public class AmpElevator extends Command { - - Elevator s_Elevator; - - /** Creates a new AmpScore. */ - public AmpElevator(Elevator s_Elevator) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_Elevator = s_Elevator; - - addRequirements(s_Elevator); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/AmpElevatorRetract.java b/src/main/java/frc/robot/commands/AmpElevatorRetract.java deleted file mode 100644 index a0e3ae2..0000000 --- a/src/main/java/frc/robot/commands/AmpElevatorRetract.java +++ /dev/null @@ -1,43 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Elevator; - -public class AmpElevatorRetract extends Command { - - Elevator s_Elevator; - - - /** Creates a new AmpScore. */ - public AmpElevatorRetract(Elevator s_Elevator) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_Elevator = s_Elevator; - - addRequirements(s_Elevator); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_Elevator.SetElevatorPosition(0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/AmpShooterPivot.java b/src/main/java/frc/robot/commands/AmpShooterPivot.java deleted file mode 100644 index 6e39536..0000000 --- a/src/main/java/frc/robot/commands/AmpShooterPivot.java +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterPivot; - -public class AmpShooterPivot extends Command { - - ShooterPivot s_ShooterPivot; - - /** Creates a new AmpScore. */ - public AmpShooterPivot(ShooterPivot s_ShooterPivot) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_ShooterPivot = s_ShooterPivot; - - addRequirements(s_ShooterPivot); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - System.out.println("AmpShooterPivot"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java b/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java deleted file mode 100644 index 0b59373..0000000 --- a/src/main/java/frc/robot/commands/AmpShooterPivotRetract.java +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.ShooterPivot; - -public class AmpShooterPivotRetract extends Command { - - ShooterPivot s_ShooterPivot; - - /** Creates a new AmpScore. */ - public AmpShooterPivotRetract(ShooterPivot s_ShooterPivot) { - // Use addRequirements() here to declare subsystem dependencies. - this.s_ShooterPivot = s_ShooterPivot; - - addRequirements(s_ShooterPivot); - - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return true; - } -} diff --git a/src/main/java/frc/robot/commands/ShootPodium.java b/src/main/java/frc/robot/commands/ShootPodium.java deleted file mode 100644 index 1a07bbb..0000000 --- a/src/main/java/frc/robot/commands/ShootPodium.java +++ /dev/null @@ -1,74 +0,0 @@ - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootPodium extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - // positions - - //Higher note shot is lower angle!!! - - private final double podiumAngle = 130; - private final double podiumLeftShooterSpeed = 90.0; - private final double podiumRightShooterSpeed = podiumLeftShooterSpeed; - - - // constructor - public ShootPodium(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(podiumAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(podiumRightShooterSpeed, podiumLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootSubwoofer.java b/src/main/java/frc/robot/commands/ShootSubwoofer.java deleted file mode 100644 index bfc6220..0000000 --- a/src/main/java/frc/robot/commands/ShootSubwoofer.java +++ /dev/null @@ -1,76 +0,0 @@ - - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootSubwoofer extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - - // positions - - //Higher note shot is lower angle!!! - - private final double subwooferAngle = 115; - private final double subwooferLeftShooterSpeed = 90.0; - private final double subwooferRightShooterSpeed = subwooferLeftShooterSpeed; - - - // constructor - public ShootSubwoofer(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(subwooferAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(subwooferRightShooterSpeed, subwooferLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootXSpot.java b/src/main/java/frc/robot/commands/ShootXSpot.java deleted file mode 100644 index f72f28d..0000000 --- a/src/main/java/frc/robot/commands/ShootXSpot.java +++ /dev/null @@ -1,76 +0,0 @@ - - -package frc.robot.commands; - -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Elevator; -import frc.robot.subsystems.Eyes; -import frc.robot.subsystems.Shooter; -import frc.robot.subsystems.ShooterPivot; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - - -public class ShootXSpot extends Command { - - // required subystems - private ShooterPivot shooterPivot; - private Shooter shooter; - private Elevator elevator; - - private double distance; - - // required WPILib class objects - - // local variables - private double shooterAngle; - private double leftShooterSpeed; - private double rightShooterSpeed; - - - // positions - - //Higher note shot is lower angle!!! - - private final double subwooferAngle = 115; - private final double subwooferLeftShooterSpeed = 90.0; - private final double subwooferRightShooterSpeed = subwooferLeftShooterSpeed; - - - // constructor - public ShootXSpot(ShooterPivot shooterPivot, Shooter shooter) { - this.shooterPivot = shooterPivot; - this.shooter = shooter; - - addRequirements(shooterPivot, shooter); - - // instantiate objects - - } - - @Override - public void execute() { - - // move shooter to calculated angle - shooterPivot.moveShooterPivot(subwooferAngle); - - // set shooter speed power to calculated value - shooter.shootingMotorsSetControl(subwooferRightShooterSpeed, subwooferLeftShooterSpeed); - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - shooter.setLoaderVoltage(shooter.stopLoaderVoltage); - shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 762bc61..f35cfa9 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -187,6 +187,19 @@ public boolean atPosition() { } + public boolean atPosition(double setPosition) { + + double error = Math.abs(e_Elevator.getPosition() - setPosition); + + if (Constants.ELEVATOR_TOLERANCE >= error) { + return true; + + } else { + return false; + } + + } + /* The below method is public, which means that it can be * accessed outside of this class. It also takes in a * single double value as a parameter. It then takes that @@ -257,6 +270,10 @@ public void SetElevatorPosition (double inches){ public Command ElevatorAtPosition(){ return Commands.waitUntil(() -> atPosition()); } + + public Command ElevatorAtPosition(double setPosition){ + return Commands.waitUntil(() -> atPosition(setPosition)); + } } From bd6669e4be88dcbe359065bbf3d2d729ca544730 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sat, 30 Mar 2024 22:53:56 -0500 Subject: [PATCH 31/82] remove duplicate spin up. New control scheme (untested) --- src/main/java/frc/robot/RobotContainer.java | 76 ++++++++----------- .../java/frc/robot/subsystems/Elevator.java | 6 ++ 2 files changed, 39 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07fe49e..ccf3ed9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; @@ -206,10 +207,10 @@ public RobotContainer() { NamedCommands.registerCommand("Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) //Make rollers spin after at position/0.25s + .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s ); NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) - .until(() -> !s_Shooter.getBreakBeamOutput()) //Make rollers spin after at position/0.25s + .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s .withTimeout(.5) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); @@ -334,15 +335,6 @@ private void configureButtonBindings() { ); } - //spin up shooter when we have a note in the indexer - //NOTE: IF REMOVED NEED TO ADD SHOOTER SPINUP FOR AMP SCORE - s_Shooter.setDefaultCommand( - new ConditionalCommand( - new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(40, 40), s_Shooter), - () -> s_Shooter.getBreakBeamOutput()) - ); - // shoot speaker driverRightTrigger.onTrue( new ParallelCommandGroup( @@ -363,7 +355,7 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotIntakePosition)), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) ), - s_Intake.IntakeAtPosition().repeatedly().withTimeout(0.25), //TODO Test if this works now instead of just being time based + s_Intake.IntakeAtPosition().withTimeout(0.25), //TODO figure out if this actually is working or just time delay new InstantCommand(() -> s_Intake.setIntakeVoltage(s_Intake.runIntakeVoltage)).repeatedly()) .until(() -> !s_Shooter.getBreakBeamOutput()) .andThen(new ParallelCommandGroup( @@ -400,33 +392,35 @@ private void configureButtonBindings() { + //Climbing + driverRB.onTrue(new ConditionalCommand( + //climb pull up + new ParallelCommandGroup( + new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(2.0)), + new InstantCommand(() -> SmartDashboard.putString("ClimbCommand", "up")), + new InstantCommand(() -> s_Elevator.isClimbed(true))), - // climb reach - driverLB.onTrue( + //Reach for Climb new SequentialCommandGroup( new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_SAFE_LEVEL)), - s_Elevator.ElevatorAtPosition(), - + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), new ParallelCommandGroup( new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) - ) - ) - ); - - - // climb pull up - driverRB.onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(2)), - new InstantCommand(() -> SmartDashboard.putString("ClimbCommand", "up")) - - ) - ); - - // escape climb + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + new InstantCommand(() -> s_Elevator.isClimbed(false)) + )), + () -> { + if(s_Elevator.isClimbed){ + return false; + } else{ + return true; + } + })); + + + // abort climb driverSelect.onTrue( new SequentialCommandGroup( @@ -443,13 +437,9 @@ private void configureButtonBindings() { ); - - /* Operator Buttons */ - - //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { - operatorRightTrigger.whileTrue(new TeleopSwerve( + driverRStick.whileTrue(new TeleopSwerve( s_Swerve, () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), @@ -462,7 +452,7 @@ private void configureButtonBindings() { ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) ).onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); } else { - operatorRightTrigger.whileTrue(new TeleopSwerve( + driverRStick.whileTrue(new TeleopSwerve( s_Swerve, () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), @@ -478,9 +468,8 @@ private void configureButtonBindings() { ); } - //reach amp - operatorLeftTrigger.whileTrue( + driverLB.whileTrue( new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), @@ -494,7 +483,8 @@ private void configureButtonBindings() { ) ); - + + /* Operator Buttons */ // dummy shoot commands operatorDpadDown.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25)) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index f35cfa9..4875546 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -70,6 +70,7 @@ public class Elevator extends SubsystemBase { private double elevatorD = 0.0; private double voltage = 0.0; + public boolean isClimbed = true; /* Constructor @@ -224,6 +225,11 @@ private void goToShootingPos(){ } } +public void isClimbed(boolean climbState) { + isClimbed = climbState; +} + + From c529079265a4fb807469953cc8582ceaafe03c1e Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sun, 31 Mar 2024 09:01:56 -0500 Subject: [PATCH 32/82] add climb state to abort climb --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ccf3ed9..994e09a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -431,7 +431,8 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), new InstantCommand(() -> SmartDashboard.putString("ShooterPivot", "return")), s_ShooterPivot.ShooterPivotAtPosition(), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), + new InstantCommand(() -> s_Elevator.isClimbed(true)) ) ); From 9f31e6a2d42372fa8d3c48338462944aab5a9281 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:10:57 -0500 Subject: [PATCH 33/82] safety for amp/climb --- src/main/java/frc/robot/RobotContainer.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 994e09a..3c5a4e6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -407,7 +407,7 @@ private void configureButtonBindings() { new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), new ParallelCommandGroup( - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), + //new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), //TODO add this back once confirm safe level works for amp new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), new InstantCommand(() -> s_Elevator.isClimbed(false)) )), @@ -474,10 +474,12 @@ private void configureButtonBindings() { new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) + new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink(""))//TODO: this is to test that the at position command is actually working before it breaks something--remove this and uncomment the pivot below if this works + //new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) ) ).onFalse( new SequentialCommandGroup( + new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), //remove this if the test above works new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), s_ShooterPivot.ShooterPivotAtPosition(), new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)) From 9e00c7dd988554b0f4878728a619c33ffda10177 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:16:21 -0500 Subject: [PATCH 34/82] remove driver rotation during autoaim --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3c5a4e6..e69adb7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -262,7 +262,7 @@ private void configureButtonBindings() { s_Swerve, () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), - () -> driver.getRawAxis(rightX), + () -> 0, //driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), () -> true, @@ -276,7 +276,7 @@ private void configureButtonBindings() { s_Swerve, () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), - () -> driver.getRawAxis(rightX), + () -> 0,//driver.getRawAxis(rightX), () -> driverDpadUp.getAsBoolean(), () -> s_Eyes.getMovingTargetRotation(), () -> true, From f0c09a596ebe0f2125de8fb247c3088bd5055705 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sun, 31 Mar 2024 23:08:08 -0500 Subject: [PATCH 35/82] added trap path generation --- .../paths/Position Locator (DO NOT USE).path | 52 ++++++ .../java/frc/robot/commands/GoToTrap.java | 176 ++++++++++++++++++ src/main/java/frc/robot/subsystems/Eyes.java | 16 ++ 3 files changed, 244 insertions(+) create mode 100644 src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path create mode 100644 src/main/java/frc/robot/commands/GoToTrap.java diff --git a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path new file mode 100644 index 0000000..5f7fe3f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 10.4, + "y": 4.08519490024762 + }, + "prevControl": null, + "nextControl": { + "x": 11.400000000000006, + "y": 4.08519490024762 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.655631889462832, + "y": 7.0 + }, + "prevControl": { + "x": 5.655631889462832, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/GoToTrap.java b/src/main/java/frc/robot/commands/GoToTrap.java new file mode 100644 index 0000000..7dd98af --- /dev/null +++ b/src/main/java/frc/robot/commands/GoToTrap.java @@ -0,0 +1,176 @@ +package frc.robot.commands; + +import java.util.List; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + +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.wpilibj2.command.Command; +import frc.robot.subsystems.Swerve; +import frc.robot.subsystems.Eyes; + +public class GoToTrap extends Command { + + Eyes s_Eyes; + Swerve s_Swerve; + + public double robotX = 0.0; + public double robotY = 0.0; + public double robotR = 0.0; + + public double targetX = 0.0; + public double targetY = 0.0; + public double targetR = 0.0; + + public double closestDistance = 0.0; + public double positionTolerance = 0.1; + public double rotationTolerance = 2.0; + public double distanceLimit = 3.0; + + // blue trap positions + public double blueTrapLeftX = 4.3; + public double blueTrapLeftY = 5.0; + public double blueTrapLeftR = -60.0; + public double blueTrapLeftDistance = 0.0; // Leave as 0.0 + + public double blueTrapRightX = 4.3; + public double blueTrapRightY = 3.0; + public double blueTrapRightR = 60.0; + public double blueTrapRightDistance = 0.0; // Leave as 0.0 + + public double blueTrapCenterX = 6.1; + public double blueTrapCenterY = 4.1; + public double blueTrapCenterR = 180.0; + public double blueTrapCenterDistance = 0.0; // Leave as 0.0 + + // red trap positions + public double redTrapLeftX = 12.3; + public double redTrapLeftY = 3.0; + public double redTrapLeftR = 121.64; + public double redTrapLeftDistance = 0.0; // Leave as 0.0 + + public double redTrapRightX = 12.3; + public double redTrapRightY = 5.0; + public double redTrapRightR = -121.64; + public double redTrapRightDistance = 0.0; // Leave as 0.0 + + public double redTrapCenterX = 10.4; + public double redTrapCenterY = 4.1; + public double redTrapCenterR = 0.0; + public double redTrapCenterDistance = 0.0; // Leave as 0.0 + + public GoToTrap(Eyes s_Eyes, Swerve s_Swerve) { + + this.s_Eyes = s_Eyes; + this.s_Swerve = s_Swerve; + + addRequirements(s_Eyes, s_Swerve); + } + + @Override + public void initialize() { + + robotX = s_Eyes.getRobotPose().getX(); + robotY = s_Eyes.getRobotPose().getY(); + robotR = s_Eyes.getRobotPose().getRotation().getDegrees(); + + blueTrapLeftDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapLeftX, blueTrapLeftY); + blueTrapRightDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapRightX, blueTrapRightY); + blueTrapCenterDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapCenterX, blueTrapCenterY); + + redTrapLeftDistance = s_Eyes.getDistanceFromTargetTrap(redTrapLeftX, redTrapRightY); + redTrapRightDistance = s_Eyes.getDistanceFromTargetTrap(redTrapRightX, redTrapRightY); + redTrapCenterDistance = s_Eyes.getDistanceFromTargetTrap(redTrapCenterX, redTrapCenterY); + + double[] distances = { + blueTrapLeftDistance, + blueTrapRightDistance, + blueTrapCenterDistance, + redTrapLeftDistance, + redTrapRightDistance, + redTrapCenterDistance + }; + + closestDistance = blueTrapLeftDistance; + + for(int i = 0; i < 6; i++) { + if (closestDistance < distances[i]) { + closestDistance = distances[i]; + } + } + + if (closestDistance == blueTrapLeftDistance) { + targetX = blueTrapLeftX; + targetY = blueTrapLeftY; + targetR = blueTrapLeftR; + } else if (closestDistance == blueTrapRightDistance) { + targetX = blueTrapRightX; + targetY = blueTrapRightY; + targetR = blueTrapRightR; + } else if (closestDistance == blueTrapCenterDistance) { + targetX = blueTrapCenterX; + targetY = blueTrapCenterY; + targetR = blueTrapCenterR; + } else if (closestDistance == redTrapLeftDistance) { + targetX = redTrapLeftX; + targetY = redTrapLeftY; + targetR = redTrapLeftR; + } else if (closestDistance == redTrapRightDistance) { + targetX = redTrapRightX; + targetY = redTrapRightY; + targetR = redTrapRightR; + } else if (closestDistance == redTrapCenterDistance) { + targetX = redTrapCenterX; + targetY = redTrapCenterY; + targetR = redTrapCenterR; + } + + List bezierPoints = PathPlannerPath.bezierFromPoses( + + new Pose2d(s_Eyes.getRobotPose().getX(), s_Eyes.getRobotPose().getY(), Rotation2d.fromDegrees(0)), + new Pose2d(targetX, targetY, Rotation2d.fromDegrees(0)) + + ); + + PathPlannerPath path = new PathPlannerPath( + bezierPoints, + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), + new GoalEndState(0.0, Rotation2d.fromDegrees(targetR)) + ); + + if (closestDistance < distanceLimit) { + AutoBuilder.followPath(path); + } + + } + + @Override + public void execute() { + + } + + @Override + public void end(boolean interrupted) { + + } + + @Override + public boolean isFinished() { + + double xError = Math.abs(targetX + s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); + double yError = Math.abs(targetX + s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); + double rError = Math.abs(targetR + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + + if (xError <= positionTolerance && yError <= positionTolerance && rError <= rotationTolerance) { + return true; + } else { + return false; + } + + } +} diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 26247ca..0bc52f3 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -432,6 +432,22 @@ public double getDistanceFromTargetAuto() { } + public double getDistanceFromTargetTrap(double trapX, double trapY) { + + double distance = 0.0; + + if(DriverStation.getAlliance().get() == Alliance.Blue) { + + double xDistanceToSpeaker = trapX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); + double yDistanceToSpeaker = trapY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); + distance = Math.sqrt(Math.pow(xDistanceToSpeaker, 2) + Math.pow(yDistanceToSpeaker, 2)); + + } + + return distance; + + } + public void updatePoseEstimatorWithVisionBotPose() { //invalid limelight if (LimelightHelpers.getTV("") == false) { From 25cd423aa48e8c03cb64cd74ddb298d8d6194fb3 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sun, 31 Mar 2024 23:11:06 -0500 Subject: [PATCH 36/82] Added trap path button binding --- src/main/java/frc/robot/RobotContainer.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e69adb7..4cc85a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -436,7 +436,9 @@ private void configureButtonBindings() { ) ); - + + // generate and run path to closest trap + driverStart.whileTrue(new GoToTrap(s_Eyes, s_Swerve)); //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { From 69a43bfcc05fce1445ed67c3250e58bf55bfc580 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Mon, 1 Apr 2024 01:24:51 -0500 Subject: [PATCH 37/82] Target Logging, Alliance protection, minor error changes --- .../java/frc/robot/commands/GoToTrap.java | 85 ++++++++++++------- 1 file changed, 54 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/commands/GoToTrap.java b/src/main/java/frc/robot/commands/GoToTrap.java index 7dd98af..f52adbf 100644 --- a/src/main/java/frc/robot/commands/GoToTrap.java +++ b/src/main/java/frc/robot/commands/GoToTrap.java @@ -10,6 +10,11 @@ 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.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Swerve; import frc.robot.subsystems.Eyes; @@ -19,6 +24,8 @@ public class GoToTrap extends Command { Eyes s_Eyes; Swerve s_Swerve; + private StructPublisher translationPublisher; + public double robotX = 0.0; public double robotY = 0.0; public double robotR = 0.0; @@ -27,10 +34,10 @@ public class GoToTrap extends Command { public double targetY = 0.0; public double targetR = 0.0; - public double closestDistance = 0.0; public double positionTolerance = 0.1; public double rotationTolerance = 2.0; public double distanceLimit = 3.0; + public double closestDistance = distanceLimit; // blue trap positions public double blueTrapLeftX = 4.3; @@ -75,6 +82,8 @@ public GoToTrap(Eyes s_Eyes, Swerve s_Swerve) { @Override public void initialize() { + translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); + robotX = s_Eyes.getRobotPose().getX(); robotY = s_Eyes.getRobotPose().getY(); robotR = s_Eyes.getRobotPose().getRotation().getDegrees(); @@ -96,40 +105,47 @@ public void initialize() { redTrapCenterDistance }; - closestDistance = blueTrapLeftDistance; - - for(int i = 0; i < 6; i++) { + for(int i = 0; i < distances.length; i++) { if (closestDistance < distances[i]) { closestDistance = distances[i]; } } - - if (closestDistance == blueTrapLeftDistance) { - targetX = blueTrapLeftX; - targetY = blueTrapLeftY; - targetR = blueTrapLeftR; - } else if (closestDistance == blueTrapRightDistance) { - targetX = blueTrapRightX; - targetY = blueTrapRightY; - targetR = blueTrapRightR; - } else if (closestDistance == blueTrapCenterDistance) { - targetX = blueTrapCenterX; - targetY = blueTrapCenterY; - targetR = blueTrapCenterR; - } else if (closestDistance == redTrapLeftDistance) { - targetX = redTrapLeftX; - targetY = redTrapLeftY; - targetR = redTrapLeftR; - } else if (closestDistance == redTrapRightDistance) { - targetX = redTrapRightX; - targetY = redTrapRightY; - targetR = redTrapRightR; - } else if (closestDistance == redTrapCenterDistance) { - targetX = redTrapCenterX; - targetY = redTrapCenterY; - targetR = redTrapCenterR; + + if (DriverStation.getAlliance().get() == Alliance.Blue) { + if (closestDistance == blueTrapLeftDistance) { + targetX = blueTrapLeftX; + targetY = blueTrapLeftY; + targetR = blueTrapLeftR; + } else if (closestDistance == blueTrapRightDistance) { + targetX = blueTrapRightX; + targetY = blueTrapRightY; + targetR = blueTrapRightR; + } else if (closestDistance == blueTrapCenterDistance) { + targetX = blueTrapCenterX; + targetY = blueTrapCenterY; + targetR = blueTrapCenterR; + } + } else if (DriverStation.getAlliance().get() == Alliance.Red) { + if (closestDistance == redTrapLeftDistance) { + targetX = redTrapLeftX; + targetY = redTrapLeftY; + targetR = redTrapLeftR; + } else if (closestDistance == redTrapRightDistance) { + targetX = redTrapRightX; + targetY = redTrapRightY; + targetR = redTrapRightR; + } else if (closestDistance == redTrapCenterDistance) { + targetX = redTrapCenterX; + targetY = redTrapCenterY; + targetR = redTrapCenterR; + } + } else { + closestDistance = distanceLimit; } + //Log Target Trap Location + translationPublisher.set(new Translation2d(targetX, targetY)); + List bezierPoints = PathPlannerPath.bezierFromPoses( new Pose2d(s_Eyes.getRobotPose().getX(), s_Eyes.getRobotPose().getY(), Rotation2d.fromDegrees(0)), @@ -137,14 +153,21 @@ public void initialize() { ); + PathPlannerPath path = new PathPlannerPath( bezierPoints, new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), new GoalEndState(0.0, Rotation2d.fromDegrees(targetR)) ); - if (closestDistance < distanceLimit) { + // Prevent the path from being flipped as all coordinates are blue origin + path.preventFlipping = true; + + if (closestDistance < distanceLimit) { //DO NOT INCLUDE DISTANCE LIMIT AutoBuilder.followPath(path); + SmartDashboard.putString("Trap Status", "Going to Trap"); + } else{ + SmartDashboard.putString("Trap Status", "Too Far Away To Trap"); } } @@ -163,7 +186,7 @@ public void end(boolean interrupted) { public boolean isFinished() { double xError = Math.abs(targetX + s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); - double yError = Math.abs(targetX + s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); + double yError = Math.abs(targetY + s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); double rError = Math.abs(targetR + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); if (xError <= positionTolerance && yError <= positionTolerance && rError <= rotationTolerance) { From 833e2bf3cc6d3e81f6fd119106cbe36e44d1574a Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Mon, 1 Apr 2024 01:31:38 -0500 Subject: [PATCH 38/82] logging and further error correction --- src/main/java/frc/robot/commands/GoToTrap.java | 10 +++++++--- src/main/java/frc/robot/subsystems/Eyes.java | 12 +++--------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/commands/GoToTrap.java b/src/main/java/frc/robot/commands/GoToTrap.java index f52adbf..bc36325 100644 --- a/src/main/java/frc/robot/commands/GoToTrap.java +++ b/src/main/java/frc/robot/commands/GoToTrap.java @@ -185,9 +185,13 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - double xError = Math.abs(targetX + s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); - double yError = Math.abs(targetY + s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); - double rError = Math.abs(targetR + s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + double xError = Math.abs(targetX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); + double yError = Math.abs(targetY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); + double rError = Math.abs(targetR - s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); + + SmartDashboard.putBoolean("Trap X Error", xError <= positionTolerance); + SmartDashboard.putBoolean("Trap Y Error", yError <= positionTolerance); + SmartDashboard.putBoolean("Trap Rotation Error", rError <= rotationTolerance); if (xError <= positionTolerance && yError <= positionTolerance && rError <= rotationTolerance) { return true; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 0bc52f3..9995f23 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -434,15 +434,9 @@ public double getDistanceFromTargetAuto() { public double getDistanceFromTargetTrap(double trapX, double trapY) { - double distance = 0.0; - - if(DriverStation.getAlliance().get() == Alliance.Blue) { - - double xDistanceToSpeaker = trapX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); - double yDistanceToSpeaker = trapY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); - distance = Math.sqrt(Math.pow(xDistanceToSpeaker, 2) + Math.pow(yDistanceToSpeaker, 2)); - - } + double xDistance = trapX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX(); + double yDistance = trapY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY(); + double distance = Math.sqrt(Math.pow(xDistance, 2) + Math.pow(yDistance, 2)); return distance; From 21e10f604db516a1df9d4c207ee9060e8b50c506 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Mon, 1 Apr 2024 08:58:53 -0500 Subject: [PATCH 39/82] Cancel Trap Command on Release, Remove robot oriented and autorotate on raised elevator shot --- src/main/java/frc/robot/RobotContainer.java | 26 ++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4cc85a8..065538b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -145,7 +145,7 @@ public RobotContainer() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Swerve.getGyroYaw().getDegrees(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, @@ -161,7 +161,7 @@ public RobotContainer() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Swerve.getGyroYaw().getDegrees(), () -> driverLeftTrigger.getAsBoolean(), rotationSpeed, @@ -192,7 +192,7 @@ public RobotContainer() { () -> 0, () -> 0, () -> 0, - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getTargetRotation(), () -> false, rotationSpeed, @@ -263,7 +263,7 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> 0, //driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getMovingTargetRotation(), () -> true, rotationSpeed, @@ -277,7 +277,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> 0,//driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getMovingTargetRotation(), () -> true, rotationSpeed, @@ -297,11 +297,11 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getTargetRotation(), () -> false,//driverB.getAsBoolean(), rotationSpeed, - true + false //TODO Determine if we want auto rotation aiming ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) @@ -319,11 +319,11 @@ private void configureButtonBindings() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> driver.getRawAxis(rightX), - () -> driverDpadUp.getAsBoolean(), - () -> s_Eyes.getTargetRotation(), + () -> false, + () -> s_Eyes.getTargetRotation(), () -> false,//driverB.getAsBoolean(), rotationSpeed, - true + false //TODO: Determine if we want auto rotation aiming ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, true, false, false)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)) ) @@ -438,7 +438,7 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new GoToTrap(s_Eyes, s_Swerve)); + driverStart.whileTrue(new GoToTrap(s_Eyes, s_Swerve)).onFalse(s_Swerve.getDefaultCommand()); //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { @@ -447,7 +447,7 @@ private void configureButtonBindings() { () -> -driver.getRawAxis(leftY), () -> -driver.getRawAxis(leftX), () -> 0, - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getFeedRotation(), () -> false, rotationSpeed, @@ -460,7 +460,7 @@ private void configureButtonBindings() { () -> driver.getRawAxis(leftY), () -> driver.getRawAxis(leftX), () -> 0, - () -> driverDpadUp.getAsBoolean(), + () -> false, () -> s_Eyes.getFeedRotation(), () -> false, rotationSpeed, From 71ca851d3280a9223cf84daa4fc59ef9c4aa5f87 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 2 Apr 2024 18:51:50 -0500 Subject: [PATCH 40/82] GoToTrap doesn't move --- src/main/java/frc/robot/RobotContainer.java | 8 +++----- src/main/java/frc/robot/commands/GoToTrap.java | 18 +++++++++++------- 2 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 065538b..642b164 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -407,7 +407,7 @@ private void configureButtonBindings() { new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), new ParallelCommandGroup( - //new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), //TODO add this back once confirm safe level works for amp + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotClimbPosition)), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), new InstantCommand(() -> s_Elevator.isClimbed(false)) )), @@ -419,7 +419,6 @@ private void configureButtonBindings() { } })); - // abort climb driverSelect.onTrue( @@ -476,12 +475,11 @@ private void configureButtonBindings() { new SequentialCommandGroup( new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), - new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceBlink(""))//TODO: this is to test that the at position command is actually working before it breaks something--remove this and uncomment the pivot below if this works - //new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) ) ).onFalse( new SequentialCommandGroup( - new InstantCommand(() -> s_Eyes.limelight.setLEDMode_ForceOff("")), //remove this if the test above works + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), s_ShooterPivot.ShooterPivotAtPosition(), new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)) diff --git a/src/main/java/frc/robot/commands/GoToTrap.java b/src/main/java/frc/robot/commands/GoToTrap.java index bc36325..34f08e2 100644 --- a/src/main/java/frc/robot/commands/GoToTrap.java +++ b/src/main/java/frc/robot/commands/GoToTrap.java @@ -36,7 +36,7 @@ public class GoToTrap extends Command { public double positionTolerance = 0.1; public double rotationTolerance = 2.0; - public double distanceLimit = 3.0; + public double distanceLimit = 6.0; public double closestDistance = distanceLimit; // blue trap positions @@ -71,6 +71,8 @@ public class GoToTrap extends Command { public double redTrapCenterR = 0.0; public double redTrapCenterDistance = 0.0; // Leave as 0.0 + public PathPlannerPath path; + public GoToTrap(Eyes s_Eyes, Swerve s_Swerve) { this.s_Eyes = s_Eyes; @@ -106,7 +108,7 @@ public void initialize() { }; for(int i = 0; i < distances.length; i++) { - if (closestDistance < distances[i]) { + if (closestDistance > distances[i]) { closestDistance = distances[i]; } } @@ -156,13 +158,19 @@ public void initialize() { PathPlannerPath path = new PathPlannerPath( bezierPoints, - new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), + new PathConstraints(0.25, 0.25, 2 * Math.PI, 4 * Math.PI), new GoalEndState(0.0, Rotation2d.fromDegrees(targetR)) ); // Prevent the path from being flipped as all coordinates are blue origin path.preventFlipping = true; + + } + + @Override + public void execute() { + if (closestDistance < distanceLimit) { //DO NOT INCLUDE DISTANCE LIMIT AutoBuilder.followPath(path); SmartDashboard.putString("Trap Status", "Going to Trap"); @@ -171,11 +179,7 @@ public void initialize() { } } - - @Override - public void execute() { - } @Override public void end(boolean interrupted) { From de40a969b7d5f73106e5ab979bdb8afe6afa1e94 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Tue, 2 Apr 2024 20:57:58 -0500 Subject: [PATCH 41/82] 930 tuning --- .../paths/Sourceside Auto Path 1.path | 2 +- .../paths/Sourceside Auto Path 2.path | 2 +- .../paths/Sourceside Auto Path 3.path | 18 +++++++++--------- .../paths/Sourceside Auto Path 4.path | 2 +- .../paths/Sourceside Auto Path 5.path | 16 ++++++++-------- .../paths/Sourceside missed Note 2.path | 2 +- src/main/java/frc/robot/RobotContainer.java | 6 +++--- src/main/java/frc/robot/commands/AimShoot.java | 12 ++++++------ src/main/java/frc/robot/subsystems/Eyes.java | 2 +- 9 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path index fb2c0cb..512917c 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path @@ -57,7 +57,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 5.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path index 9495150..11f06bf 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 5.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path index 2c845e5..ddce14f 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 4.9658667218569015, - "y": 0.7909763967676083 + "x": 5.640944520323205, + "y": -0.3213603580232063 }, "isLocked": false, "linkedName": "Loadside Auto Path 2 Endpoint" @@ -20,8 +20,8 @@ "y": 2.4150717815077387 }, "prevControl": { - "x": 6.93456182044388, - "y": 1.9440666129695687 + "x": 7.345128237112941, + "y": 1.1491067347496495 }, "nextControl": null, "isLocked": false, @@ -31,15 +31,15 @@ "rotationTargets": [ { "waypointRelativePos": 0.7, - "rotationDegrees": 23.19029156666067, - "rotateFast": false + "rotationDegrees": 30.707075654360423, + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.6, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { @@ -57,13 +57,13 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 5.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 26.075355583948827, + "rotation": 34.62415507994909, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path index 296cdfb..a0e2150 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path @@ -33,7 +33,7 @@ "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 6.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path index f1be36d..ec2ac32 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.4446939700083, - "y": 0.19979096765891036 + "x": 6.040210419685371, + "y": -0.3116221653558371 }, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" }, { "anchor": { - "x": 8.153398228504642, - "y": 4.060826342293255 + "x": 8.279994733180452, + "y": 4.099779112962734 }, "prevControl": { - "x": 6.21346150912503, - "y": 1.0627423214338552 + "x": 6.390785355710689, + "y": 1.5581108267791854 }, "nextControl": null, "isLocked": false, @@ -51,13 +51,13 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 5.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 25.942295489871594, + "rotation": 42.089162173832335, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path index 726dd69..bc5fad5 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.35, + "waypointRelativePos": 0.1, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 642b164..7add85f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -199,9 +199,9 @@ public RobotContainer() { true )), new SequentialCommandGroup( - new WaitCommand(1.0).until(() -> prepareShot()), + new WaitCommand(2.0).until(() -> prepareShot()), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), - new WaitCommand(1.0)).until(() -> s_Shooter.getBreakBeamOutput()) + new WaitCommand(1.5)).until(() -> s_Shooter.getBreakBeamOutput()) ); @@ -211,7 +211,7 @@ public RobotContainer() { ); NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s - .withTimeout(.5) + .withTimeout(1.0) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, false)); diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index a2e8d8e..387526a 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -57,23 +57,23 @@ public class AimShoot extends Command { //Higher note shot is lower angle!!! private final double subWooferDistance = 1.21; //1.21 at 930, 1.25 at comp - private final double subWooferAngle = 115.0; - private final double subWooferSpeed = 50.0; + private final double subWooferAngle = 115.0; //115 + private final double subWooferSpeed = 50.0; //50 private final double xSpotDistance = 2.4; - private final double xSpotAngle = 131.0; + private final double xSpotAngle = 133.0; private final double xSpotSpeed = 50.0; private final double podiumDistance = 3.17; - private final double podiumAngle = 137; + private final double podiumAngle = 139; private final double podiumSpeed = 60.0; private final double d3Distance = 4.0; - private final double d3Angle = 140.0; + private final double d3Angle = 142.5; private final double d3Speed = 70.0; private final double wingerDistance = 5.44; - private final double wingerAngle = 145; + private final double wingerAngle = 147; private final double wingerSpeed = 85.0; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 9995f23..a5652dd 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -298,7 +298,7 @@ public boolean swerveAtPosition(boolean onMove) { public double getShotTime(double distance) { double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio - return (distance / linearSpeed) + 0.2; //TODO: tune constant for shot accel/feeding time + return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time } public double getDistanceFromTarget() { From 017ffe4a1b1eeb79f01b0bfe191059e3c96dcf3e Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Wed, 3 Apr 2024 21:59:02 -0500 Subject: [PATCH 42/82] Added Smart Ampside Auto --- .../pathplanner/autos/Ampside Score.auto | 2 +- .../autos/Copy of Ampside Score.auto | 2 +- .../autos/Prototype Ampside Smart Auto.auto | 355 ++++++++++++++++++ .../paths/Ampside Auto Path 6.path | 2 +- .../paths/Ampside Auto Path 7.path | 70 ++++ .../paths/Ampside Auto Path 8.path | 52 +++ .../paths/Ampside Auto path 1.path | 8 +- ...h path 4.path => Ampside Auto path 4.path} | 0 .../paths/Ampside Missed Note 1.path | 70 ++++ .../paths/Ampside Missed Note 2.path | 70 ++++ .../paths/Ampside Missed Note 3.path | 70 ++++ .../{New Path.path => Ampside Smart 1.path} | 20 +- 12 files changed, 704 insertions(+), 17 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path create mode 100644 src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path rename src/main/deploy/pathplanner/paths/{Ampside Auth path 4.path => Ampside Auto path 4.path} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path create mode 100644 src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path create mode 100644 src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path rename src/main/deploy/pathplanner/paths/{New Path.path => Ampside Smart 1.path} (69%) diff --git a/src/main/deploy/pathplanner/autos/Ampside Score.auto b/src/main/deploy/pathplanner/autos/Ampside Score.auto index 1ffc82b..c08f8d3 100644 --- a/src/main/deploy/pathplanner/autos/Ampside Score.auto +++ b/src/main/deploy/pathplanner/autos/Ampside Score.auto @@ -56,7 +56,7 @@ { "type": "path", "data": { - "pathName": "Ampside Auth path 4" + "pathName": "Ampside Auto path 4" } }, { diff --git a/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto b/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto index 8430520..29b82c0 100644 --- a/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Ampside Score.auto @@ -80,7 +80,7 @@ { "type": "path", "data": { - "pathName": "Ampside Auth path 4" + "pathName": "Ampside Auto path 4" } }, { diff --git a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto new file mode 100644 index 0000000..df4c451 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto @@ -0,0 +1,355 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7987428243044774, + "y": 6.68 + }, + "rotation": 60.20283245949364 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 2" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 6" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 7" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Ampside Missed Note 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Not Got Note" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Ampside Auto Path 8" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + } + ] + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path index 6185060..86a82be 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "Ampside Auto Path 6 Endpoint" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path new file mode 100644 index 0000000..f476573 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.9863466887427603, + "y": 6.610786554906443 + }, + "prevControl": null, + "nextControl": { + "x": 2.9863466887427603, + "y": 6.610786554906443 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 6 Endpoint" + }, + { + "anchor": { + "x": 8.157095812938493, + "y": 4.156084903648504 + }, + "prevControl": { + "x": 6.696293693339267, + "y": 7.468608019922806 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -39.80557109226524, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path new file mode 100644 index 0000000..309dbad --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.157095812938493, + "y": 4.156084903648504 + }, + "prevControl": null, + "nextControl": { + "x": 5.770433195004229, + "y": 2.9113169003249295 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + }, + { + "anchor": { + "x": 2.47848475647076, + "y": 6.419299455137156 + }, + "prevControl": { + "x": 3.2191731551407905, + "y": 5.750622428560045 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 8 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.89055165624832, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": -31.429565614838474, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path index 71489f7..6b245e5 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.086934844340443, - "y": 6.785397528912033 + "x": 1.1822800587957067, + "y": 6.820505671086531 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.03 }, "prevControl": { - "x": 1.1581313038590775, - "y": 6.7137636416806785 + "x": 1.624635630223642, + "y": 6.789643654475279 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auth path 4.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Ampside Auth path 4.path rename to src/main/deploy/pathplanner/paths/Ampside Auto path 4.path diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path new file mode 100644 index 0000000..b51bb0a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.54, + "y": 7.03 + }, + "prevControl": null, + "nextControl": { + "x": 3.54, + "y": 7.03 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 1 Endpoint" + }, + { + "anchor": { + "x": 8.149590620168896, + "y": 7.325500083098932 + }, + "prevControl": { + "x": 7.149590620168896, + "y": 7.325500083098932 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto path 3 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path new file mode 100644 index 0000000..9e04b94 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.149590620168896, + "y": 7.325500083098932 + }, + "prevControl": null, + "nextControl": { + "x": 7.303246686693876, + "y": 7.355447292348219 + }, + "isLocked": false, + "linkedName": "Ampside Auto path 3 Endpoint" + }, + { + "anchor": { + "x": 8.149590620168896, + "y": 5.76090976743357 + }, + "prevControl": { + "x": 7.344396042175544, + "y": 6.151828644509419 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.4, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -26.70661318450872, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path new file mode 100644 index 0000000..bba57dc --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.211827384508862, + "y": 5.7844864444177295 + }, + "prevControl": null, + "nextControl": { + "x": 7.087212570415116, + "y": 5.658036378729399 + }, + "isLocked": false, + "linkedName": "Ampside Auto Path 5 Endpoint" + }, + { + "anchor": { + "x": 8.157095812938493, + "y": 4.156084903648504 + }, + "prevControl": { + "x": 7.107787248155949, + "y": 4.516141764113103 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Ampside Auto Path 7 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -21.54097591853849, + "rotateFast": false + }, + "reversed": false, + "folder": "Amp Side", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path similarity index 69% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/Ampside Smart 1.path index fc9a85e..65a456b 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 0.7223726295842452, + "y": 6.691938312786578 }, "prevControl": null, "nextControl": { - "x": 2.018336848061597, - "y": 7.069927876510557 + "x": 1.9284777906097335, + "y": 7.470070674738507 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.726063461610459, + "y": 6.857291439701363 }, "prevControl": { - "x": 1.93, - "y": 7.0 + "x": 1.909024481560935, + "y": 6.3125987863350135 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 30.173520029644234, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0, + "rotation": 58.29857033049431, "velocity": 0 }, "useDefaultConstraints": true From 529f766d535f9e5a588302695167bd5155e8488c Mon Sep 17 00:00:00 2001 From: leo Date: Thu, 4 Apr 2024 18:26:49 -0500 Subject: [PATCH 43/82] debug views added --- src/main/java/frc/robot/subsystems/Elevator.java | 4 ++++ src/main/java/frc/robot/subsystems/Intake.java | 5 +++++ src/main/java/frc/robot/subsystems/Shooter.java | 7 +++++++ src/main/java/frc/robot/subsystems/ShooterPivot.java | 3 +++ src/main/java/frc/robot/subsystems/Swerve.java | 12 ++++++++++++ 5 files changed, 31 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 762bc61..3113441 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -240,6 +240,10 @@ public void periodic() { SmartDashboard.putNumber("Elevator Encoder Value: ", getPosition()); SmartDashboard.putNumber("Current Elevator Position",e_Elevator.getPosition()); SmartDashboard.putNumber("Elevator Voltage", voltage); + SmartDashboard.putNumber("debug/ELEVATOR TARGET POSITION", targetElevatorPosition); + SmartDashboard.putNumber("debug/Elevator Encoder Value: ", getPosition()); + SmartDashboard.putNumber("debug/Current Elevator Position",e_Elevator.getPosition()); + SmartDashboard.putNumber("debug/Elevator Voltage", voltage); // logData(); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 41ef523..ca7b6ca 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -150,5 +150,10 @@ public void periodic() { SmartDashboard.putNumber("Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); SmartDashboard.putNumber("Intake setpoint", m_setPoint); SmartDashboard.putNumber("Intake Current", m_Intake.getOutputCurrent()); + SmartDashboard.putNumber("debug/Intake Voltage", intakePivotVoltage); + SmartDashboard.putNumber("debug/Intake CANcoder", cancoderInDegrees()); + SmartDashboard.putNumber("debug/Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Intake setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Intake Current", m_Intake.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index a6855dd..ce83f23 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -239,5 +239,12 @@ public void periodic() { SmartDashboard.putBoolean("driver/Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putNumber("Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putBoolean("debug/Break Beam Sensor", getBreakBeamOutput()); + SmartDashboard.putNumber("debug/Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index da85009..034f71c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -145,5 +145,8 @@ public void periodic() { SmartDashboard.putNumber("Shooter CANcoder", cancoderInDegrees()); SmartDashboard.putNumber("Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); SmartDashboard.putNumber("Shooter setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Shooter Pivot Voltage", m_ShooterPivotVoltage); + SmartDashboard.putNumber("debug/Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Shooter setpoint", m_setPoint); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ee19e4..e1ec3ee 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -294,6 +294,9 @@ public void periodic(){ SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); @@ -303,9 +306,18 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + for(SwerveModule mod : mSwerveMods){ + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + } + SmartDashboard.putNumber("Robot X", swerveOdometry.getPoseMeters().getX()); SmartDashboard.putNumber("Robot Y", swerveOdometry.getPoseMeters().getY()); SmartDashboard.putNumber("gyro angle", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("debug/Robot X", swerveOdometry.getPoseMeters().getX()); + SmartDashboard.putNumber("debug/Robot Y", swerveOdometry.getPoseMeters().getY()); + SmartDashboard.putNumber("debug/gyro angle", getGyroYaw().getDegrees()); From a913d67a9d1218facb2402d90c40f0a96e08cea4 Mon Sep 17 00:00:00 2001 From: leo Date: Thu, 4 Apr 2024 18:48:36 -0500 Subject: [PATCH 44/82] Revert "Merge branch 'ShuffleBoard' into pre-worlds" This reverts commit bec0d95601542f1860a903113f8e09f451eb218a, reversing changes made to 140227128cf2807aa08da1996cd5b1972c145d21. --- src/main/java/frc/robot/subsystems/Elevator.java | 4 ---- src/main/java/frc/robot/subsystems/Intake.java | 5 ----- src/main/java/frc/robot/subsystems/Shooter.java | 7 ------- src/main/java/frc/robot/subsystems/ShooterPivot.java | 3 --- src/main/java/frc/robot/subsystems/Swerve.java | 12 ------------ 5 files changed, 31 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b52d480..23ddaf1 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -252,10 +252,6 @@ public void periodic() { SmartDashboard.putNumber("Elevator Encoder Value: ", getPosition()); SmartDashboard.putNumber("Current Elevator Position",e_Elevator.getPosition()); SmartDashboard.putNumber("Elevator Voltage", voltage); - SmartDashboard.putNumber("debug/ELEVATOR TARGET POSITION", targetElevatorPosition); - SmartDashboard.putNumber("debug/Elevator Encoder Value: ", getPosition()); - SmartDashboard.putNumber("debug/Current Elevator Position",e_Elevator.getPosition()); - SmartDashboard.putNumber("debug/Elevator Voltage", voltage); // logData(); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1fbb275..8494743 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -150,10 +150,5 @@ public void periodic() { SmartDashboard.putNumber("Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); SmartDashboard.putNumber("Intake setpoint", m_setPoint); SmartDashboard.putNumber("Intake Current", m_Intake.getOutputCurrent()); - SmartDashboard.putNumber("debug/Intake Voltage", intakePivotVoltage); - SmartDashboard.putNumber("debug/Intake CANcoder", cancoderInDegrees()); - SmartDashboard.putNumber("debug/Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); - SmartDashboard.putNumber("debug/Intake setpoint", m_setPoint); - SmartDashboard.putNumber("debug/Intake Current", m_Intake.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index f7694d1..417e0b2 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -247,12 +247,5 @@ public void periodic() { SmartDashboard.putBoolean("driver/Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putNumber("Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("debug/Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); - SmartDashboard.putNumber("debug/Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); - SmartDashboard.putNumber("debug/Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); - SmartDashboard.putNumber("debug/Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); - SmartDashboard.putBoolean("debug/Break Beam Sensor", getBreakBeamOutput()); - SmartDashboard.putNumber("debug/Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); - SmartDashboard.putNumber("debug/Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index 0698ea2..4d71c79 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -145,8 +145,5 @@ public void periodic() { SmartDashboard.putNumber("Shooter CANcoder", cancoderInDegrees()); SmartDashboard.putNumber("Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); SmartDashboard.putNumber("Shooter setpoint", m_setPoint); - SmartDashboard.putNumber("debug/Shooter Pivot Voltage", m_ShooterPivotVoltage); - SmartDashboard.putNumber("debug/Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); - SmartDashboard.putNumber("debug/Shooter setpoint", m_setPoint); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index e1ec3ee..6ee19e4 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -294,9 +294,6 @@ public void periodic(){ SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); - SmartDashboard.putNumber("debug/ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); - SmartDashboard.putNumber("debug/ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); - SmartDashboard.putNumber("debug/ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); @@ -306,18 +303,9 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } - for(SwerveModule mod : mSwerveMods){ - SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); - SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); - SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); - } - SmartDashboard.putNumber("Robot X", swerveOdometry.getPoseMeters().getX()); SmartDashboard.putNumber("Robot Y", swerveOdometry.getPoseMeters().getY()); SmartDashboard.putNumber("gyro angle", getGyroYaw().getDegrees()); - SmartDashboard.putNumber("debug/Robot X", swerveOdometry.getPoseMeters().getX()); - SmartDashboard.putNumber("debug/Robot Y", swerveOdometry.getPoseMeters().getY()); - SmartDashboard.putNumber("debug/gyro angle", getGyroYaw().getDegrees()); From c2c827298f2279c4934d8e456a3d67e4616ddb16 Mon Sep 17 00:00:00 2001 From: leo Date: Thu, 4 Apr 2024 18:56:20 -0500 Subject: [PATCH 45/82] Revert "Revert "Merge branch 'ShuffleBoard' into pre-worlds"" This reverts commit a913d67a9d1218facb2402d90c40f0a96e08cea4. --- src/main/java/frc/robot/subsystems/Elevator.java | 4 ++++ src/main/java/frc/robot/subsystems/Intake.java | 5 +++++ src/main/java/frc/robot/subsystems/Shooter.java | 7 +++++++ src/main/java/frc/robot/subsystems/ShooterPivot.java | 3 +++ src/main/java/frc/robot/subsystems/Swerve.java | 12 ++++++++++++ 5 files changed, 31 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 23ddaf1..b52d480 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -252,6 +252,10 @@ public void periodic() { SmartDashboard.putNumber("Elevator Encoder Value: ", getPosition()); SmartDashboard.putNumber("Current Elevator Position",e_Elevator.getPosition()); SmartDashboard.putNumber("Elevator Voltage", voltage); + SmartDashboard.putNumber("debug/ELEVATOR TARGET POSITION", targetElevatorPosition); + SmartDashboard.putNumber("debug/Elevator Encoder Value: ", getPosition()); + SmartDashboard.putNumber("debug/Current Elevator Position",e_Elevator.getPosition()); + SmartDashboard.putNumber("debug/Elevator Voltage", voltage); // logData(); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 8494743..1fbb275 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -150,5 +150,10 @@ public void periodic() { SmartDashboard.putNumber("Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); SmartDashboard.putNumber("Intake setpoint", m_setPoint); SmartDashboard.putNumber("Intake Current", m_Intake.getOutputCurrent()); + SmartDashboard.putNumber("debug/Intake Voltage", intakePivotVoltage); + SmartDashboard.putNumber("debug/Intake CANcoder", cancoderInDegrees()); + SmartDashboard.putNumber("debug/Intake Pivot Motor Position", e_intakePivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Intake setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Intake Current", m_Intake.getOutputCurrent()); } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 417e0b2..f7694d1 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -247,5 +247,12 @@ public void periodic() { SmartDashboard.putBoolean("driver/Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putNumber("Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("debug/Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); + SmartDashboard.putBoolean("debug/Break Beam Sensor", getBreakBeamOutput()); + SmartDashboard.putNumber("debug/Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("debug/Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index 4d71c79..0698ea2 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -145,5 +145,8 @@ public void periodic() { SmartDashboard.putNumber("Shooter CANcoder", cancoderInDegrees()); SmartDashboard.putNumber("Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); SmartDashboard.putNumber("Shooter setpoint", m_setPoint); + SmartDashboard.putNumber("debug/Shooter Pivot Voltage", m_ShooterPivotVoltage); + SmartDashboard.putNumber("debug/Shooter Pivot Motor Position", e_ShooterPivotIntegrated.getPosition()); + SmartDashboard.putNumber("debug/Shooter setpoint", m_setPoint); } } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 6ee19e4..e1ec3ee 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -294,6 +294,9 @@ public void periodic(){ SmartDashboard.putNumber("ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); SmartDashboard.putNumber("ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedX", getChassisSpeed().vxMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedY", getChassisSpeed().vyMetersPerSecond); + SmartDashboard.putNumber("debug/ChassisSpeedOmega", getChassisSpeed().omegaRadiansPerSecond); @@ -303,9 +306,18 @@ public void periodic(){ SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); } + for(SwerveModule mod : mSwerveMods){ + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " CANcoder", mod.getCANcoder().getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Angle", mod.getPosition().angle.getDegrees()); + SmartDashboard.putNumber("debug/Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond); + } + SmartDashboard.putNumber("Robot X", swerveOdometry.getPoseMeters().getX()); SmartDashboard.putNumber("Robot Y", swerveOdometry.getPoseMeters().getY()); SmartDashboard.putNumber("gyro angle", getGyroYaw().getDegrees()); + SmartDashboard.putNumber("debug/Robot X", swerveOdometry.getPoseMeters().getX()); + SmartDashboard.putNumber("debug/Robot Y", swerveOdometry.getPoseMeters().getY()); + SmartDashboard.putNumber("debug/gyro angle", getGyroYaw().getDegrees()); From bea764f60096301e34705802284b3a82298f5419 Mon Sep 17 00:00:00 2001 From: leo Date: Thu, 4 Apr 2024 19:34:53 -0500 Subject: [PATCH 46/82] Apmside smart auto adjusted --- .../autos/Prototype Ampside Smart Auto.auto | 12 ++++++++++++ .../paths/Ampside Auto (Inside Center) Note 2.path | 8 ++++---- .../pathplanner/paths/Ampside Auto path 2.path | 10 +++++----- .../pathplanner/paths/Ampside Auto path 3.path | 8 ++++---- 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto index df4c451..bc4ed03 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto @@ -308,6 +308,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "Check Note" + } + }, { "type": "race", "data": { @@ -343,6 +349,12 @@ } ] } + }, + { + "type": "named", + "data": { + "name": "Got Note" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path index 01e9ab8..2a4a15b 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto (Inside Center) Note 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": null, "nextControl": { - "x": 2.69, - "y": 6.86 + "x": 2.9306931140552677, + "y": 6.8946404084978985 }, "isLocked": false, "linkedName": "Ampside Auto path 2 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path index d1c0f72..c031971 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": { - "x": 1.6548674903626555, - "y": 6.824867490362656 + "x": 1.8955606044179232, + "y": 6.859507898860554 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 29.744881296942317, + "rotation": 26.56505117707805, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path index 2fba58b..9ffcb74 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.6887664017838722, - "y": 6.862550215827963 + "x": 1.9306931140552677, + "y": 6.8946404084978985 }, "prevControl": null, "nextControl": { - "x": 7.274835434121652, - "y": 7.576911245120637 + "x": 7.516762146393047, + "y": 7.609001437790572 }, "isLocked": false, "linkedName": "Ampside Auto path 2 Endpoint" From bd7c05441a29c9ffc51d02b1713ead7d34ec033e Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sat, 6 Apr 2024 00:00:39 -0500 Subject: [PATCH 47/82] changed shot time calculation to use current shooter velocity --- src/main/java/frc/robot/subsystems/Eyes.java | 2 +- src/main/java/frc/robot/subsystems/Shooter.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index a5652dd..5638288 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -297,7 +297,7 @@ public boolean swerveAtPosition(boolean onMove) { public double getShotTime(double distance) { - double linearSpeed = ((Math.PI * 4 * s_Shooter.m_setSpeed * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio + double linearSpeed = ((Math.PI * 4 * s_Shooter.m_leftShooter.getVelocity().getValue() * (2 * Math.PI)) / 1/1.375) * 0.0254; //TODO: change shooter gear ratio return (distance / linearSpeed) + 0.25; //TODO: tune constant for shot accel/feeding time } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index f576a24..8fa40eb 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -64,8 +64,8 @@ public class Shooter extends SubsystemBase { private final double rShooterMotorVGains = 0.12; // WPILib class objects - private TalonFX m_leftShooter; - private TalonFX m_rightShooter; + public TalonFX m_leftShooter; + public TalonFX m_rightShooter; private TalonFX m_loader; private Slot0Configs slotConfigsR; From 9947326bc1765e65779890e786423200dfc4eefb Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sat, 6 Apr 2024 00:20:54 -0500 Subject: [PATCH 48/82] Added megatag 2 support and updated libraries --- src/main/java/frc/robot/LimelightHelpers.java | 45 ++++++++++++++++- src/main/java/frc/robot/subsystems/Eyes.java | 21 ++++++++ vendordeps/PathplannerLib.json | 6 +-- vendordeps/Phoenix6.json | 48 +++++++++---------- vendordeps/REVLib.json | 10 ++-- 5 files changed, 97 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 4d5c6d3..3f0b9af 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.4.0 (March 21, 2024) +//LimelightHelpers v1.5.0 (March 27, 2024) package frc.robot; @@ -758,6 +758,17 @@ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -782,6 +793,16 @@ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -865,6 +886,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 5638288..6c9c7db 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -483,6 +483,27 @@ else if (lastResult.avgTagArea > 0.1 && poseDifference < 0.3) { fpgaTimestamp - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0)); } + public void updatePoseEstimatorWithVisionBotPoseMegatag2() { + //invalid limelight + if (LimelightHelpers.getTV("") == false) { + return; + } + + boolean doRejectUpdate = false; + LimelightHelpers.SetRobotOrientation("limelight", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); + if(Math.abs(s_Swerve.gyro.getRate()) > 720) // if our angular velocity is greater than 720 degrees per second, ignore vision updates + { + doRejectUpdate = true; + } + if(!doRejectUpdate) + { + s_Swerve.m_poseEstimator.setVisionMeasurementStdDevs(VecBuilder.fill(.6,.6,9999999)); + s_Swerve.m_poseEstimator.addVisionMeasurement( + mt2.pose, + mt2.timestampSeconds); + } + } @Override public void periodic() { diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index cae1363..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.6", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.6" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.6", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 69a4079..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.1.0", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.1.0" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.1.0", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..f85acd4 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, From 2882f54eed8e18ce492f5cb708155f4479283e15 Mon Sep 17 00:00:00 2001 From: Samuel Moore <1sammoore714@gmail.com> Date: Sat, 6 Apr 2024 00:25:18 -0500 Subject: [PATCH 49/82] finished megatag2 development (I forgot to click save before my last commit) --- src/main/java/frc/robot/subsystems/Eyes.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 6c9c7db..d473118 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -510,13 +510,16 @@ public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); - //updatePoseEstimatorWithVisionBotPose(); + updatePoseEstimatorWithVisionBotPoseMegatag2(); + + /* if (LimelightHelpers.getTV("") == true) { s_Swerve.m_poseEstimator.addVisionMeasurement( getRobotPose(), Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0) ); } + */ SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); From 55f8ccc77ee6ef3b08fd81fa03d7708c96507830 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 6 Apr 2024 09:10:44 -0500 Subject: [PATCH 50/82] Changed intake positions --- src/main/java/frc/robot/subsystems/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 59db86b..30bf58d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -30,8 +30,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 28.0; - public final double intakeGroundPosition = -70.0; + public final double intakeSafePosition = 48.6; + public final double intakeGroundPosition = -54.58; public final double intakeSourcePosition = intakeSafePosition; // PID values From a08c3c7ea0d436e332149aee55d5ebf845f3d4d1 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 6 Apr 2024 16:01:37 -0500 Subject: [PATCH 51/82] marquette changes --- .../Prototype Sourceside Smart Auto.auto | 6 +++ .../paths/Four Piece Auto Path 4.path | 8 ++-- .../paths/Four Piece Auto Path 5.path | 8 ++-- .../paths/Sourceside Auto Path 1.path | 8 ++-- .../paths/Sourceside Auto Path 2.path | 22 ++++++---- .../paths/Sourceside Auto Path 3.path | 12 +++--- .../paths/Sourceside Auto Path 4.path | 22 ++++++---- .../paths/Sourceside Auto Path 5.path | 8 ++-- .../paths/Sourceside missed note 1.path | 8 ++-- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 21 ++++------ .../java/frc/robot/commands/AimShoot.java | 40 +++++++++++-------- src/main/java/frc/robot/subsystems/Eyes.java | 16 ++++---- .../java/frc/robot/subsystems/Shooter.java | 8 +--- 14 files changed, 102 insertions(+), 87 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto index 2a4c4b5..e96f2bb 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto @@ -163,6 +163,12 @@ "name": "AutoScore" } }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path index dbe1b87..5c75498 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.104707265167793, - "y": 5.68710451774403 + "x": 8.065754494498313, + "y": 5.823439215087209 }, "prevControl": { - "x": 5.682106864411436, - "y": 6.541047954025964 + "x": 5.621468134988465, + "y": 7.508146546542204 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path index 75ba5d7..7c79122 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.104707265167793, - "y": 5.68710451774403 + "x": 8.065754494498313, + "y": 5.823439215087209 }, "prevControl": null, "nextControl": { - "x": 5.2365711585252095, - "y": 6.494637984662815 + "x": 5.485133437645286, + "y": 7.663957629220123 }, "isLocked": false, "linkedName": "Four Piece Auto Path 4 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path index 512917c..6835069 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.260518347838573, - "y": 0.6427207160475723 + "x": 8.279994733180452, + "y": 0.7693172207222235 }, "prevControl": { - "x": 7.260518347838573, - "y": 0.6427207160475723 + "x": 7.279994733180452, + "y": 0.7693172207222235 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path index 11f06bf..90791bd 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.260518347838573, - "y": 0.6427207160475723 + "x": 8.279994733180452, + "y": 0.7693172207222235 }, "prevControl": null, "nextControl": { - "x": 8.299471118508054, - "y": 0.6427207160475723 + "x": 8.318947503849932, + "y": 0.7693172207222235 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" @@ -21,14 +21,20 @@ }, "prevControl": { "x": 1.5509036000278411, - "y": 1.3730851660991583 + "y": 1.3730851660991585 }, "nextControl": null, "isLocked": false, - "linkedName": "Loadside Auto Path 2 Endpoint" + "linkedName": "Sourceside Auto Path 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -29.89890183861451, + "rotateFast": true } ], - "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -30.52970589993407, + "rotation": -43.451842301022026, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path index ddce14f..a731257 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path @@ -8,11 +8,11 @@ }, "prevControl": null, "nextControl": { - "x": 5.640944520323205, - "y": -0.3213603580232063 + "x": 2.534461059432202, + "y": 0.2142402386821383 }, "isLocked": false, - "linkedName": "Loadside Auto Path 2 Endpoint" + "linkedName": "Sourceside Auto Path 2 Endpoint" }, { "anchor": { @@ -20,8 +20,8 @@ "y": 2.4150717815077387 }, "prevControl": { - "x": 7.345128237112941, - "y": 1.1491067347496495 + "x": 7.24774631043924, + "y": 0.13633469734318018 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.7, + "waypointRelativePos": 0.35, "rotationDegrees": 30.707075654360423, "rotateFast": true } diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path index a0e2150..a9146e2 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path @@ -8,27 +8,33 @@ }, "prevControl": null, "nextControl": { - "x": 4.608696097586862, - "y": 0.3310985506857081 + "x": 6.517381860386496, + "y": -0.01947638533473953 }, "isLocked": false, "linkedName": "Sourceside Auto path 3 Endpoint" }, { "anchor": { - "x": 2.261791664745845, - "y": 3.953706222952186 + "x": 1.385354324682553, + "y": 4.52825959032701 }, "prevControl": { - "x": 3.157705390143877, - "y": 2.960410570880455 + "x": 1.1029467373288258, + "y": 1.4217761294360076 }, "nextControl": null, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.05, + "rotationDegrees": -39.370000000000005, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [], "globalConstraints": { @@ -39,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -39.3693172423648, + "rotation": -45.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path index ec2ac32..25b84e9 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.261791664745845, - "y": 3.953706222952186 + "x": 1.385354324682553, + "y": 4.52825959032701 }, "prevControl": null, "nextControl": { - "x": 6.040210419685371, - "y": -0.3116221653558371 + "x": 5.1637730796220795, + "y": 0.2629312020189867 }, "isLocked": false, "linkedName": "Sourceside Auto Path 4 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path index dac3497..ad6467b 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.260518347838573, - "y": 0.6427207160475723 + "x": 8.279994733180452, + "y": 0.7693172207222235 }, "prevControl": null, "nextControl": { - "x": 6.595287401718319, - "y": 1.2367504687571356 + "x": 6.614763787060197, + "y": 1.363346973431787 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e43804..986c2f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,7 +89,7 @@ public static final class Swerve { /* These values are used by the drive falcon to ramp in open loop and closed loop driving. * We found a small open loop ramp (0.25) helps with tread wear, tipping, etc */ - public static final double openLoopRamp = 0.0; + public static final double openLoopRamp = 0.1; public static final double closedLoopRamp = 0.0; /* Angle Motor PID Values */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7add85f..6f92802 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -175,8 +175,8 @@ public RobotContainer() { //NOTE: IF REMOVED NEED TO ADD SHOOTER SPINUP FOR AMP SCORE s_Shooter.setDefaultCommand( new ConditionalCommand( - new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), - new InstantCommand(() -> s_Shooter.setShooterVoltage(5, 5), s_Shooter), //s_Shooter.shootingMotorsSetControl(20, 20) + new InstantCommand (() -> s_Shooter.shootingMotorsSetControl(0, 0), s_Shooter), + new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(35.0, 35.0), s_Shooter), //s_Shooter.shootingMotorsSetControl(20, 20) () -> s_Shooter.getBreakBeamOutput()) ); @@ -187,21 +187,14 @@ public RobotContainer() { //Auto Commands Command AimThenShootAuto = new ParallelRaceGroup( - new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false).alongWith(new TeleopSwerve( - s_Swerve, - () -> 0, - () -> 0, - () -> 0, - () -> false, - () -> s_Eyes.getTargetRotation(), - () -> false, - rotationSpeed, - true - )), + + new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, true, false), + new SequentialCommandGroup( new WaitCommand(2.0).until(() -> prepareShot()), new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)), new WaitCommand(1.5)).until(() -> s_Shooter.getBreakBeamOutput()) + ); @@ -211,7 +204,7 @@ public RobotContainer() { ); NamedCommands.registerCommand("Confirm Intake", new RunIntake(s_Intake, s_ShooterPivot, s_Shooter, s_Eyes) .until(() -> !s_Shooter.getBreakBeamOutput()) //TODO Make rollers spin after at position/0.25s - .withTimeout(1.0) + .withTimeout(1.25) ); NamedCommands.registerCommand("AutoScore", AimThenShootAuto); NamedCommands.registerCommand("Aim", new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, false)); diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 387526a..aa18270 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -19,6 +19,9 @@ import frc.robot.subsystems.Eyes; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.ShooterPivot; + +import java.sql.Driver; + import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.interpolation.InterpolatingTreeMap; import edu.wpi.first.wpilibj.DriverStation; @@ -56,35 +59,35 @@ public class AimShoot extends Command { // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.21; //1.21 at 930, 1.25 at comp + private final double subWooferDistance = 1.31; //1.21 at 930, 1.25 at comp, 1.31 at marquette private final double subWooferAngle = 115.0; //115 - private final double subWooferSpeed = 50.0; //50 + private final double subWooferSpeed = 35.0; //50 - private final double xSpotDistance = 2.4; - private final double xSpotAngle = 133.0; - private final double xSpotSpeed = 50.0; + private final double xSpotDistance = 2.45; //2.45 at marquette + private final double xSpotAngle = 131.0; + private final double xSpotSpeed = 45.0; - private final double podiumDistance = 3.17; - private final double podiumAngle = 139; + private final double podiumDistance = 3.02; //3.17 at comp, 3.02 at marquette + private final double podiumAngle = 137; private final double podiumSpeed = 60.0; - private final double d3Distance = 4.0; - private final double d3Angle = 142.5; - private final double d3Speed = 70.0; + private final double chainDistance = 4.33; //4.33 at marquette (NOT ACCCURATE) + private final double chainAngle = 145.0; + private final double chainSpeed = 80.0; private final double wingerDistance = 5.44; private final double wingerAngle = 147; - private final double wingerSpeed = 85.0; + private final double wingerSpeed = 40.25; private final double feedDistance = 2.4; private final double feedAngle = 131.0; - private final double feedSpeed = 60.0; + private final double feedSpeed = 30.0; private final double elevatorShotDistance = 2.65; private final double elevatorShotAngle = 146; - private final double elevatorShotShooterSpeed = 60; + private final double elevatorShotShooterSpeed = 30.0; // constructor @@ -108,7 +111,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); + shooterAngleInterpolation.put(chainDistance, chainAngle); shooterAngleInterpolation.put(wingerDistance, wingerAngle); @@ -117,7 +120,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i // create points in shooter linear interpolation line // TODO tune these values shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); - shooterSpeedInterpolation.put(d3Distance, d3Speed); + shooterSpeedInterpolation.put(chainDistance, chainSpeed); shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); @@ -142,13 +145,13 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, double ma shooterAngleInterpolation.put(subWooferDistance, subWooferAngle); shooterAngleInterpolation.put(podiumDistance, podiumAngle); shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); - shooterAngleInterpolation.put(d3Distance, d3Angle); + shooterAngleInterpolation.put(chainDistance, chainAngle); shooterAngleInterpolation.put(wingerDistance, wingerAngle); // create points in angle linear interpolation line // TODO tune these values shooterSpeedInterpolation.put(podiumDistance, podiumSpeed); - shooterSpeedInterpolation.put(d3Distance, d3Speed); + shooterSpeedInterpolation.put(chainDistance, chainSpeed); shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); @@ -213,6 +216,9 @@ public void execute() { public void end(boolean interrupted) { shooter.setLoaderVoltage(shooter.stopLoaderVoltage); shooter.setShooterVoltage(shooter.stopShooterVoltage, shooter.stopShooterVoltage); + if (DriverStation.isAutonomous()) { + shooter.shootingMotorsSetControl(subWooferSpeed, subWooferSpeed); + } eyes.controllerRumble = false; } diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index d473118..bc4b67a 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -335,17 +335,19 @@ public Pose2d getMovingTarget() { double robotAccelX = s_Swerve.fieldRelativeAccel.ax; double robotAccelY = s_Swerve.fieldRelativeAccel.ay; + double velocityCompensation = 2.0; + for(int i=0;i<5;i++){ double virtualGoalX; double virtualGoalY; if(DriverStation.getAlliance().get() == Alliance.Blue) { - virtualGoalX = getTargetPose().getX() - shotTime * (robotVelX + robotAccelX * accelerationCompensation); - virtualGoalY = getTargetPose().getY() - shotTime * (robotVelY + robotAccelY * accelerationCompensation); + virtualGoalX = getTargetPose().getX() - shotTime * ((robotVelX * velocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() - shotTime * ((robotVelY * velocityCompensation) + robotAccelY * accelerationCompensation); } else { - virtualGoalX = getTargetPose().getX() + shotTime * (robotVelX + robotAccelX * accelerationCompensation); - virtualGoalY = getTargetPose().getY() + shotTime * (robotVelY + robotAccelY * accelerationCompensation); + virtualGoalX = getTargetPose().getX() + shotTime * ((robotVelX * velocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() + shotTime * ((robotVelY * velocityCompensation) + robotAccelY * accelerationCompensation); } Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); @@ -510,16 +512,16 @@ public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); - updatePoseEstimatorWithVisionBotPoseMegatag2(); + //updatePoseEstimatorWithVisionBotPose(); - /* + if (LimelightHelpers.getTV("") == true) { s_Swerve.m_poseEstimator.addVisionMeasurement( getRobotPose(), Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0) ); } - */ + SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 8fa40eb..a3ac130 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -46,20 +46,16 @@ public class Shooter extends SubsystemBase { // left shooter motor PID - private final double lShooterMotorPGains = 0.05; + private final double lShooterMotorPGains = 0.4; private final double lShooterMotorIGains = 0.0; private final double lShooterMotorDGains = 0.0; private final double lShooterMotorSGains = 0.0; private final double lShooterMotorVGains = 0.12; // right shooter motor PID - private final double rShooterMotorPGains = 0.05; + private final double rShooterMotorPGains = 0.4; private final double rShooterMotorIGains = 0.0; private final double rShooterMotorDGains = 0.0; - - - - private final double rShooterMotorSGains = 0.0; private final double rShooterMotorVGains = 0.12; From 199821c51e7a63552602c008a0230edab10225d8 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 6 Apr 2024 16:37:55 -0500 Subject: [PATCH 52/82] added source intake --- src/main/java/frc/robot/RobotContainer.java | 34 +++++++++---- .../java/frc/robot/commands/RunLoader.java | 51 +++++++++++++++++++ 2 files changed, 74 insertions(+), 11 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RunLoader.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 94137c1..e03e234 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -434,19 +434,31 @@ private void configureButtonBindings() { ) ); //source intake 1: shooter pivot will flip and the note will go in on the intake side. - operatorY.onTrue( + //TODO: debug + operatorY.whileTrue( + + new SequentialCommandGroup( + new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(8.85)), + s_Elevator.ElevatorAtPosition(), + + new ParallelCommandGroup( + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(325)), + new RunLoader(s_Shooter).until(() -> !s_Shooter.getBreakBeamOutput()) + .andThen(new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) + )))) + + ).onFalse( new ParallelCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition(/*TODO height of elevator for source intake 1 */)), - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(/*TODO pivot position for source intake 1 */)), - new InstantCommand(() -> s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage)) + new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) + ), + new SequentialCommandGroup( + new AmpShooterPivotRetract(s_ShooterPivot), + s_ShooterPivot.ShooterPivotAtPosition(), + new AmpElevatorRetract(s_Elevator) ) - ); - //source intake 2: note will come in through the shooter side. - operatorB.onTrue( - new ParallelCommandGroup( - new InstantCommand(() -> s_Elevator.SetElevatorPosition(/*TODO height of elevator for source intake 2 */)), - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(/*TODO pivot position for source intake 2 */)), - new InstantCommand(() -> s_Shooter.setShooterVoltage(/*TODO value to make wheels go oppisite way*/),)//wheels will spin the oppisite way ) ); diff --git a/src/main/java/frc/robot/commands/RunLoader.java b/src/main/java/frc/robot/commands/RunLoader.java new file mode 100644 index 0000000..a0ebc11 --- /dev/null +++ b/src/main/java/frc/robot/commands/RunLoader.java @@ -0,0 +1,51 @@ + +package frc.robot.commands; + +import frc.robot.subsystems.Shooter; +import edu.wpi.first.wpilibj2.command.Command; + + +public class RunLoader extends Command { + + // required subystems + Shooter s_Shooter; + + // required WPILib class objects + + + // local variables + + + + + // constructor + public RunLoader(Shooter s_Shooter) { + + this.s_Shooter = s_Shooter; + + addRequirements(s_Shooter); + + + } + + @Override + public void execute() { + + s_Shooter.setLoaderVoltage(s_Shooter.runLoaderVoltage); + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + s_Shooter.setLoaderVoltage(s_Shooter.stopLoaderVoltage); + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file From 7c5e055edc115db40252be691e743ec099801161 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 6 Apr 2024 17:04:37 -0500 Subject: [PATCH 53/82] tuned feeding --- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/commands/AimShoot.java | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f92802..0a302b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -479,6 +479,8 @@ private void configureButtonBindings() { ) ); + operatorRightTrigger.onTrue(new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(50, 50))) + .onFalse(new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(0, 0))); /* Operator Buttons */ diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index aa18270..b9d9124 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -82,8 +82,8 @@ public class AimShoot extends Command { private final double feedDistance = 2.4; - private final double feedAngle = 131.0; - private final double feedSpeed = 30.0; + private final double feedAngle = 117.0; + private final double feedSpeed = 65.0; private final double elevatorShotDistance = 2.65; private final double elevatorShotAngle = 146; From f87a2c425b4410e7d34bae1dcf078f8f83060d06 Mon Sep 17 00:00:00 2001 From: GraceGivens <140421192+GraceGivens@users.noreply.github.com> Date: Mon, 8 Apr 2024 18:00:02 -0500 Subject: [PATCH 54/82] changed comment and reviewed code --- src/main/java/frc/robot/RobotContainer.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e03e234..fe1b724 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,8 +433,7 @@ private void configureButtonBindings() { new AmpElevatorRetract(s_Elevator) ) ); - //source intake 1: shooter pivot will flip and the note will go in on the intake side. - //TODO: debug + //source intake operatorY.whileTrue( new SequentialCommandGroup( From 2cbb07b9f1a31c1fa1234bd92ffe20419618bac5 Mon Sep 17 00:00:00 2001 From: GraceGivens <140421192+GraceGivens@users.noreply.github.com> Date: Mon, 8 Apr 2024 18:16:04 -0500 Subject: [PATCH 55/82] got rid of manual climb --- src/main/java/frc/robot/RobotContainer.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 79d6607..219a5e8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -407,8 +407,6 @@ private void configureButtonBindings() { ) ); - //Manual climb - driverDpadDown.onTrue(new InstantCommand(() -> s_Elevator.climb())); /* Operator Buttons */ From 45f8464dc2eb952f9c2be6691559dc01d1be9f98 Mon Sep 17 00:00:00 2001 From: ColinChen25 Date: Tue, 9 Apr 2024 18:17:45 -0500 Subject: [PATCH 56/82] Create TemplateSubsystem.java --- .../robot/subsystems/TemplateSubsystem.java | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/TemplateSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/TemplateSubsystem.java b/src/main/java/frc/robot/subsystems/TemplateSubsystem.java new file mode 100644 index 0000000..4795cab --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TemplateSubsystem.java @@ -0,0 +1,80 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + +public class TemplateSubsystem extends SubsystemBase { + /* Instance Variables + * This section should contain any variables that need to + * be accessed by your entire subsystem/class. This usually includes any phyical motors or sensors that are a part of your subsystem, + * as well as any data that needs to be accessed by your + * entire subsystem/class E.G. the height of the elevator + * or angle of the wrist. + */ + + /* These are variable declarations. Their access modifiers, + * types, and names are specified but they are not given a + * value yet. They should be given a value in the + * constructor. In this example, they is private, which + * means that nothing outside of this class can access it. + */ + private CANSparkMax motor; + private double internalData; + + /* Constructor + * The Constructor is a special type of method that gets + * called when this subsystem/class is created, usually + * when the robot turns on. You should give a value to most + * instance variables created above in this method. You can + * also do anything else that you want to happen right away. + */ + public TemplateSubsystem() { + /* These are variable initializations, where a variable + * is given a value. In this case, the `new` keyword is + * used to create a new CANSparkMax object and give it + * to `motor` as it's value. + */ + motor = new CANSparkMax(0, MotorType.kBrushless); + internalData = 0.4; + } + + /* Methods + * The below section should contain any methods you want + * to use in your class. + */ + + /* The below method is a private method, which means that + * it can only be used inside of this class. It wants a + * single double value as input, which it is going to + * store in the variable `num`. It then takes `num` and + * multiplies it by 3 and returns the resulting value. + */ + private double tripleNum(double num) { + return num * 3; + } + + /* The below method is public, which means that it can be + * accessed outside of this class. It also takes in a + * single double value as a parameter. It then takes that + * value (called `number`) and multiplies it by + * `internalData` to change it's value. + */ + public void doSomething(double number) { + internalData = internalData * number; + } + + /* The below method is included in every Subsystem. You can + * think of it as an infinite loop that runs constantly + * while the robot is on. + * + */ + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From ea74e34ad31d26b0abb0e438a17bec3bbbf5703a Mon Sep 17 00:00:00 2001 From: SubzeroHero76 Date: Tue, 9 Apr 2024 19:20:31 -0500 Subject: [PATCH 57/82] Fixed issues with internal encoder being out of sync with cancoder --- src/main/java/frc/robot/subsystems/Intake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 1fbb275..9788965 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -82,7 +82,7 @@ public Intake() { // TODO: Go over this part with Dylan and student that wrote this. Can we simplify this? e_intakePivotIntegrated = m_IntakePivot.getEncoder(); e_intakePivotIntegrated.setPositionConversionFactor(360 / intakePivotMotorGearRatio); - e_intakePivotIntegrated.setPosition(e_intakePivot.getPosition().getValue()); + e_intakePivotIntegrated.setPosition(e_intakePivot.getPosition().getValue() * 360); e_intakePivotIntegrated.setVelocityConversionFactor(360 / intakePivotMotorGearRatio); // create PID loop for intake pivot @@ -141,7 +141,7 @@ public double cancoderInDegrees() { public void periodic() { // move the intake pivot motor to the current desired position - intakePivotVoltage = pid.calculate(cancoderInDegrees(), m_setPoint)/*+ feedForward) */; + intakePivotVoltage = pid.calculate(e_intakePivotIntegrated.getPosition(), m_setPoint)/*+ feedForward) */; m_IntakePivot.setVoltage(intakePivotVoltage); // log values From da8a701aaa892ccf8d81b4337e61a443bcf75b47 Mon Sep 17 00:00:00 2001 From: DriverStation1714 Date: Tue, 9 Apr 2024 20:02:06 -0500 Subject: [PATCH 58/82] blower subsystem in progress --- .../{TemplateSubsystem.java => Blower.java} | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) rename src/main/java/frc/robot/subsystems/{TemplateSubsystem.java => Blower.java} (90%) diff --git a/src/main/java/frc/robot/subsystems/TemplateSubsystem.java b/src/main/java/frc/robot/subsystems/Blower.java similarity index 90% rename from src/main/java/frc/robot/subsystems/TemplateSubsystem.java rename to src/main/java/frc/robot/subsystems/Blower.java index 4795cab..cfd0c89 100644 --- a/src/main/java/frc/robot/subsystems/TemplateSubsystem.java +++ b/src/main/java/frc/robot/subsystems/Blower.java @@ -1,14 +1,11 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems; + import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkLowLevel.MotorType; -public class TemplateSubsystem extends SubsystemBase { +public class Blower extends SubsystemBase { /* Instance Variables * This section should contain any variables that need to * be accessed by your entire subsystem/class. This usually includes any phyical motors or sensors that are a part of your subsystem, @@ -33,7 +30,7 @@ public class TemplateSubsystem extends SubsystemBase { * instance variables created above in this method. You can * also do anything else that you want to happen right away. */ - public TemplateSubsystem() { + public Blower() { /* These are variable initializations, where a variable * is given a value. In this case, the `new` keyword is * used to create a new CANSparkMax object and give it @@ -77,4 +74,4 @@ public void doSomething(double number) { public void periodic() { // This method will be called once per scheduler run } -} +} \ No newline at end of file From a01ebb1d676b39e3bb4d82f9219f6481ba9d5934 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Wed, 10 Apr 2024 12:43:22 -0500 Subject: [PATCH 59/82] Split command to subsystems --- src/main/java/frc/robot/Constants.java | 31 ++++++- src/main/java/frc/robot/RobotContainer.java | 3 +- src/main/java/frc/robot/subsystems/Eyes.java | 91 +++++++++++++++++++ .../java/frc/robot/subsystems/Swerve.java | 33 ++++++- 4 files changed, 155 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 986c2f3..442bfe7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -178,7 +178,8 @@ public static final class AutoConstants { //TODO: The below constants are used i } public static final class Positions { - + + //Speaker positions public static final double speakerBlueX = 0; public static final double speakerBlueY = 5.5; public static final double speakerBlueR = 0; @@ -187,6 +188,7 @@ public static final class Positions { public static final double speakerRedY = 5.5; public static final double speakerRedR = 180; + // amp positions (used for Feeding) public static final double ampBlueX = 1.9; public static final double ampBlueY = 8.0; public static final double ampBlueR = 270; @@ -195,7 +197,34 @@ public static final class Positions { public static final double ampRedY = 8.0; public static final double ampRedR = 270; + public static final double distanceLimit = 3.0; + + // blue trap positions + public static final double blueTrapLeftX = 4.3; + public static final double blueTrapLeftY = 5.0; + public static final double blueTrapLeftR = -60.0; + + public static final double blueTrapRightX = 4.3; + public static final double blueTrapRightY = 3.0; + public static final double blueTrapRightR = 60.0; + + public static final double blueTrapCenterX = 6.1; + public static final double blueTrapCenterY = 4.1; + public static final double blueTrapCenterR = 180.0; + // red trap positions + public static final double redTrapLeftX = 12.3; + public static final double redTrapLeftY = 3.0; + public static final double redTrapLeftR = 121.64; + + public static final double redTrapRightX = 12.3; + public static final double redTrapRightY = 5.0; + public static final double redTrapRightR = -121.64; + + public static final double redTrapCenterX = 10.4; + public static final double redTrapCenterY = 4.1; + public static final double redTrapCenterR = 0.0; + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 510c7b1..101b455 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -432,7 +432,8 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new GoToTrap(s_Eyes, s_Swerve)).onFalse(s_Swerve.getDefaultCommand()); + driverStart.whileTrue(new ConditionalCommand(s_Swerve.onTheFly(s_Eyes.closestTrapPath()), new InstantCommand(), () -> s_Eyes.closeToTrap)) + .onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index bc4b67a..1da2fe9 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -16,17 +16,25 @@ import frc.lib.util.FieldRelativeSpeed; import frc.robot.Constants; import java.util.List; + +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; @@ -51,13 +59,16 @@ public class Eyes extends SubsystemBase { private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times private StructPublisher posePublisher; private StructPublisher translationPublisher; + private StructPublisher trapPublisher; public boolean controllerRumble = false; + public boolean closeToTrap = false; // constuctor public Eyes(Swerve swerve, Shooter shooter) { posePublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal pose", Pose2d.struct).publish(); translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal translation", Translation2d.struct).publish(); + trapPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); s_Swerve = swerve; s_Shooter = shooter; } @@ -444,6 +455,86 @@ public double getDistanceFromTargetTrap(double trapX, double trapY) { } + public Pose2d getClosestTrap() { + + double trapLeftDistance; + double trapRightDistance; + double trapCenterDistance; + + Pose2d closestTrap = new Pose2d(); + double closestDistance; + + if(DriverStation.getAlliance().get() == Alliance.Blue) { + trapLeftDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapLeftX, Constants.Positions.blueTrapLeftY); + trapRightDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapRightX, Constants.Positions.blueTrapRightY); + trapCenterDistance = getDistanceFromTargetTrap(Constants.Positions.blueTrapCenterX, Constants.Positions.blueTrapCenterY); + + closestDistance = Math.min(Math.min(trapLeftDistance, trapRightDistance), trapCenterDistance); + + if (closestDistance == trapLeftDistance) { + closestTrap = new Pose2d(Constants.Positions.blueTrapLeftX, Constants.Positions.blueTrapLeftY, Rotation2d.fromDegrees(Constants.Positions.blueTrapLeftR)); + } else if (closestDistance == trapRightDistance){ + closestTrap = new Pose2d(Constants.Positions.blueTrapRightX, Constants.Positions.blueTrapRightY, Rotation2d.fromDegrees(Constants.Positions.blueTrapRightR)); + } else { + closestTrap = new Pose2d(Constants.Positions.blueTrapCenterX, Constants.Positions.blueTrapCenterY, Rotation2d.fromDegrees(Constants.Positions.blueTrapCenterR)); + } + + } else { + trapLeftDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapLeftX, Constants.Positions.redTrapLeftY); + trapRightDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapRightX, Constants.Positions.redTrapRightY); + trapCenterDistance = getDistanceFromTargetTrap(Constants.Positions.redTrapCenterX, Constants.Positions.redTrapCenterY); + + closestDistance = Math.min(Math.min(trapLeftDistance, trapRightDistance), trapCenterDistance); + + if (closestDistance == trapLeftDistance) { + closestTrap = new Pose2d(Constants.Positions.redTrapLeftX, Constants.Positions.redTrapLeftY, Rotation2d.fromDegrees(Constants.Positions.redTrapLeftR)); + } else if (closestDistance == trapRightDistance){ + closestTrap = new Pose2d(Constants.Positions.redTrapRightX, Constants.Positions.redTrapRightY, Rotation2d.fromDegrees(Constants.Positions.redTrapRightR)); + } else { + closestTrap = new Pose2d(Constants.Positions.redTrapCenterX, Constants.Positions.redTrapCenterY, Rotation2d.fromDegrees(Constants.Positions.redTrapCenterR)); + } + } + + if (closestDistance < Constants.Positions.distanceLimit) { + closeToTrap = true; + } else { + closeToTrap = false; + } + + //Log Target Trap Location + trapPublisher.set(new Translation2d(closestTrap.getX(), closestTrap.getY())); + + return closestTrap; + } + + + public PathPlannerPath closestTrapPath() { + + Pose2d closestTrap = getClosestTrap(); + Pose2d currentPose = getRobotPose(); + + //The bezierFromPoses method required that the rotation component of each pose is the direction of travel, not the rotation of a swerve chassis. To set the rotation the path should end with, use the GoalEndState. + + List bezierPoints = PathPlannerPath.bezierFromPoses( + + new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.minus(closestTrap).getRotation()), //TODO Rotation may be wrong + new Pose2d(closestTrap.getX(), closestTrap.getY(), closestTrap.minus(currentPose).getRotation()) //TODO Rotation may be wrong + + ); + + PathPlannerPath path = new PathPlannerPath( + bezierPoints, + new PathConstraints(0.25, 0.25, 2 * Math.PI, 4 * Math.PI), //TODO adjust speeds + new GoalEndState(0.0, closestTrap.getRotation()) + ); + + // Prevent the path from being flipped as all coordinates are blue origin + path.preventFlipping = true; + + return path; + } + + public void updatePoseEstimatorWithVisionBotPose() { //invalid limelight if (LimelightHelpers.getTV("") == false) { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 35d9a53..31dd322 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -16,6 +16,8 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; +import java.util.List; + import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; @@ -48,6 +50,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; @@ -295,7 +299,34 @@ public void resetModulesToAbsolute(){ } } - + public Command onTheFly(PathPlannerPath path) { + + return new FollowPathHolonomic( + path, + this::getPose, // Robot pose supplier + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeed, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + Constants.Swerve.maxSpeed, // Max module speed, in m/s + 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } @Override From e864ade3b7d309f166951158fd712a6064f7f94c Mon Sep 17 00:00:00 2001 From: Driver Station Date: Wed, 10 Apr 2024 21:10:35 -0500 Subject: [PATCH 60/82] Only runs on the fly on startup in robotcontainer :( --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/Eyes.java | 36 ++++++++++++++++++- .../java/frc/robot/subsystems/Swerve.java | 28 --------------- 4 files changed, 37 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 442bfe7..4b5e5da 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,7 +197,7 @@ public static final class Positions { public static final double ampRedY = 8.0; public static final double ampRedR = 270; - public static final double distanceLimit = 3.0; + public static final double distanceLimit = 100.0; // blue trap positions public static final double blueTrapLeftX = 4.3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 101b455..2e14310 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -432,7 +432,7 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new ConditionalCommand(s_Swerve.onTheFly(s_Eyes.closestTrapPath()), new InstantCommand(), () -> s_Eyes.closeToTrap)) + driverStart.whileTrue(new ConditionalCommand(s_Eyes.onTheFly(), new InstantCommand(), () -> s_Eyes.closeToTrap)) .onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble //Feed diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 1da2fe9..5f7c5ae 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -17,9 +17,13 @@ import frc.robot.Constants; import java.util.List; +import com.pathplanner.lib.commands.FollowPathHolonomic; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -511,7 +515,7 @@ public Pose2d getClosestTrap() { public PathPlannerPath closestTrapPath() { Pose2d closestTrap = getClosestTrap(); - Pose2d currentPose = getRobotPose(); + Pose2d currentPose = s_Swerve.m_poseEstimator.getEstimatedPosition(); //The bezierFromPoses method required that the rotation component of each pose is the direction of travel, not the rotation of a swerve chassis. To set the rotation the path should end with, use the GoalEndState. @@ -534,6 +538,36 @@ public PathPlannerPath closestTrapPath() { return path; } + public Command onTheFly() { + PathPlannerPath path = closestTrapPath(); + return new FollowPathHolonomic( + path, + () -> s_Swerve.getPose(), // Robot pose supplier + () -> s_Swerve.getChassisSpeed(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + (speeds) -> s_Swerve.setChassisSpeed(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + Constants.Swerve.maxSpeed, // Max module speed, in m/s + 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + s_Swerve // Reference to this subsystem to set requirements + ); + } + + public void updatePoseEstimatorWithVisionBotPose() { //invalid limelight diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 31dd322..c36ec78 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -299,35 +299,7 @@ public void resetModulesToAbsolute(){ } } - public Command onTheFly(PathPlannerPath path) { - return new FollowPathHolonomic( - path, - this::getPose, // Robot pose supplier - this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - this::setChassisSpeed, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - Constants.Swerve.maxSpeed, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - this // Reference to this subsystem to set requirements - ); - } - @Override public void periodic(){ From 74a3127f06be7f0ec6796f7fe08c8002ad507a2f Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Thu, 11 Apr 2024 00:01:02 -0500 Subject: [PATCH 61/82] Resolve running once of OTF (untested) --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/commands/GoToTrap.java | 207 ------------------ src/main/java/frc/robot/subsystems/Eyes.java | 54 ++--- .../java/frc/robot/subsystems/Swerve.java | 27 +++ 5 files changed, 57 insertions(+), 236 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/GoToTrap.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4b5e5da..8a9cf00 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,7 +197,7 @@ public static final class Positions { public static final double ampRedY = 8.0; public static final double ampRedR = 270; - public static final double distanceLimit = 100.0; + public static final double distanceLimit = 100.0; //TODO make safe distance // blue trap positions public static final double blueTrapLeftX = 4.3; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2e14310..02b342e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,6 +31,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -432,7 +433,7 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new ConditionalCommand(s_Eyes.onTheFly(), new InstantCommand(), () -> s_Eyes.closeToTrap)) + driverStart.whileTrue(new ConditionalCommand(new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), new InstantCommand(), () -> s_Eyes.closeToTrap)) //TODO Test this, was only running on init earlier .onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble //Feed diff --git a/src/main/java/frc/robot/commands/GoToTrap.java b/src/main/java/frc/robot/commands/GoToTrap.java deleted file mode 100644 index 34f08e2..0000000 --- a/src/main/java/frc/robot/commands/GoToTrap.java +++ /dev/null @@ -1,207 +0,0 @@ -package frc.robot.commands; - -import java.util.List; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; - -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.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.StructPublisher; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Swerve; -import frc.robot.subsystems.Eyes; - -public class GoToTrap extends Command { - - Eyes s_Eyes; - Swerve s_Swerve; - - private StructPublisher translationPublisher; - - public double robotX = 0.0; - public double robotY = 0.0; - public double robotR = 0.0; - - public double targetX = 0.0; - public double targetY = 0.0; - public double targetR = 0.0; - - public double positionTolerance = 0.1; - public double rotationTolerance = 2.0; - public double distanceLimit = 6.0; - public double closestDistance = distanceLimit; - - // blue trap positions - public double blueTrapLeftX = 4.3; - public double blueTrapLeftY = 5.0; - public double blueTrapLeftR = -60.0; - public double blueTrapLeftDistance = 0.0; // Leave as 0.0 - - public double blueTrapRightX = 4.3; - public double blueTrapRightY = 3.0; - public double blueTrapRightR = 60.0; - public double blueTrapRightDistance = 0.0; // Leave as 0.0 - - public double blueTrapCenterX = 6.1; - public double blueTrapCenterY = 4.1; - public double blueTrapCenterR = 180.0; - public double blueTrapCenterDistance = 0.0; // Leave as 0.0 - - // red trap positions - public double redTrapLeftX = 12.3; - public double redTrapLeftY = 3.0; - public double redTrapLeftR = 121.64; - public double redTrapLeftDistance = 0.0; // Leave as 0.0 - - public double redTrapRightX = 12.3; - public double redTrapRightY = 5.0; - public double redTrapRightR = -121.64; - public double redTrapRightDistance = 0.0; // Leave as 0.0 - - public double redTrapCenterX = 10.4; - public double redTrapCenterY = 4.1; - public double redTrapCenterR = 0.0; - public double redTrapCenterDistance = 0.0; // Leave as 0.0 - - public PathPlannerPath path; - - public GoToTrap(Eyes s_Eyes, Swerve s_Swerve) { - - this.s_Eyes = s_Eyes; - this.s_Swerve = s_Swerve; - - addRequirements(s_Eyes, s_Swerve); - } - - @Override - public void initialize() { - - translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); - - robotX = s_Eyes.getRobotPose().getX(); - robotY = s_Eyes.getRobotPose().getY(); - robotR = s_Eyes.getRobotPose().getRotation().getDegrees(); - - blueTrapLeftDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapLeftX, blueTrapLeftY); - blueTrapRightDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapRightX, blueTrapRightY); - blueTrapCenterDistance = s_Eyes.getDistanceFromTargetTrap(blueTrapCenterX, blueTrapCenterY); - - redTrapLeftDistance = s_Eyes.getDistanceFromTargetTrap(redTrapLeftX, redTrapRightY); - redTrapRightDistance = s_Eyes.getDistanceFromTargetTrap(redTrapRightX, redTrapRightY); - redTrapCenterDistance = s_Eyes.getDistanceFromTargetTrap(redTrapCenterX, redTrapCenterY); - - double[] distances = { - blueTrapLeftDistance, - blueTrapRightDistance, - blueTrapCenterDistance, - redTrapLeftDistance, - redTrapRightDistance, - redTrapCenterDistance - }; - - for(int i = 0; i < distances.length; i++) { - if (closestDistance > distances[i]) { - closestDistance = distances[i]; - } - } - - if (DriverStation.getAlliance().get() == Alliance.Blue) { - if (closestDistance == blueTrapLeftDistance) { - targetX = blueTrapLeftX; - targetY = blueTrapLeftY; - targetR = blueTrapLeftR; - } else if (closestDistance == blueTrapRightDistance) { - targetX = blueTrapRightX; - targetY = blueTrapRightY; - targetR = blueTrapRightR; - } else if (closestDistance == blueTrapCenterDistance) { - targetX = blueTrapCenterX; - targetY = blueTrapCenterY; - targetR = blueTrapCenterR; - } - } else if (DriverStation.getAlliance().get() == Alliance.Red) { - if (closestDistance == redTrapLeftDistance) { - targetX = redTrapLeftX; - targetY = redTrapLeftY; - targetR = redTrapLeftR; - } else if (closestDistance == redTrapRightDistance) { - targetX = redTrapRightX; - targetY = redTrapRightY; - targetR = redTrapRightR; - } else if (closestDistance == redTrapCenterDistance) { - targetX = redTrapCenterX; - targetY = redTrapCenterY; - targetR = redTrapCenterR; - } - } else { - closestDistance = distanceLimit; - } - - //Log Target Trap Location - translationPublisher.set(new Translation2d(targetX, targetY)); - - List bezierPoints = PathPlannerPath.bezierFromPoses( - - new Pose2d(s_Eyes.getRobotPose().getX(), s_Eyes.getRobotPose().getY(), Rotation2d.fromDegrees(0)), - new Pose2d(targetX, targetY, Rotation2d.fromDegrees(0)) - - ); - - - PathPlannerPath path = new PathPlannerPath( - bezierPoints, - new PathConstraints(0.25, 0.25, 2 * Math.PI, 4 * Math.PI), - new GoalEndState(0.0, Rotation2d.fromDegrees(targetR)) - ); - - // Prevent the path from being flipped as all coordinates are blue origin - path.preventFlipping = true; - - - } - - @Override - public void execute() { - - if (closestDistance < distanceLimit) { //DO NOT INCLUDE DISTANCE LIMIT - AutoBuilder.followPath(path); - SmartDashboard.putString("Trap Status", "Going to Trap"); - } else{ - SmartDashboard.putString("Trap Status", "Too Far Away To Trap"); - } - - } - - - @Override - public void end(boolean interrupted) { - - } - - @Override - public boolean isFinished() { - - double xError = Math.abs(targetX - s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); - double yError = Math.abs(targetY - s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); - double rError = Math.abs(targetR - s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees() % 360); - - SmartDashboard.putBoolean("Trap X Error", xError <= positionTolerance); - SmartDashboard.putBoolean("Trap Y Error", yError <= positionTolerance); - SmartDashboard.putBoolean("Trap Rotation Error", rError <= rotationTolerance); - - if (xError <= positionTolerance && yError <= positionTolerance && rError <= rotationTolerance) { - return true; - } else { - return false; - } - - } -} diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 5f7c5ae..bd10e3c 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -538,34 +538,34 @@ public PathPlannerPath closestTrapPath() { return path; } - public Command onTheFly() { - PathPlannerPath path = closestTrapPath(); - return new FollowPathHolonomic( - path, - () -> s_Swerve.getPose(), // Robot pose supplier - () -> s_Swerve.getChassisSpeed(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - (speeds) -> s_Swerve.setChassisSpeed(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class - new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants - Constants.Swerve.maxSpeed, // Max module speed, in m/s - 0.4, // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig() // Default path replanning config. See the API for the options here - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + // public Command onTheFly() { + // PathPlannerPath path = closestTrapPath(); + // return new FollowPathHolonomic( + // path, + // () -> s_Swerve.getPose(), // Robot pose supplier + // () -> s_Swerve.getChassisSpeed(), // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + // (speeds) -> s_Swerve.setChassisSpeed(speeds), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + // new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + // new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + // new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + // Constants.Swerve.maxSpeed, // Max module speed, in m/s + // 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + // new ReplanningConfig() // Default path replanning config. See the API for the options here + // ), + // () -> { + // // Boolean supplier that controls when the path will be mirrored for the red alliance + // // This will flip the path being followed to the red side of the field. + // // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - return alliance.get() == DriverStation.Alliance.Red; - } - return false; - }, - s_Swerve // Reference to this subsystem to set requirements - ); - } + // var alliance = DriverStation.getAlliance(); + // if (alliance.isPresent()) { + // return alliance.get() == DriverStation.Alliance.Red; + // } + // return false; + // }, + // s_Swerve // Reference to this subsystem to set requirements + // ); + // } diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index c36ec78..d4b39a2 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -299,7 +299,34 @@ public void resetModulesToAbsolute(){ } } + public Command onTheFly(PathPlannerPath path) { + return new FollowPathHolonomic( + path, + this::getPose, // Robot pose supplier + this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + this::setChassisSpeed, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class + new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants + new PIDConstants(5.0, 0.0, 0.0), // Rotation PID constants + Constants.Swerve.maxSpeed, // Max module speed, in m/s + 0.4, // Drive base radius in meters. Distance from robot center to furthest module. + new ReplanningConfig() // Default path replanning config. See the API for the options here + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + this // Reference to this subsystem to set requirements + ); + } @Override public void periodic(){ From 86636dd18948578bc908f5ae9acfd0572ba849f9 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Thu, 11 Apr 2024 13:53:58 -0500 Subject: [PATCH 62/82] Warmup and pathfinding command (untested) --- src/main/java/frc/robot/Robot.java | 6 ++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/Swerve.java | 18 ++++++++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 532e5c8..d183b0c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,6 +4,8 @@ package frc.robot; +import com.pathplanner.lib.commands.PathfindingCommand; + import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.cscore.VideoMode; @@ -39,8 +41,12 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + DataLogManager.start(); + // DO THIS AFTER CONFIGURATION OF YOUR DESIRED PATHFINDER + PathfindingCommand.warmupCommand().schedule(); + final UsbCamera usbCamera = CameraServer.startAutomaticCapture(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 02b342e..e9bc3fb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,7 +433,7 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new ConditionalCommand(new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), new InstantCommand(), () -> s_Eyes.closeToTrap)) //TODO Test this, was only running on init earlier + driverStart.whileTrue(new ConditionalCommand(new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), new InstantCommand(), () -> s_Eyes.closeToTrap)) //TODO Test this, was only running on init earlier, may need to be run command .onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble //Feed diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index d4b39a2..f763c2a 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -328,6 +328,24 @@ public Command onTheFly(PathPlannerPath path) { ); } + + + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + Command pathfindingCommand(Pose2d targetPose) { + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + + return AutoBuilder.pathfindToPose( + targetPose, + constraints, + 0.0, // Goal end velocity in meters/sec + 0.0 // Rotation delay distance in meters. This is how far the robot should travel before attempting to rotate. + ); + } + @Override public void periodic(){ swerveOdometry.update(getGyroYaw(), getModulePositions()); From 90772b6a4a593fd8af89cb5250f771e1efc7f1f7 Mon Sep 17 00:00:00 2001 From: leo Date: Thu, 11 Apr 2024 17:56:42 -0500 Subject: [PATCH 63/82] Blower methods added --- src/main/java/frc/robot/subsystems/Blower.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Blower.java b/src/main/java/frc/robot/subsystems/Blower.java index cfd0c89..310ccbd 100644 --- a/src/main/java/frc/robot/subsystems/Blower.java +++ b/src/main/java/frc/robot/subsystems/Blower.java @@ -36,7 +36,7 @@ public Blower() { * used to create a new CANSparkMax object and give it * to `motor` as it's value. */ - motor = new CANSparkMax(0, MotorType.kBrushless); + motor = new CANSparkMax(0, MotorType.kBrushed); internalData = 0.4; } @@ -65,6 +65,14 @@ public void doSomething(double number) { internalData = internalData * number; } + public void runBlower() { + motor.setVoltage(internalData); + } + + public void stopBlower() { + motor.setVoltage(0); + } + /* The below method is included in every Subsystem. You can * think of it as an infinite loop that runs constantly * while the robot is on. From 98d305f7d78456d809661b604e014efdc8ec0fbe Mon Sep 17 00:00:00 2001 From: DriverStation1714 Date: Thu, 11 Apr 2024 19:21:30 -0500 Subject: [PATCH 64/82] tuned smart auto missed note paths --- .../autos/Prototype Ampside Smart Auto.auto | 19 --------------- .../paths/Ampside Auto Path 7.path | 8 +++---- .../paths/Ampside Auto Path 8.path | 8 +++---- .../paths/Ampside Missed Note 2.path | 22 ++++++++++------- .../paths/Ampside Missed Note 3.path | 22 ++++++++++------- .../paths/Sourceside Auto Path 3.path | 4 ++-- .../paths/Sourceside Auto Path 4.path | 4 ++-- .../paths/Sourceside missed Note 2.path | 24 +++++++++---------- .../paths/Sourceside missed note 1.path | 22 ++++++++--------- 9 files changed, 63 insertions(+), 70 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto index bc4ed03..99bb9da 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto @@ -43,25 +43,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - }, - { - "type": "path", - "data": { - "pathName": "Ampside Auto path 2" - } - } - ] - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path index f476573..2d7f780 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 8.157095812938493, - "y": 4.156084903648504 + "x": 8.31703315828284, + "y": 4.128036518181299 }, "prevControl": { - "x": 6.696293693339267, - "y": 7.468608019922806 + "x": 6.856231038683613, + "y": 7.4405596344556 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path index 309dbad..535d48e 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.157095812938493, - "y": 4.156084903648504 + "x": 8.31703315828284, + "y": 4.128036518181299 }, "prevControl": null, "nextControl": { - "x": 5.770433195004229, - "y": 2.9113169003249295 + "x": 5.930370540348576, + "y": 2.8832685148577237 }, "isLocked": false, "linkedName": "Ampside Auto Path 7 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path index 9e04b94..28316b5 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path @@ -8,27 +8,33 @@ }, "prevControl": null, "nextControl": { - "x": 7.303246686693876, - "y": 7.355447292348219 + "x": 8.23517407718576, + "y": 6.4902557155541665 }, "isLocked": false, "linkedName": "Ampside Auto path 3 Endpoint" }, { "anchor": { - "x": 8.149590620168896, - "y": 5.76090976743357 + "x": 8.281950694955519, + "y": 5.81199475789265 }, "prevControl": { - "x": 7.344396042175544, - "y": 6.151828644509419 + "x": 8.23517407718576, + "y": 6.127736927838529 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.15, + "rotationDegrees": -90.0, + "rotateFast": true + } + ], "constraintZones": [], "eventMarkers": [ { @@ -57,7 +63,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -26.70661318450872, + "rotation": -90.96093859731644, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path index bba57dc..cd7f587 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path @@ -8,27 +8,33 @@ }, "prevControl": null, "nextControl": { - "x": 7.087212570415116, - "y": 5.658036378729399 + "x": 8.23517407718576, + "y": 5.063568873576495 }, "isLocked": false, "linkedName": "Ampside Auto Path 5 Endpoint" }, { "anchor": { - "x": 8.157095812938493, - "y": 4.156084903648504 + "x": 8.31703315828284, + "y": 4.128036518181299 }, "prevControl": { - "x": 7.107787248155949, - "y": 4.516141764113103 + "x": 8.281950694955519, + "y": 4.6542734680910955 }, "nextControl": null, "isLocked": false, "linkedName": "Ampside Auto Path 7 Endpoint" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": -90.0, + "rotateFast": false + } + ], "constraintZones": [], "eventMarkers": [ { @@ -57,7 +63,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -21.54097591853849, + "rotation": -91.64239797493919, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path index a731257..7d0aec7 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 3.path @@ -17,11 +17,11 @@ { "anchor": { "x": 8.241041962510971, - "y": 2.4150717815077387 + "y": 2.4440782784662045 }, "prevControl": { "x": 7.24774631043924, - "y": 0.13633469734318018 + "y": 0.16534119430164596 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path index a9146e2..e353772 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 4.path @@ -4,12 +4,12 @@ { "anchor": { "x": 8.241041962510971, - "y": 2.4150717815077387 + "y": 2.4440782784662045 }, "prevControl": null, "nextControl": { "x": 6.517381860386496, - "y": -0.01947638533473953 + "y": 0.0095301116237263 }, "isLocked": false, "linkedName": "Sourceside Auto path 3 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path index bc5fad5..657a9d1 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path @@ -4,12 +4,12 @@ { "anchor": { "x": 8.241041962510971, - "y": 2.4150717815077387 + "y": 2.4440782784662045 }, "prevControl": null, "nextControl": { - "x": 6.14733053902644, - "y": 3.40836743357947 + "x": 8.23517407718576, + "y": 2.8884561472826653 }, "isLocked": false, "linkedName": "Sourceside Auto path 3 Endpoint" @@ -20,8 +20,8 @@ "y": 4.119255498297474 }, "prevControl": { - "x": 6.770574869738114, - "y": 3.8368479109437468 + "x": 8.25856238607064, + "y": 3.2392807805558634 }, "nextControl": null, "isLocked": false, @@ -30,9 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.4, - "rotationDegrees": 25.624492662715717, - "rotateFast": false + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0, + "rotateFast": true } ], "constraintZones": [], @@ -56,20 +56,20 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, + "maxVelocity": 4.5, "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 700.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 20.28255908891649, + "rotation": 90.67246599163494, "rotateFast": false }, "reversed": false, "folder": "Source Side", "previewStartingState": { - "rotation": 0, + "rotation": 87.76461147672893, "velocity": 0 }, "useDefaultConstraints": false diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path index ad6467b..05816e4 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.614763787060197, - "y": 1.363346973431787 + "x": 8.293644849397962, + "y": 1.567016695286953 }, "isLocked": false, "linkedName": "Loadside Auto Path 1 Endpoint" @@ -17,11 +17,11 @@ { "anchor": { "x": 8.241041962510971, - "y": 2.4150717815077387 + "y": 2.4440782784662045 }, "prevControl": { - "x": 6.799789447745093, - "y": 2.200831542820732 + "x": 8.25856238607064, + "y": 1.9351536710761752 }, "nextControl": null, "isLocked": false, @@ -30,9 +30,9 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.55, - "rotationDegrees": 32.61473904001839, - "rotateFast": false + "waypointRelativePos": 0.0, + "rotationDegrees": 90.0, + "rotateFast": true } ], "constraintZones": [], @@ -56,14 +56,14 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, + "maxVelocity": 4.5, "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, + "maxAngularVelocity": 700.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 89.47604771489111, "rotateFast": false }, "reversed": false, From 69316fa4dd0f7802593df609509bb9acdd654956 Mon Sep 17 00:00:00 2001 From: DriverStation1714 Date: Thu, 11 Apr 2024 19:26:18 -0500 Subject: [PATCH 65/82] tuned ampside auto --- .../deploy/pathplanner/paths/Ampside Auto path 1.path | 10 +++++----- .../deploy/pathplanner/paths/Ampside Auto path 2.path | 8 ++++---- .../pathplanner/paths/Ampside Missed Note 1.path | 8 ++++---- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path index 6b245e5..e8eacdb 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.54, - "y": 7.03 + "x": 2.63367409925703, + "y": 6.876162812154685 }, "prevControl": { - "x": 1.624635630223642, - "y": 6.789643654475279 + "x": 1.7183097294806722, + "y": 6.635806466629964 }, "nextControl": null, "isLocked": false, @@ -57,7 +57,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 12.80426606528676, + "rotation": 28.395068251547436, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path index c031971..8625268 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.54, - "y": 7.03 + "x": 2.63367409925703, + "y": 6.876162812154685 }, "prevControl": null, "nextControl": { - "x": 1.9581156423368469, - "y": 6.9328152351026535 + "x": 2.0517897415938773, + "y": 6.778978047257338 }, "isLocked": false, "linkedName": "Ampside Auto Path 1 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path index b51bb0a..343a29b 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.54, - "y": 7.03 + "x": 2.63367409925703, + "y": 6.876162812154685 }, "prevControl": null, "nextControl": { - "x": 3.54, - "y": 7.03 + "x": 3.63367409925703, + "y": 6.876162812154685 }, "isLocked": false, "linkedName": "Ampside Auto Path 1 Endpoint" From 8c6898795ff465d52d6f8f733274a3d46fb78657 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 11 Apr 2024 19:54:15 -0500 Subject: [PATCH 66/82] P value changed and current limit name for loader --- src/main/java/frc/robot/subsystems/Shooter.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index f7694d1..edc5af4 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -43,17 +43,17 @@ public class Shooter extends SubsystemBase { // left shooter motor PID - private final double lShooterMotorPGains = 0.05; + private final double lShooterMotorPGains = 0.2; private final double lShooterMotorIGains = 0.0; private final double lShooterMotorDGains = 0.0; private final double lShooterMotorSGains = 0.0; private final double lShooterMotorVGains = 0.12; // right shooter motor PID - private final double rShooterMotorPGains = 0.05; + private final double rShooterMotorPGains = 0.2; private final double rShooterMotorIGains = 0.0; private final double rShooterMotorDGains = 0.0; - private final int m_CurrentLimit = 40; + private final int m_LoaderCurrentLimit = 40; private final double rShooterMotorSGains = 0.0; private final double rShooterMotorVGains = 0.12; @@ -89,7 +89,7 @@ public Shooter() { configF.apply( new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) - .withSupplyCurrentLimit(m_CurrentLimit) + .withSupplyCurrentLimit(m_LoaderCurrentLimit) ); // right shooter motor configuration @@ -241,6 +241,8 @@ public void periodic() { // log shooting data SmartDashboard.putNumber("Right Shooter Speed", m_rightShooter.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Speed", m_leftShooter.getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("Right Shooter Voltage", m_rightShooter.getMotorVoltage().getValue()); + SmartDashboard.putNumber("Left Shooter Voltage", m_leftShooter.getMotorVoltage().getValue()); SmartDashboard.putNumber("Right Shooter Temp", m_rightShooter.getDeviceTemp().getValueAsDouble()); SmartDashboard.putNumber("Left Shooter Temp", m_leftShooter.getDeviceTemp().getValueAsDouble()); SmartDashboard.putBoolean("Break Beam Sensor", getBreakBeamOutput()); @@ -254,5 +256,8 @@ public void periodic() { SmartDashboard.putBoolean("debug/Break Beam Sensor", getBreakBeamOutput()); SmartDashboard.putNumber("debug/Right Shooter Current", m_rightShooter.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("debug/Left Shooter Current", m_leftShooter.getSupplyCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Right Shooter Stator Current", m_rightShooter.getStatorCurrent().getValueAsDouble()); + SmartDashboard.putNumber("Left Shooter Stator Current", m_leftShooter.getStatorCurrent().getValueAsDouble()); + } } From 04a04575973f3143ae046cc05bdc563fed256938 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Fri, 12 Apr 2024 18:36:36 -0500 Subject: [PATCH 67/82] Added method to determine when robot is at correct trap position --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++++-- src/main/java/frc/robot/subsystems/Eyes.java | 17 +++++++++++++++++ 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e9bc3fb..9cb4ee7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -433,8 +433,14 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new ConditionalCommand(new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), new InstantCommand(), () -> s_Eyes.closeToTrap)) //TODO Test this, was only running on init earlier, may need to be run command - .onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble + driverStart.whileTrue(new ConditionalCommand( + new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), + new InstantCommand(), + () -> s_Eyes.closeToTrap) + .until(() -> s_Eyes.atTrap()) + .andThen(new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)))) //TODO Test this, was only running on init earlier, may need to be run command + .onFalse(new ParallelCommandGroup(s_Swerve.getDefaultCommand(), + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)))); //TODO let driver know we are in position to trap via rumble //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index bd10e3c..3af3574 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -511,6 +511,23 @@ public Pose2d getClosestTrap() { return closestTrap; } + public boolean atTrap() { + + double trapTranslationTolerance = 3.0; + double trapRotationTolerance = 1.0; + + double xError = Math.abs(getRobotPose().getX() - getClosestTrap().getX()); + double yError = Math.abs(getRobotPose().getY() - getClosestTrap().getY()); + double rError = Math.abs(getRobotPose().getRotation().getDegrees() - getClosestTrap().getRotation().getDegrees()); + + if (trapTranslationTolerance >= xError && trapTranslationTolerance >= yError && trapRotationTolerance >= rError) { + return true; + } else { + return false; + } + + } + public PathPlannerPath closestTrapPath() { From 7b0e31709ee633dbb2a1a776996d8260877390ef Mon Sep 17 00:00:00 2001 From: Driver Station Date: Fri, 12 Apr 2024 21:44:26 -0500 Subject: [PATCH 68/82] fixed trap path generation --- src/main/java/frc/robot/Constants.java | 8 +++---- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++++------- src/main/java/frc/robot/subsystems/Eyes.java | 15 +++++++++++-- .../java/frc/robot/subsystems/Swerve.java | 11 ++++++++-- 4 files changed, 40 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8a9cf00..3a19589 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -197,7 +197,7 @@ public static final class Positions { public static final double ampRedY = 8.0; public static final double ampRedR = 270; - public static final double distanceLimit = 100.0; //TODO make safe distance + public static final double distanceLimit = 3.0; // blue trap positions public static final double blueTrapLeftX = 4.3; @@ -216,15 +216,15 @@ public static final class Positions { // red trap positions public static final double redTrapLeftX = 12.3; public static final double redTrapLeftY = 3.0; - public static final double redTrapLeftR = 121.64; + public static final double redTrapLeftR = 121.64 + 180; public static final double redTrapRightX = 12.3; public static final double redTrapRightY = 5.0; - public static final double redTrapRightR = -121.64; + public static final double redTrapRightR = -121.64 + 180; public static final double redTrapCenterX = 10.4; public static final double redTrapCenterY = 4.1; - public static final double redTrapCenterR = 0.0; + public static final double redTrapCenterR = 0.0 + 180; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9cb4ee7..2ee8d70 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.PathPlannerTrajectory; import edu.wpi.first.math.geometry.Pose2d; @@ -433,14 +434,19 @@ private void configureButtonBindings() { ); // generate and run path to closest trap - driverStart.whileTrue(new ConditionalCommand( - new InstantCommand(() -> s_Swerve.onTheFly(s_Eyes.closestTrapPath())), - new InstantCommand(), - () -> s_Eyes.closeToTrap) - .until(() -> s_Eyes.atTrap()) - .andThen(new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)))) //TODO Test this, was only running on init earlier, may need to be run command - .onFalse(new ParallelCommandGroup(s_Swerve.getDefaultCommand(), - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)))); //TODO let driver know we are in position to trap via rumble + driverStart.whileTrue(new ConditionalCommand(new InstantCommand(() -> { + s_Swerve.onTheFly(() -> s_Eyes.trapPath).schedule(); + s_Eyes.limelight.setLEDMode_ForceOff(""); + driver.setRumble(RumbleType.kBothRumble, 0); + }), + new InstantCommand(() -> { + s_Eyes.limelight.setLEDMode_ForceBlink(""); + driver.setRumble(RumbleType.kBothRumble, 1); + }), + () -> s_Eyes.closeToTrap) + // .until(() -> s_Eyes.atTrap()) + // .andThen(new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)))) //TODO Test this, was only running on init earlier, may need to be run command + ).onFalse(s_Swerve.getDefaultCommand()); //TODO let driver know we are in position to trap via rumble //Feed if (DriverStation.getAlliance().get() == Alliance.Blue) { diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 3af3574..4d9ed2e 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -18,6 +18,8 @@ import java.util.List; import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.commands.PathfindHolonomic; +import com.pathplanner.lib.commands.PathfindingCommand; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; @@ -62,10 +64,12 @@ public class Eyes extends SubsystemBase { public double tID; private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times private StructPublisher posePublisher; + // private StructPublisher trapPathCurrentPosePub; private StructPublisher translationPublisher; private StructPublisher trapPublisher; public boolean controllerRumble = false; public boolean closeToTrap = false; + public PathPlannerPath trapPath; // constuctor public Eyes(Swerve swerve, Shooter shooter) { @@ -73,8 +77,10 @@ public Eyes(Swerve swerve, Shooter shooter) { posePublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal pose", Pose2d.struct).publish(); translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal translation", Translation2d.struct).publish(); trapPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); + // trapPathCurrentPosePub = NetworkTableInstance.getDefault().getStructTopic("/Trap Path Current Pose", Pose2d.struct).publish(); s_Swerve = swerve; s_Shooter = shooter; + trapPath = closestTrapPath(); } @@ -538,7 +544,7 @@ public PathPlannerPath closestTrapPath() { List bezierPoints = PathPlannerPath.bezierFromPoses( - new Pose2d(currentPose.getX(), currentPose.getY(), currentPose.minus(closestTrap).getRotation()), //TODO Rotation may be wrong + new Pose2d(currentPose.getX(), currentPose.getY(), closestTrap.minus(currentPose).getRotation()), //TODO Rotation may be wrong new Pose2d(closestTrap.getX(), closestTrap.getY(), closestTrap.minus(currentPose).getRotation()) //TODO Rotation may be wrong ); @@ -649,6 +655,8 @@ public void updatePoseEstimatorWithVisionBotPoseMegatag2() { } } + //public Command onTheFly = new PathfindHolonomic(getClosestTrap(), null, null, null, null, null, null) + @Override public void periodic() { s_Swerve.m_poseEstimator.update(s_Swerve.getGyroYaw(), s_Swerve.getModulePositions()); @@ -663,8 +671,10 @@ public void periodic() { Timer.getFPGATimestamp() - (LimelightHelpers.getLatency_Pipeline("")/1000.0) - (LimelightHelpers.getLatency_Capture("")/1000.0) ); } - + trapPath = closestTrapPath(); + trapPath.getPathPoses().get(0); + SmartDashboard.putNumber("Pose estimator rotations", s_Swerve.m_poseEstimator.getEstimatedPosition().getRotation().getDegrees()); SmartDashboard.putNumber("Pose Estimator X", s_Swerve.m_poseEstimator.getEstimatedPosition().getX()); SmartDashboard.putNumber("Pose Estimator Y", s_Swerve.m_poseEstimator.getEstimatedPosition().getY()); @@ -674,6 +684,7 @@ public void periodic() { posePublisher.set(getMovingTarget()); + // trapPathCurrentPosePub.set(trapPath.getPathPoses().get(0)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index f763c2a..61b5048 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import java.util.List; +import java.util.function.Supplier; import com.ctre.phoenix6.configs.Pigeon2Configuration; import com.ctre.phoenix6.hardware.Pigeon2; @@ -76,6 +77,7 @@ public class Swerve extends SubsystemBase { private StructPublisher posePublisher; public StructArrayPublisher swerveKinematicsPublisher; public StructPublisher estimatedRobotPosePublisher; + private StructPublisher trapPathCurrentPosePub; public SwerveDrivePoseEstimator m_poseEstimator; public FieldRelativeSpeed fieldRelativeVelocity; public FieldRelativeSpeed lastFieldRelativeVelocity; @@ -116,6 +118,7 @@ public Swerve() { posePublisher = NetworkTableInstance.getDefault().getStructTopic("/MyPose", Pose2d.struct).publish(); swerveKinematicsPublisher = NetworkTableInstance.getDefault().getStructArrayTopic("/SwerveModuleStates", SwerveModuleState.struct).publish(); estimatedRobotPosePublisher = NetworkTableInstance.getDefault().getStructTopic("/EstimatedRobotPose", Pose2d.struct).publish(); + trapPathCurrentPosePub = NetworkTableInstance.getDefault().getStructTopic("/Trap Path Current Pose", Pose2d.struct).publish(); posePublisher = NetworkTableInstance.getDefault() .getStructTopic("RobotPose", Pose2d.struct).publish(); @@ -299,11 +302,15 @@ public void resetModulesToAbsolute(){ } } - public Command onTheFly(PathPlannerPath path) { + public Command onTheFly(Supplier pathSupplier) { + PathPlannerPath path = pathSupplier.get(); + + trapPathCurrentPosePub.set(path.getPathPoses().get(0)); + return new FollowPathHolonomic( path, - this::getPose, // Robot pose supplier + this::getEstimatedPose, // Robot pose supplier this::getChassisSpeed, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE this::setChassisSpeed, // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in your Constants class From 5dc01f1633ba3b76766f6622e7a8532780bdbca1 Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sat, 13 Apr 2024 14:40:57 -0500 Subject: [PATCH 69/82] Tommy: Added midline auto --- .../pathplanner/autos/Midline Auto.auto | 103 ++++++++++++++++ .../pathplanner/paths/Midline Path 1.path | 113 ++++++++++++++++++ .../pathplanner/paths/Midline Path 2.path | 92 ++++++++++++++ .../pathplanner/paths/Midline Path 3.path | 87 ++++++++++++++ .../pathplanner/paths/Midline Path 4.path | 87 ++++++++++++++ .../pathplanner/paths/Midline Path 5.path | 87 ++++++++++++++ 6 files changed, 569 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Midline Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Midline Path 1.path create mode 100644 src/main/deploy/pathplanner/paths/Midline Path 2.path create mode 100644 src/main/deploy/pathplanner/paths/Midline Path 3.path create mode 100644 src/main/deploy/pathplanner/paths/Midline Path 4.path create mode 100644 src/main/deploy/pathplanner/paths/Midline Path 5.path diff --git a/src/main/deploy/pathplanner/autos/Midline Auto.auto b/src/main/deploy/pathplanner/autos/Midline Auto.auto new file mode 100644 index 0000000..62f19b8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Midline Auto.auto @@ -0,0 +1,103 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4055808813372015, + "y": 5.537433881885308 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 1" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 2" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 3" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 4" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + }, + { + "type": "path", + "data": { + "pathName": "Midline Path 5" + } + }, + { + "type": "named", + "data": { + "name": "Confirm Intake" + } + }, + { + "type": "named", + "data": { + "name": "AutoScore" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 1.path b/src/main/deploy/pathplanner/paths/Midline Path 1.path new file mode 100644 index 0000000..1714b57 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 1.path @@ -0,0 +1,113 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.4055808813372015, + "y": 5.537433881885308 + }, + "prevControl": null, + "nextControl": { + "x": 1.5325365738450776, + "y": 4.748494935586363 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9290491914317163, + "y": 4.830109309341426 + }, + "prevControl": { + "x": 1.9290491914317163, + "y": 4.830109309341426 + }, + "nextControl": { + "x": 3.929049191431712, + "y": 4.830109309341426 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.014749854061112, + "y": 4.648744034330174 + }, + "prevControl": { + "x": 4.684950271711083, + "y": 4.87619202215778 + }, + "nextControl": { + "x": 5.540709151593742, + "y": 4.286013484307671 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.33373438676702, + "y": 4.095579945545856 + }, + "prevControl": { + "x": 7.675738269587725, + "y": 4.095579945545856 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 1 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.9, + "rotationDegrees": -26.07474124855111, + "rotateFast": false + }, + { + "waypointRelativePos": 0.95, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 2.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 2.path b/src/main/deploy/pathplanner/paths/Midline Path 2.path new file mode 100644 index 0000000..fc658e2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 2.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 8.33373438676702, + "y": 4.095579945545856 + }, + "prevControl": null, + "nextControl": { + "x": 6.982563087931663, + "y": 4.33135480306423 + }, + "isLocked": false, + "linkedName": "Midline Path 1 Endpoint" + }, + { + "anchor": { + "x": 4.579473194032577, + "y": 4.694085353082987 + }, + "prevControl": { + "x": 5.538247561809932, + "y": 4.23890964312408 + }, + "nextControl": { + "x": 3.6817150827268805, + "y": 5.120293749363175 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8045844863619553, + "y": 4.694085353082987 + }, + "prevControl": { + "x": 2.9018444001800283, + "y": 4.612470979327924 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 2 Endpoint" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.85, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.35, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -35.21759296819273, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 3.path b/src/main/deploy/pathplanner/paths/Midline Path 3.path new file mode 100644 index 0000000..122ee8e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 3.path @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8045844863619553, + "y": 4.694085353082987 + }, + "prevControl": null, + "nextControl": { + "x": 2.1219737176316458, + "y": 4.49458355057061 + }, + "isLocked": false, + "linkedName": "Midline Path 2 Endpoint" + }, + { + "anchor": { + "x": 2.6513193714406413, + "y": 4.2361138103059135 + }, + "prevControl": { + "x": 2.3339301401709505, + "y": 4.535366514074479 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 3 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.0, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -31.639920128540666, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -36.027373385103694, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 4.path b/src/main/deploy/pathplanner/paths/Midline Path 4.path new file mode 100644 index 0000000..b780953 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 4.path @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6513193714406413, + "y": 4.2361138103059135 + }, + "prevControl": null, + "nextControl": { + "x": 1.8227210138630805, + "y": 4.802904518089738 + }, + "isLocked": false, + "linkedName": "Midline Path 3 Endpoint" + }, + { + "anchor": { + "x": 2.6513193714406413, + "y": 5.519297354384182 + }, + "prevControl": { + "x": 1.5994007763753808, + "y": 5.392341661876307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Midline Path 4 Endpoint" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.45, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -2.3552351424074494, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -31.653238844011977, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 5.path b/src/main/deploy/pathplanner/paths/Midline Path 5.path new file mode 100644 index 0000000..83456a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Midline Path 5.path @@ -0,0 +1,87 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.6513193714406413, + "y": 5.519297354384182 + }, + "prevControl": null, + "nextControl": { + "x": 1.8714486888922583, + "y": 6.135939289422439 + }, + "isLocked": false, + "linkedName": "Midline Path 4 Endpoint" + }, + { + "anchor": { + "x": 2.6513193714406413, + "y": 6.870468653218008 + }, + "prevControl": { + "x": 1.9530630626473218, + "y": 6.670966850705632 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.55, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 24.304549265936778, + "rotateFast": false + }, + "reversed": false, + "folder": "Midline", + "previewStartingState": { + "rotation": -0.5809166420702049, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 9dc520db16259f21102c827d68a3a89d5308d02f Mon Sep 17 00:00:00 2001 From: ewegner001 <88629283+ewegner001@users.noreply.github.com> Date: Sat, 13 Apr 2024 14:41:22 -0500 Subject: [PATCH 70/82] updated settings --- .pathplanner/settings.json | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 4c3a845..6b4d937 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,9 +4,10 @@ "holonomicMode": true, "pathFolders": [ "Amp Side", + "Midline", "Four Note", - "Two Note", - "Source Side" + "Source Side", + "Two Note" ], "autoFolders": [], "defaultMaxVel": 3.0, From 784c39a41226e3e1decf5f2443b687971d5f78c4 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Sat, 13 Apr 2024 18:56:24 -0500 Subject: [PATCH 71/82] Auto chNGES --- .pathplanner/settings.json | 4 +- .../autos/Prototype Ampside Smart Auto.auto | 68 ++----------------- .../Prototype Sourceside Smart Auto.auto | 6 -- .../paths/Ampside Auto Path 6.path | 14 ++-- .../paths/Ampside Auto Path 7.path | 18 ++--- .../paths/Ampside Auto Path 8.path | 4 +- .../paths/Ampside Auto path 1.path | 2 +- .../paths/Ampside Auto path 2.path | 4 +- .../paths/Ampside Auto path 4.path | 14 ++-- .../paths/Ampside Auto path 5.path | 8 +-- .../paths/Ampside Missed Note 1.path | 6 +- .../paths/Ampside Missed Note 2.path | 6 +- .../paths/Ampside Missed Note 3.path | 6 +- .../pathplanner/paths/Ampside Smart 1.path | 4 +- .../paths/Four Piece Auto Path 1.path | 4 +- .../paths/Four Piece Auto Path 2.path | 4 +- .../paths/Four Piece Auto Path 3.path | 4 +- .../pathplanner/paths/Midline Path 1.path | 49 ++++++------- .../pathplanner/paths/Midline Path 2.path | 26 +++---- .../pathplanner/paths/Midline Path 3.path | 21 +----- .../pathplanner/paths/Midline Path 4.path | 35 +++------- .../pathplanner/paths/Midline Path 5.path | 39 +++-------- .../paths/Position Locator (DO NOT USE).path | 12 ++-- .../paths/Sourceside Auto Path 5.path | 4 +- .../Sourceside missed Note 2 Path 2.path | 6 +- .../paths/Sourceside missed Note 2.path | 2 +- .../paths/Sourceside missed note 1.path | 2 +- src/main/java/frc/robot/Constants.java | 10 +-- src/main/java/frc/robot/RobotContainer.java | 15 +++- .../java/frc/robot/commands/PrepareFeed.java | 48 +++++++++++++ .../java/frc/robot/commands/Shooting.java | 52 ++++++++++++++ src/main/java/frc/robot/subsystems/Eyes.java | 7 +- .../java/frc/robot/subsystems/Intake.java | 4 +- 33 files changed, 246 insertions(+), 262 deletions(-) create mode 100644 src/main/java/frc/robot/commands/PrepareFeed.java create mode 100644 src/main/java/frc/robot/commands/Shooting.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 6b4d937..db52386 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,8 +10,8 @@ "Two Note" ], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 5.57, + "defaultMaxAccel": 5.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto index 99bb9da..d60f847 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Ampside Smart Auto.auto @@ -32,77 +32,19 @@ { "type": "named", "data": { - "name": "Check Note" + "name": "AutoScore" } }, { - "type": "race", + "type": "path", "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AutoScore" - } - }, - { - "type": "path", - "data": { - "pathName": "Ampside Auto path 3" - } - }, - { - "type": "named", - "data": { - "name": "Confirm Intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Got Note" - } - } - ] + "pathName": "Ampside Missed Note 1" } }, { - "type": "race", + "type": "named", "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Ampside Missed Note 1" - } - }, - { - "type": "named", - "data": { - "name": "Confirm Intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Not Got Note" - } - } - ] + "name": "Confirm Intake" } }, { diff --git a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto index e96f2bb..2a4c4b5 100644 --- a/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto +++ b/src/main/deploy/pathplanner/autos/Prototype Sourceside Smart Auto.auto @@ -163,12 +163,6 @@ "name": "AutoScore" } }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path index 86a82be..95cfe81 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 6.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 4.33469113851808, - "y": 6.629350542651702 + "x": 4.881365492268351, + "y": 7.1867861885189965 }, "isLocked": false, "linkedName": "Ampside Auto Path 5 Endpoint" }, { "anchor": { - "x": 1.9863466887427603, - "y": 6.610786554906443 + "x": 2.583152022769052, + "y": 6.77778209648946 }, "prevControl": { - "x": 4.0469493284665585, - "y": 6.6943244997601115 + "x": 5.709111868994794, + "y": 7.128357032514777 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 29.47588900324585, + "rotation": 22.83365417791754, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path index 2d7f780..3cac3cc 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 7.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.9863466887427603, - "y": 6.610786554906443 + "x": 2.583152022769052, + "y": 6.77778209648946 }, "prevControl": null, "nextControl": { - "x": 2.9863466887427603, - "y": 6.610786554906443 + "x": 3.5667094821734135, + "y": 6.923854986500009 }, "isLocked": false, "linkedName": "Ampside Auto Path 6 Endpoint" @@ -20,8 +20,8 @@ "y": 4.128036518181299 }, "prevControl": { - "x": 6.856231038683613, - "y": 7.4405596344556 + "x": 7.40355739311716, + "y": 7.498408353874834 }, "nextControl": null, "isLocked": false, @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.75, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -66,5 +66,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path index 535d48e..3118204 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path index e8eacdb..943b52a 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 1.path @@ -51,7 +51,7 @@ ], "globalConstraints": { "maxVelocity": 5.75, - "maxAcceleration": 3.5, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path index 8625268..bbc2ba8 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path index 7a33bdb..e70babb 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 4.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.6555989865066145, - "y": 7.121296217901078 + "x": 6.361570777708579, + "y": 7.225738959188476 }, "isLocked": false, "linkedName": "Ampside Auto path 3 Endpoint" }, { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 2.85582141745541, + "y": 7.001760527838968 }, "prevControl": { - "x": 4.438379168232951, - "y": 6.949241269692627 + "x": 5.611729942321095, + "y": 7.177047995851627 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 46.81201520995728, + "rotation": 24.1791071466892, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path index 7b5b094..3e7e0b1 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.69, - "y": 6.86 + "x": 2.85582141745541, + "y": 7.001760527838968 }, "prevControl": null, "nextControl": { - "x": 2.69, - "y": 6.86 + "x": 3.85582141745541, + "y": 7.001760527838968 }, "isLocked": false, "linkedName": "Ampside Auto Path 4 Endpoint" diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path index 343a29b..cee352c 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 1.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.75, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, @@ -66,5 +66,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path index 28316b5..f441d48 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.4, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path index cd7f587..2a363f5 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.35, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Smart 1.path b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path index 65a456b..8d417ec 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Smart 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path index 84a8fbf..f6ee485 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path index c709ce5..340987e 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path @@ -72,8 +72,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path index 490c82b..43b5b65 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path @@ -66,8 +66,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 1.path b/src/main/deploy/pathplanner/paths/Midline Path 1.path index 1714b57..47f0a8e 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 1.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 1.path @@ -16,44 +16,28 @@ }, { "anchor": { - "x": 2.9290491914317163, - "y": 4.830109309341426 + "x": 2.8947741881248894, + "y": 4.898310911687067 }, "prevControl": { - "x": 1.9290491914317163, - "y": 4.830109309341426 + "x": 1.9053384691709299, + "y": 4.753338645173666 }, "nextControl": { - "x": 3.929049191431712, - "y": 4.830109309341426 + "x": 5.553300786316876, + "y": 5.287838618381864 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.014749854061112, - "y": 4.648744034330174 + "x": 8.367638467186781, + "y": 4.099779112962734 }, "prevControl": { - "x": 4.684950271711083, - "y": 4.87619202215778 - }, - "nextControl": { - "x": 5.540709151593742, - "y": 4.286013484307671 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.33373438676702, - "y": 4.095579945545856 - }, - "prevControl": { - "x": 7.675738269587725, - "y": 4.095579945545856 + "x": 3.8732829572795318, + "y": 3.7826179223564056 }, "nextControl": null, "isLocked": false, @@ -62,7 +46,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 1.9, + "waypointRelativePos": 1.45, "rotationDegrees": -26.07474124855111, "rotateFast": false }, @@ -70,13 +54,18 @@ "waypointRelativePos": 0.95, "rotationDegrees": 0, "rotateFast": false + }, + { + "waypointRelativePos": 1.8, + "rotationDegrees": 0, + "rotateFast": true } ], "constraintZones": [], "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 2.35, + "waypointRelativePos": 1.55, "command": { "type": "parallel", "data": { @@ -93,8 +82,8 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 2.path b/src/main/deploy/pathplanner/paths/Midline Path 2.path index fc658e2..251b40c 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 2.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.33373438676702, - "y": 4.095579945545856 + "x": 8.367638467186781, + "y": 4.099779112962734 }, "prevControl": null, "nextControl": { - "x": 6.982563087931663, - "y": 4.33135480306423 + "x": 7.016467168351424, + "y": 4.335553970481109 }, "isLocked": false, "linkedName": "Midline Path 1 Endpoint" @@ -20,12 +20,12 @@ "y": 4.694085353082987 }, "prevControl": { - "x": 5.538247561809932, - "y": 4.23890964312408 + "x": 5.486823135797703, + "y": 4.14349539819615 }, "nextControl": { - "x": 3.6817150827268805, - "y": 5.120293749363175 + "x": 3.761473335520811, + "y": 5.190456691708164 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.694085353082987 }, "prevControl": { - "x": 2.9018444001800283, - "y": 4.612470979327924 + "x": 2.923988766126999, + "y": 4.8009289850133685 }, "nextControl": null, "isLocked": false, @@ -72,14 +72,14 @@ } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, "goalEndState": { "velocity": 0, - "rotation": -35.21759296819273, + "rotation": -14.892153959046919, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 3.path b/src/main/deploy/pathplanner/paths/Midline Path 3.path index 122ee8e..b6b3dbb 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 3.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 3.path @@ -47,28 +47,11 @@ ] } } - }, - { - "name": "New Event Marker", - "waypointRelativePos": 0.7, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - } - ] - } - } } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 4.path b/src/main/deploy/pathplanner/paths/Midline Path 4.path index b780953..8bdda4a 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 4.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 4.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.8227210138630805, - "y": 4.802904518089738 + "x": 1.5119508293583617, + "y": 4.742499829009148 }, "isLocked": false, "linkedName": "Midline Path 3 Endpoint" }, { "anchor": { - "x": 2.6513193714406413, - "y": 5.519297354384182 + "x": 2.85582141745541, + "y": 5.62867536173981 }, "prevControl": { - "x": 1.5994007763753808, - "y": 5.392341661876307 + "x": 1.5022126366909916, + "y": 5.356005967053452 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.2, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { @@ -47,28 +47,11 @@ ] } } - }, - { - "name": "New Event Marker", - "waypointRelativePos": 0.45, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - } - ] - } - } } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 5.path b/src/main/deploy/pathplanner/paths/Midline Path 5.path index 83456a6..f94e7d8 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 5.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.6513193714406413, - "y": 5.519297354384182 + "x": 2.85582141745541, + "y": 5.62867536173981 }, "prevControl": null, "nextControl": { - "x": 1.8714486888922583, - "y": 6.135939289422439 + "x": 1.8820021507184188, + "y": 6.302868543802893 }, "isLocked": false, "linkedName": "Midline Path 4 Endpoint" }, { "anchor": { - "x": 2.6513193714406413, - "y": 6.870468653218008 + "x": 2.7779158761164506, + "y": 6.962807757169489 }, "prevControl": { - "x": 1.9530630626473218, - "y": 6.670966850705632 + "x": 1.7554056460426093, + "y": 6.553803665139952 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.2, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { @@ -47,28 +47,11 @@ ] } } - }, - { - "name": "New Event Marker", - "waypointRelativePos": 0.55, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Aim" - } - } - ] - } - } } ], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path index 5f7fe3f..87c14ec 100644 --- a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path +++ b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 10.4, - "y": 4.08519490024762 + "x": 4.1, + "y": 2.8 }, "prevControl": null, "nextControl": { - "x": 11.400000000000006, - "y": 4.08519490024762 + "x": 5.100000000000005, + "y": 2.8 }, "isLocked": false, "linkedName": null @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.57, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path index 25b84e9..fedf3ec 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path +++ b/src/main/deploy/pathplanner/paths/Sourceside Auto Path 5.path @@ -20,8 +20,8 @@ "y": 4.099779112962734 }, "prevControl": { - "x": 6.390785355710689, - "y": 1.5581108267791854 + "x": 5.825970181003233, + "y": 0.7206262573853753 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path index 5b01fd2..80d9dbb 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2 Path 2.path @@ -20,8 +20,8 @@ "y": 3.5933930942594987 }, "prevControl": { - "x": 4.248382968889307, - "y": 1.9963294968108325 + "x": 4.725554409590433, + "y": 1.6165399827834053 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.55, - "rotationDegrees": 0, + "rotationDegrees": -0.03070135861799204, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path index 657a9d1..91425bb 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed Note 2.path @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.1, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path index 05816e4..eec9ee7 100644 --- a/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path +++ b/src/main/deploy/pathplanner/paths/Sourceside missed note 1.path @@ -39,7 +39,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.3, + "waypointRelativePos": 0.0, "command": { "type": "parallel", "data": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3a19589..8277cdd 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -202,15 +202,15 @@ public static final class Positions { // blue trap positions public static final double blueTrapLeftX = 4.3; public static final double blueTrapLeftY = 5.0; - public static final double blueTrapLeftR = -60.0; + public static final double blueTrapLeftR = -60.0 + 180; - public static final double blueTrapRightX = 4.3; - public static final double blueTrapRightY = 3.0; - public static final double blueTrapRightR = 60.0; + public static final double blueTrapRightX = 4.25; + public static final double blueTrapRightY = 2.95; + public static final double blueTrapRightR = 60.0 + 180; public static final double blueTrapCenterX = 6.1; public static final double blueTrapCenterY = 4.1; - public static final double blueTrapCenterR = 180.0; + public static final double blueTrapCenterR = 180.0 + 180; // red trap positions diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5458738..d414200 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -178,7 +178,7 @@ public RobotContainer() { //NOTE: IF REMOVED NEED TO ADD SHOOTER SPINUP FOR AMP SCORE s_Shooter.setDefaultCommand( new ConditionalCommand( - new InstantCommand (() -> s_Shooter.shootingMotorsSetControl(0, 0), s_Shooter), + new InstantCommand (() -> s_Shooter.setShooterVoltage(0, 0), s_Shooter), new InstantCommand(() -> s_Shooter.shootingMotorsSetControl(35.0, 35.0), s_Shooter), //s_Shooter.shootingMotorsSetControl(20, 20) () -> s_Shooter.getBreakBeamOutput()) ); @@ -415,6 +415,9 @@ private void configureButtonBindings() { return true; } })); + //climb down more + driverDpadDown.onTrue( + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0))); // abort climb driverSelect.onTrue( @@ -462,7 +465,7 @@ private void configureButtonBindings() { () -> false, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) + ).alongWith(new Shooting(s_ShooterPivot,s_Shooter, 65.0, 117.0))//new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, false, false, true)) ).onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); } else { driverRStick.whileTrue(new TeleopSwerve( @@ -475,7 +478,7 @@ private void configureButtonBindings() { () -> false, rotationSpeed, true - ).alongWith(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 2.4))) + ).alongWith(new Shooting(s_ShooterPivot,s_Shooter, 65.0, 117.0))) .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)) @@ -538,6 +541,7 @@ private void configureButtonBindings() { ) ); + // dummy shoot commands operatorDpadDown.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 1.25)) .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); @@ -545,6 +549,11 @@ private void configureButtonBindings() { .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); operatorDpadUp.whileTrue(new AimShoot(s_Eyes, s_ShooterPivot, s_Shooter, 3.17)) .onFalse(new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition))); + //prepare feed + operatorA.whileTrue(new PrepareFeed(s_Shooter, 65.0)); + // prepare trap + operatorX.whileTrue(new Shooting(s_ShooterPivot, s_Shooter, 45.0, s_ShooterPivot.shooterPivotStowPosition)); + } diff --git a/src/main/java/frc/robot/commands/PrepareFeed.java b/src/main/java/frc/robot/commands/PrepareFeed.java new file mode 100644 index 0000000..be1635b --- /dev/null +++ b/src/main/java/frc/robot/commands/PrepareFeed.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Eyes; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterPivot; + + +public class PrepareFeed extends Command { + private Shooter shooter; + + private double shooterAngle; + private double shooterSpeed; + + public PrepareFeed(Shooter shooter, double shooterSpeed) { + + // Use addRequirements() here to declare subsystem dependencies. + this.shooter = shooter; + this.shooterSpeed = shooterSpeed; + addRequirements(shooter); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooter.shootingMotorsSetControl(shooterSpeed, shooterSpeed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/Shooting.java b/src/main/java/frc/robot/commands/Shooting.java new file mode 100644 index 0000000..46b0bf6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Shooting.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Elevator; +import frc.robot.subsystems.Eyes; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterPivot; + + +public class Shooting extends Command { + private ShooterPivot shooterPivot; + private Shooter shooter; + + private double shooterAngle; + private double shooterSpeed; + + public Shooting(ShooterPivot shooterPivot, Shooter shooter, double shooterSpeed, double shooterAngle) { + + // Use addRequirements() here to declare subsystem dependencies. + this.shooterPivot = shooterPivot; + this.shooter = shooter; + this.shooterSpeed = shooterSpeed; + this.shooterAngle = shooterAngle; + addRequirements(shooter,shooterPivot); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + shooter.shootingMotorsSetControl(shooterSpeed,shooterSpeed); + shooterPivot.moveShooterPivot(shooterAngle); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index 4d9ed2e..f2af8aa 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -64,7 +64,7 @@ public class Eyes extends SubsystemBase { public double tID; private double accelerationCompensation = 0.0; //Note this caused a ton of jitter due to inconsistent loop times private StructPublisher posePublisher; - // private StructPublisher trapPathCurrentPosePub; + private StructPublisher closestTrapPose; private StructPublisher translationPublisher; private StructPublisher trapPublisher; public boolean controllerRumble = false; @@ -77,7 +77,7 @@ public Eyes(Swerve swerve, Shooter shooter) { posePublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal pose", Pose2d.struct).publish(); translationPublisher = NetworkTableInstance.getDefault().getStructTopic("/Moving Goal translation", Translation2d.struct).publish(); trapPublisher = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap", Translation2d.struct).publish(); - // trapPathCurrentPosePub = NetworkTableInstance.getDefault().getStructTopic("/Trap Path Current Pose", Pose2d.struct).publish(); + closestTrapPose = NetworkTableInstance.getDefault().getStructTopic("/Closest Trap Pose", Pose2d.struct).publish(); s_Swerve = swerve; s_Shooter = shooter; trapPath = closestTrapPath(); @@ -513,6 +513,7 @@ public Pose2d getClosestTrap() { //Log Target Trap Location trapPublisher.set(new Translation2d(closestTrap.getX(), closestTrap.getY())); + closestTrapPose.set(closestTrap); return closestTrap; } @@ -551,7 +552,7 @@ public PathPlannerPath closestTrapPath() { PathPlannerPath path = new PathPlannerPath( bezierPoints, - new PathConstraints(0.25, 0.25, 2 * Math.PI, 4 * Math.PI), //TODO adjust speeds + new PathConstraints(3.0, 3.0, 2 * Math.PI, 4 * Math.PI), //TODO adjust speeds new GoalEndState(0.0, closestTrap.getRotation()) ); diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index e619e63..48d048a 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -30,8 +30,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 48.6; - public final double intakeGroundPosition = -54.58; + public final double intakeSafePosition = 327.5; + public final double intakeGroundPosition = 227.9; public final double intakeSourcePosition = intakeSafePosition; // PID values From 779244592cbe2e80420e4e17b55d9658e8414110 Mon Sep 17 00:00:00 2001 From: Dylan Powers Date: Sat, 13 Apr 2024 20:10:58 -0500 Subject: [PATCH 72/82] removed unnecessary variable --- src/main/java/frc/robot/subsystems/Shooter.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index e09bf95..db90ccf 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -32,7 +32,6 @@ public class Shooter extends SubsystemBase { public final double runLoaderVoltage = 12.0; public final double reverseLoaderVoltage = -3.0; public final double stopLoaderVoltage = 0.0; - private final int loaderCurrentLimit = 40; // shooter speeds public final double runShooterVoltage = 6.0; From ffc6063f9c4ccbab36af205c6190357959b559a1 Mon Sep 17 00:00:00 2001 From: Dylan Powers Date: Sun, 14 Apr 2024 21:38:24 -0500 Subject: [PATCH 73/82] reverted change in default max velocity and acceleration --- .pathplanner/settings.json | 4 ++-- src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path | 4 ++-- src/main/deploy/pathplanner/paths/Ampside Auto path 2.path | 4 ++-- src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path | 4 ++-- src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path | 4 ++-- src/main/deploy/pathplanner/paths/Ampside Smart 1.path | 4 ++-- src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path | 4 ++-- src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path | 4 ++-- src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path | 4 ++-- src/main/deploy/pathplanner/paths/Midline Path 1.path | 2 +- src/main/deploy/pathplanner/paths/Midline Path 2.path | 2 +- src/main/deploy/pathplanner/paths/Midline Path 3.path | 2 +- src/main/deploy/pathplanner/paths/Midline Path 4.path | 2 +- src/main/deploy/pathplanner/paths/Midline Path 5.path | 2 +- .../pathplanner/paths/Position Locator (DO NOT USE).path | 4 ++-- 15 files changed, 25 insertions(+), 25 deletions(-) diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index db52386..6b4d937 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,8 +10,8 @@ "Two Note" ], "autoFolders": [], - "defaultMaxVel": 5.57, - "defaultMaxAccel": 5.0, + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path index 3118204..535d48e 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto Path 8.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path index bbc2ba8..8625268 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Auto path 2.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path index f441d48..0377efe 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 2.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path index 2a363f5..e322524 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path +++ b/src/main/deploy/pathplanner/paths/Ampside Missed Note 3.path @@ -56,8 +56,8 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Ampside Smart 1.path b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path index 8d417ec..65a456b 100644 --- a/src/main/deploy/pathplanner/paths/Ampside Smart 1.path +++ b/src/main/deploy/pathplanner/paths/Ampside Smart 1.path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path index f6ee485..84a8fbf 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 1.path @@ -50,8 +50,8 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path index 340987e..c709ce5 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 2.path @@ -72,8 +72,8 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path index 43b5b65..490c82b 100644 --- a/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path +++ b/src/main/deploy/pathplanner/paths/Four Piece Auto Path 3.path @@ -66,8 +66,8 @@ } ], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, diff --git a/src/main/deploy/pathplanner/paths/Midline Path 1.path b/src/main/deploy/pathplanner/paths/Midline Path 1.path index 47f0a8e..1942edf 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 1.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 1.path @@ -98,5 +98,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 2.path b/src/main/deploy/pathplanner/paths/Midline Path 2.path index 251b40c..0d27c54 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 2.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 2.path @@ -88,5 +88,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 3.path b/src/main/deploy/pathplanner/paths/Midline Path 3.path index b6b3dbb..a9160f8 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 3.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 3.path @@ -66,5 +66,5 @@ "rotation": -36.027373385103694, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 4.path b/src/main/deploy/pathplanner/paths/Midline Path 4.path index 8bdda4a..e08b17b 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 4.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 4.path @@ -66,5 +66,5 @@ "rotation": -31.653238844011977, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Midline Path 5.path b/src/main/deploy/pathplanner/paths/Midline Path 5.path index f94e7d8..9b7d718 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 5.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 5.path @@ -66,5 +66,5 @@ "rotation": -0.5809166420702049, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path index 87c14ec..9d7f386 100644 --- a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path +++ b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path @@ -32,8 +32,8 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.57, - "maxAcceleration": 5.0, + "maxVelocity": 3.0, + "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 }, From eed785b70334289339930c119d27f7f38cc95f58 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Wed, 17 Apr 2024 18:39:03 -0500 Subject: [PATCH 74/82] practice day changes --- src/main/java/frc/robot/RobotContainer.java | 1 + src/main/java/frc/robot/commands/AimShoot.java | 8 ++++---- src/main/java/frc/robot/subsystems/Intake.java | 4 ++-- 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d414200..bdef3a5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -488,6 +488,7 @@ private void configureButtonBindings() { //reach amp driverLB.whileTrue( new SequentialCommandGroup( + new PrepareFeed(s_Shooter, 25), new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 5ff8850..079260e 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -60,19 +60,19 @@ public class AimShoot extends Command { // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.31; //1.21 at 930, 1.25 at comp, 1.31 at marquette + private final double subWooferDistance = 1.33; //1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) private final double subWooferAngle = 115.0; //115 private final double subWooferSpeed = 35.0; //50 - private final double xSpotDistance = 2.45; //2.45 at marquette + private final double xSpotDistance = 2.45; //2.45 at marquette //2.45 at Archimedes (blue) 2.37 private final double xSpotAngle = 131.0; private final double xSpotSpeed = 45.0; - private final double podiumDistance = 3.02; //3.17 at comp, 3.02 at marquette + private final double podiumDistance = 3.08; //3.17 at comp, 3.02 at marquette, 3.08 Archimedes (blue) 3.09 (red) private final double podiumAngle = 137; private final double podiumSpeed = 60.0; - private final double chainDistance = 4.33; //4.33 at marquette (NOT ACCCURATE) + private final double chainDistance = 4.32; //4.33 at marquette (NOT ACCCURATE) //4.32 at Archimedes (blue) 4.22 (red) private final double chainAngle = 145.0; private final double chainSpeed = 80.0; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 4e6a071..9e92ce2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -30,8 +30,8 @@ public class Intake extends SubsystemBase { // positions // NOTE: these positions are also used in robotcontainer. - public final double intakeSafePosition = 327.5; - public final double intakeGroundPosition = 227.9; + public final double intakeSafePosition = -35; + public final double intakeGroundPosition = -135; public final double intakeSourcePosition = intakeSafePosition; // PID values From 8d4e6ff9f13e933f3afed2dd5614374c03562475 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Wed, 17 Apr 2024 18:48:51 -0500 Subject: [PATCH 75/82] added slow shooter speed for amp --- src/main/java/frc/robot/RobotContainer.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bdef3a5..5239eec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -487,11 +487,13 @@ private void configureButtonBindings() { //reach amp driverLB.whileTrue( - new SequentialCommandGroup( + new ParallelCommandGroup( new PrepareFeed(s_Shooter, 25), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), - s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) + new SequentialCommandGroup( + new InstantCommand(() -> s_Elevator.SetElevatorPosition(Constants.ELEVATOR_HIGH_LEVEL)), + s_Elevator.ElevatorAtPosition(Constants.ELEVATOR_SAFE_LEVEL), + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotAmpPosition)) + ) ) ).onFalse( new SequentialCommandGroup( From 338d120df97b648087bdf92d12a0295dc5e81505 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 10:58:54 -0500 Subject: [PATCH 76/82] day 1 working code --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 35 +++----------- .../java/frc/robot/commands/AimShoot.java | 4 +- .../frc/robot/commands/ManualElevator.java | 46 +++++++++++++++++++ .../java/frc/robot/subsystems/Elevator.java | 4 +- .../frc/robot/subsystems/ShooterPivot.java | 2 +- 6 files changed, 59 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ManualElevator.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8277cdd..3494e81 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,7 +19,7 @@ public final class Constants { public static final double ELEVATOR_GEAR_RATIO = 20.0; public static final double ELEVATOR_ROTATIONS_TO_IN = (1.0 / ELEVATOR_GEAR_RATIO) * ELEVATOR_SPROCKET_DIAMETER * Math.PI * 2.0; public static final double ELEVATOR_TOLERANCE = 1.0; - public static final double ELEVATOR_HIGH_LEVEL = 16.0; + public static final double ELEVATOR_HIGH_LEVEL = 16.5; public static final double ELEVATOR_SAFE_LEVEL = 7.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5239eec..17f66d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -500,37 +501,13 @@ private void configureButtonBindings() { new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), s_ShooterPivot.ShooterPivotAtPosition(), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)) + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)), + s_Elevator.ElevatorAtPosition(0.0), + new InstantCommand(() -> s_Elevator.resetEncoder()) ) ); - //source intake - //source intake - operatorY.whileTrue( - new SequentialCommandGroup( - new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(8.85)), - s_Elevator.ElevatorAtPosition(), - - new ParallelCommandGroup( - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(325)), - new RunLoader(s_Shooter).until(() -> !s_Shooter.getBreakBeamOutput()) - .andThen(new ParallelCommandGroup( - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) - )))) - - ).onFalse( - new ParallelCommandGroup( - new ParallelCommandGroup( - new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) - ), - new SequentialCommandGroup( - new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), - s_ShooterPivot.ShooterPivotAtPosition(), - new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)) - ) - ) - ); + operatorRightTrigger.onTrue( new ParallelCommandGroup( @@ -557,7 +534,7 @@ private void configureButtonBindings() { // prepare trap operatorX.whileTrue(new Shooting(s_ShooterPivot, s_Shooter, 45.0, s_ShooterPivot.shooterPivotStowPosition)); - + operatorLB.onTrue(new InstantCommand(() -> s_Elevator.resetEncoder())); } diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 079260e..e6d1b0e 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -61,11 +61,11 @@ public class AimShoot extends Command { //Higher note shot is lower angle!!! private final double subWooferDistance = 1.33; //1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) - private final double subWooferAngle = 115.0; //115 + private final double subWooferAngle = 116.0; //115 private final double subWooferSpeed = 35.0; //50 private final double xSpotDistance = 2.45; //2.45 at marquette //2.45 at Archimedes (blue) 2.37 - private final double xSpotAngle = 131.0; + private final double xSpotAngle = 132.0; private final double xSpotSpeed = 45.0; private final double podiumDistance = 3.08; //3.17 at comp, 3.02 at marquette, 3.08 Archimedes (blue) 3.09 (red) diff --git a/src/main/java/frc/robot/commands/ManualElevator.java b/src/main/java/frc/robot/commands/ManualElevator.java new file mode 100644 index 0000000..b78cac8 --- /dev/null +++ b/src/main/java/frc/robot/commands/ManualElevator.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.subsystems.Elevator; + + +public class ManualElevator extends Command { + private Elevator s_Elevator; + private double speed; + + public ManualElevator(Elevator s_Elevator, double speed) { + + // Use addRequirements() here to declare subsystem dependencies. + this.s_Elevator = s_Elevator; + this.speed = speed; + addRequirements(s_Elevator); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + s_Elevator.move(speed); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + s_Elevator.move(0.0); + s_Elevator.resetEncoder(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index b1469ee..8f42d27 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -242,7 +242,9 @@ public void isClimbed(boolean climbState) { } - +public void resetEncoder() { + e_Elevator.setPosition(0.0); +} /* The below method is included in every Subsystem. You can diff --git a/src/main/java/frc/robot/subsystems/ShooterPivot.java b/src/main/java/frc/robot/subsystems/ShooterPivot.java index abe465b..6b5e034 100644 --- a/src/main/java/frc/robot/subsystems/ShooterPivot.java +++ b/src/main/java/frc/robot/subsystems/ShooterPivot.java @@ -29,7 +29,7 @@ public class ShooterPivot extends SubsystemBase { // positions public final double shooterPivotStowPosition = 115.0; public final double shooterPivotIntakePosition = 136.75; - public final double shooterPivotAmpPosition = 200; + public final double shooterPivotAmpPosition = 192.5; public final double shooterPivotClimbPosition = 309; // pivot motor PID From c4802d778ed3bfe8b249e722f517db4458231aa4 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 14:31:28 -0500 Subject: [PATCH 77/82] shooter adjustments --- src/main/java/frc/robot/commands/AimShoot.java | 10 +++++----- src/main/java/frc/robot/subsystems/Eyes.java | 11 ++++++----- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index e6d1b0e..902cb33 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -61,23 +61,23 @@ public class AimShoot extends Command { //Higher note shot is lower angle!!! private final double subWooferDistance = 1.33; //1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) - private final double subWooferAngle = 116.0; //115 + private final double subWooferAngle = 115.5; //115 private final double subWooferSpeed = 35.0; //50 private final double xSpotDistance = 2.45; //2.45 at marquette //2.45 at Archimedes (blue) 2.37 - private final double xSpotAngle = 132.0; + private final double xSpotAngle = 132.0; //132 private final double xSpotSpeed = 45.0; private final double podiumDistance = 3.08; //3.17 at comp, 3.02 at marquette, 3.08 Archimedes (blue) 3.09 (red) - private final double podiumAngle = 137; + private final double podiumAngle = 135; //135 private final double podiumSpeed = 60.0; private final double chainDistance = 4.32; //4.33 at marquette (NOT ACCCURATE) //4.32 at Archimedes (blue) 4.22 (red) - private final double chainAngle = 145.0; + private final double chainAngle = 145.0; //145 private final double chainSpeed = 80.0; private final double wingerDistance = 5.44; - private final double wingerAngle = 147; + private final double wingerAngle = 147; //147 private final double wingerSpeed = 40.25; diff --git a/src/main/java/frc/robot/subsystems/Eyes.java b/src/main/java/frc/robot/subsystems/Eyes.java index f2af8aa..a8939eb 100644 --- a/src/main/java/frc/robot/subsystems/Eyes.java +++ b/src/main/java/frc/robot/subsystems/Eyes.java @@ -356,7 +356,8 @@ public Pose2d getMovingTarget() { double robotAccelX = s_Swerve.fieldRelativeAccel.ax; double robotAccelY = s_Swerve.fieldRelativeAccel.ay; - double velocityCompensation = 2.0; + double xVelocityCompensation = 1.0; + double yVelocityCompensation = 1.5; for(int i=0;i<5;i++){ @@ -364,11 +365,11 @@ public Pose2d getMovingTarget() { double virtualGoalY; if(DriverStation.getAlliance().get() == Alliance.Blue) { - virtualGoalX = getTargetPose().getX() - shotTime * ((robotVelX * velocityCompensation) + robotAccelX * accelerationCompensation); - virtualGoalY = getTargetPose().getY() - shotTime * ((robotVelY * velocityCompensation) + robotAccelY * accelerationCompensation); + virtualGoalX = getTargetPose().getX() - shotTime * ((robotVelX * xVelocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() - shotTime * ((robotVelY * yVelocityCompensation) + robotAccelY * accelerationCompensation); } else { - virtualGoalX = getTargetPose().getX() + shotTime * ((robotVelX * velocityCompensation) + robotAccelX * accelerationCompensation); - virtualGoalY = getTargetPose().getY() + shotTime * ((robotVelY * velocityCompensation) + robotAccelY * accelerationCompensation); + virtualGoalX = getTargetPose().getX() + shotTime * ((robotVelX * xVelocityCompensation) + robotAccelX * accelerationCompensation); + virtualGoalY = getTargetPose().getY() + shotTime * ((robotVelY * yVelocityCompensation) + robotAccelY * accelerationCompensation); } Translation2d testGoalLocation = new Translation2d(virtualGoalX, virtualGoalY); From 98a7d975af4dec642a1822447244455cfe896c3d Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 15:30:39 -0500 Subject: [PATCH 78/82] added source intake --- src/main/java/frc/robot/RobotContainer.java | 29 ++++++++++++++++++++- 1 file changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 17f66d5..b53cc3f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -507,7 +507,34 @@ private void configureButtonBindings() { ) ); - + //source intake + operatorY.whileTrue( + new SequentialCommandGroup( + new InstantCommand(() -> s_Shooter.setShooterVoltage(0,0)), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(8.85)), + s_Elevator.ElevatorAtPosition(), + + new ParallelCommandGroup( + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(325)), + new RunLoader(s_Shooter).until(() -> !s_Shooter.getBreakBeamOutput()) + .andThen(new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 1)) + )))) + + ).onFalse( + new ParallelCommandGroup( + new ParallelCommandGroup( + new InstantCommand(() -> driver.setRumble(RumbleType.kBothRumble, 0)) + ), + new SequentialCommandGroup( + new InstantCommand(() -> s_ShooterPivot.moveShooterPivot(s_ShooterPivot.shooterPivotStowPosition)), + s_ShooterPivot.ShooterPivotAtPosition(), + new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), + s_Elevator.ElevatorAtPosition(0.0), + new InstantCommand(() -> s_Elevator.resetEncoder()) + ) + ) + ); operatorRightTrigger.onTrue( new ParallelCommandGroup( From a746ad92fd0967b8b7000cae63b237db5ff7e24b Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 15:50:44 -0500 Subject: [PATCH 79/82] Encoder reset in full command; xsptoAngle changed --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/AimShoot.java | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b53cc3f..9191c03 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -561,7 +561,6 @@ private void configureButtonBindings() { // prepare trap operatorX.whileTrue(new Shooting(s_ShooterPivot, s_Shooter, 45.0, s_ShooterPivot.shooterPivotStowPosition)); - operatorLB.onTrue(new InstantCommand(() -> s_Elevator.resetEncoder())); } diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 902cb33..73c411a 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -65,7 +65,7 @@ public class AimShoot extends Command { private final double subWooferSpeed = 35.0; //50 private final double xSpotDistance = 2.45; //2.45 at marquette //2.45 at Archimedes (blue) 2.37 - private final double xSpotAngle = 132.0; //132 + private final double xSpotAngle = 131.0; //132 private final double xSpotSpeed = 45.0; private final double podiumDistance = 3.08; //3.17 at comp, 3.02 at marquette, 3.08 Archimedes (blue) 3.09 (red) From 8343395df598457f42dcd126245211d2cd3a90e5 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 16:57:13 -0500 Subject: [PATCH 80/82] fixed the elevator encoder stacking bug --- src/main/java/frc/robot/RobotContainer.java | 2 ++ src/main/java/frc/robot/subsystems/Elevator.java | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9191c03..ee57102 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -503,6 +503,7 @@ private void configureButtonBindings() { s_ShooterPivot.ShooterPivotAtPosition(), new InstantCommand(() -> s_Elevator.SetElevatorPosition(0.0)), s_Elevator.ElevatorAtPosition(0.0), + new WaitCommand(0.5), new InstantCommand(() -> s_Elevator.resetEncoder()) ) ); @@ -531,6 +532,7 @@ private void configureButtonBindings() { s_ShooterPivot.ShooterPivotAtPosition(), new InstantCommand(() -> s_Elevator.SetElevatorPosition(0)), s_Elevator.ElevatorAtPosition(0.0), + new WaitCommand(0.5), new InstantCommand(() -> s_Elevator.resetEncoder()) ) ) diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 8f42d27..2fcdcea 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -63,7 +63,7 @@ public class Elevator extends SubsystemBase { private double heightlimit = 16; public double elevatorspeed = 0.1; - public double restingposition = 0; + public double restingposition = 0.0; public double climbingPosition = -2.0; public double shootingPosition = 12.0; From e4f4ce0aec0ef80037df930a19bc29c9c4b81df0 Mon Sep 17 00:00:00 2001 From: Driver Station Date: Thu, 18 Apr 2024 23:19:19 -0500 Subject: [PATCH 81/82] added point to linear interpolation for midline auto shot 3 (needs testing) --- .../pathplanner/paths/Position Locator (DO NOT USE).path | 8 ++++---- src/main/java/frc/robot/commands/AimShoot.java | 9 +++++++-- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path index 9d7f386..f6b1c19 100644 --- a/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path +++ b/src/main/deploy/pathplanner/paths/Position Locator (DO NOT USE).path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.1, - "y": 2.8 + "x": 3.0, + "y": 5.375482352388193 }, "prevControl": null, "nextControl": { - "x": 5.100000000000005, - "y": 2.8 + "x": 4.000000000000005, + "y": 5.375482352388193 }, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/commands/AimShoot.java b/src/main/java/frc/robot/commands/AimShoot.java index 73c411a..4d34f49 100644 --- a/src/main/java/frc/robot/commands/AimShoot.java +++ b/src/main/java/frc/robot/commands/AimShoot.java @@ -60,11 +60,11 @@ public class AimShoot extends Command { // positions //Higher note shot is lower angle!!! - private final double subWooferDistance = 1.33; //1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) + private final double subWooferDistance = 1.38; //(1.33 real field) 1.38 at practice field 1.21 at 930, 1.25 at comp, 1.31 at marquette //1.33 at Archimedes (blue) 1.26 (red) private final double subWooferAngle = 115.5; //115 private final double subWooferSpeed = 35.0; //50 - private final double xSpotDistance = 2.45; //2.45 at marquette //2.45 at Archimedes (blue) 2.37 + private final double xSpotDistance = 2.57; //2.57 at practice field 2.45 at marquette //2.45 at Archimedes (blue) 2.37 private final double xSpotAngle = 131.0; //132 private final double xSpotSpeed = 45.0; @@ -80,6 +80,9 @@ public class AimShoot extends Command { private final double wingerAngle = 147; //147 private final double wingerSpeed = 40.25; + //private final double midlineAutoShot3distance = 2.22; + //private final double midlineAutoShot3Angle = 125; + //private final double midlineAutoShot3Speed = 42; private final double feedDistance = 2.4; @@ -114,6 +117,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i shooterAngleInterpolation.put(xSpotDistance, xSpotAngle); shooterAngleInterpolation.put(chainDistance, chainAngle); shooterAngleInterpolation.put(wingerDistance, wingerAngle); + //shooterAngleInterpolation.put(midlineAutoShot3distance, midlineAutoShot3Angle); shooterAngleInterpolationElevator.put(elevatorShotDistance, elevatorShotAngle); @@ -125,6 +129,7 @@ public AimShoot(Eyes eyes, ShooterPivot shooterPivot, Shooter shooter, boolean i shooterSpeedInterpolation.put(xSpotDistance, xSpotSpeed); shooterSpeedInterpolation.put(wingerDistance, wingerSpeed); shooterSpeedInterpolation.put(subWooferDistance, subWooferSpeed); + //shooterSpeedInterpolation.put(midlineAutoShot3distance, midlineAutoShot3Speed); } From 6f8d3d02bf91b4959dc135ceb2dc6744abc96f3f Mon Sep 17 00:00:00 2001 From: Driver Station Date: Fri, 19 Apr 2024 10:20:40 -0500 Subject: [PATCH 82/82] tuned midline auto last shot rotation --- src/main/deploy/pathplanner/paths/Midline Path 5.path | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/paths/Midline Path 5.path b/src/main/deploy/pathplanner/paths/Midline Path 5.path index 9b7d718..6956419 100644 --- a/src/main/deploy/pathplanner/paths/Midline Path 5.path +++ b/src/main/deploy/pathplanner/paths/Midline Path 5.path @@ -57,7 +57,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 24.304549265936778, + "rotation": 43.15238973400542, "rotateFast": false }, "reversed": false,