From 994eb9e16676c7f6b79f75f0b52dc167bad62316 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Wed, 3 Apr 2024 17:28:34 -0500 Subject: [PATCH 01/24] slowed down auto pickup and tuned shots --- .../autos/(AP)StartSourceScore3FarSource.auto | 2 +- .../paths/Blue4AutoPickupSourceSide.path | 12 +++++----- src/main/java/frc/robot/RobotContainer.java | 6 ++--- .../java/frc/robot/commands/CollectNote.java | 4 ++-- .../java/frc/robot/settings/Constants.java | 22 ++++++++++--------- 5 files changed, 24 insertions(+), 22 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto index c3cf2004..b0c27942 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartSourceScore3FarSource.auto @@ -5,7 +5,7 @@ "x": 0.7607347530216694, "y": 4.480646518544946 }, - "rotation": 121.7 + "rotation": 116.0 }, "command": { "type": "sequential", diff --git a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path index 2ab5626c..9c8d1a43 100644 --- a/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path +++ b/src/main/deploy/pathplanner/paths/Blue4AutoPickupSourceSide.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.156417275797516, - "y": 1.7934317350116178 + "x": 6.9848151084202525, + "y": 2.113187808819271 }, "prevControl": { - "x": 6.046263311717348, - "y": 1.3552130649799718 + "x": 5.8746611443400845, + "y": 1.674969138787625 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.6, - "rotationDegrees": -157.01, + "rotationDegrees": -168.42994609958126, "rotateFast": true } ], @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -159.86, + "rotation": -165.06858282186255, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 34cd201b..bcd92584 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -326,8 +326,8 @@ private void configureBindings() { new InstantCommand(()->intake.setNoteHeld(true))), new SequentialCommandGroup( new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain), - new DriveTimeCommand(1, 0, 0, 0.5, driveTrain), + new DriveTimeCommand(-0.7, 0, 0, 1.5, driveTrain), + new DriveTimeCommand(0.7, 0, 0, 0.5, driveTrain), new DriveTimeCommand(-1, 0, 0, 0.5, driveTrain), new WaitCommand(0.5) )).withTimeout(4); @@ -385,7 +385,7 @@ private void configureBindings() { new WaitCommand(0.5), new InstantCommand(()->intake.setNoteHeld(false)) ); - SmartDashboard.putNumber("Indexer Amp Speed", shooterAmpSpeed); + SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( new InstantCommand(()->shooter.shootWithSupplier(()->SmartDashboard.getNumber("amp RPS", shooterAmpSpeed), true), shooter), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index 8033597f..cbbbbf4b 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -103,7 +103,7 @@ public void execute() { SmartDashboard.putNumber("CollectNote/forward speed limited", forwardSpeed); } else { drivetrain.drive(new ChassisSpeeds( - -1, 0, 0)); + -0.6, 0, 0)); runsInvalid++; } // else { @@ -125,7 +125,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - drivetrain.pointWheelsInward(); + // drivetrain.pointWheelsInward(); runsInvalid = 0; closeNote = false; } diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 0c4ee24a..1f3c4d6a 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -455,21 +455,23 @@ public final class PS4Operator{ public final class Field{ public static final double ROBOT_CENTER_TO_BUMBER = 0.419; - public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.79;//NOT fOUND - public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.33; //NOT FOUND - public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 13.99; - public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y = 3.99; + public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.48;//NOT fOUND + public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.89; //NOT FOUND + public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 14.11; + public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y = 4.02; public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X-ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y+ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X+ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y-ROBOT_CENTER_TO_BUMBER; - public static final double ROBOT_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3; - public static final double ROBOT_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in + public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;//5.46; + public static final double RED_SPEAKER_Y = 5.5; public static final double SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in - public static final double SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;//16.38; changed so that shots from the side wil aim to the opposite side, and bank in - public static final double RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;//5.46; - public static final double BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263;//5.46; + public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6;// AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3; + public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; + public static final double SHOOTER_RED_SPEAKER_X = 16.5;//AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;//16.38; changed so that shots from the side wil aim to the opposite side, and bank in + public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in + public static final double BLUE_SPEAKER_Y = 5.4;//AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263;//5.46; public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in public static final double MAX_SHOOTING_DISTANCE = 9; @@ -477,7 +479,7 @@ public final class Field{ public static final double AMPLIFIER_SHOOTER_ANGLE = 108; public static final double SUBWOOFER_ANGLE = 60; - public static final double PODIUM_SHOOTER_ANGLE = 36.3; + public static final double PODIUM_SHOOTER_ANGLE = 40;//36.3; public static final double FAR_STAGE_SHOOTER_ANGLE = 25.5; //24.5; public static final double OPPOSITE_STAGE_SHOOTER_ANGLE = 26;//24.5; From 239bcc5f406f7dc60e03178621d6d965c4e25fc3 Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Wed, 3 Apr 2024 21:28:45 -0500 Subject: [PATCH 02/24] reformatted calibrating code --- .../java/frc/robot/settings/Constants.java | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 1f3c4d6a..927a5a2c 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -464,16 +464,20 @@ public final class Field{ public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X+ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y-ROBOT_CENTER_TO_BUMBER; - public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263;//5.46; - public static final double RED_SPEAKER_Y = 5.5; - public static final double SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in - public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6;// AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3; public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; - public static final double SHOOTER_RED_SPEAKER_X = 16.5;//AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213;//16.38; changed so that shots from the side wil aim to the opposite side, and bank in - public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3;//16.38;//16.87; changed so that shots from the side wil aim to the opposite side, and bank in - public static final double BLUE_SPEAKER_Y = 5.4;//AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263;//5.46; - public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in + public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; + public static final double RED_SPEAKER_Y = CALCULATED_RED_SPEAKER_Y; + public static final double SHOOTER_RED_SPEAKER_X = CALCULATED_SHOOTER_RED_SPEAKER_X;//changed so that shots from the side wil aim to the opposite side, and bank in + public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in + + public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in + public static final double CALCULATED_BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263; + public static final double BLUE_SPEAKER_Y = CALCULATED_BLUE_SPEAKER_Y; + public static final double SHOOTER_BLUE_SPEAKER_X = CALCULATED_SHOOTER_BLUE_SPEAKER_X; + public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6; + public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in + public static final double MAX_SHOOTING_DISTANCE = 9; public static final double SHORT_RANGE_SHOOTING_DIST = 3; From 17cfbd45307477680c7106ac09741015ba9bef97 Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Wed, 3 Apr 2024 21:31:43 -0500 Subject: [PATCH 03/24] sped up autoPickup again --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/commands/CollectNote.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bcd92584..bd47b097 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -326,8 +326,8 @@ private void configureBindings() { new InstantCommand(()->intake.setNoteHeld(true))), new SequentialCommandGroup( new CollectNote(driveTrain, limelight), - new DriveTimeCommand(-0.7, 0, 0, 1.5, driveTrain), - new DriveTimeCommand(0.7, 0, 0, 0.5, driveTrain), + new DriveTimeCommand(-1, 0, 0, 1.5, driveTrain), + new DriveTimeCommand(1, 0, 0, 0.5, driveTrain), new DriveTimeCommand(-1, 0, 0, 0.5, driveTrain), new WaitCommand(0.5) )).withTimeout(4); diff --git a/src/main/java/frc/robot/commands/CollectNote.java b/src/main/java/frc/robot/commands/CollectNote.java index cbbbbf4b..eefd876e 100644 --- a/src/main/java/frc/robot/commands/CollectNote.java +++ b/src/main/java/frc/robot/commands/CollectNote.java @@ -103,7 +103,7 @@ public void execute() { SmartDashboard.putNumber("CollectNote/forward speed limited", forwardSpeed); } else { drivetrain.drive(new ChassisSpeeds( - -0.6, 0, 0)); + -1, 0, 0)); runsInvalid++; } // else { From 35c71eb18fd37a79fc82454f69436792d2251518 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 08:44:43 -0500 Subject: [PATCH 04/24] amp speed doesn't use smartdashboard --- 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 bd47b097..61c33e1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -387,14 +387,14 @@ private void configureBindings() { ); SmartDashboard.putNumber("Indexer Amp Speed", indexerAmpSpeed); SequentialCommandGroup orbitAmpShot = new SequentialCommandGroup( - new InstantCommand(()->shooter.shootWithSupplier(()->SmartDashboard.getNumber("amp RPS", shooterAmpSpeed), true), shooter), + new InstantCommand(()->shooter.shootWithSupplier(()->shooterAmpSpeed, true), shooter), new MoveMeters(driveTrain, 0.015, 0.5, 0, 0), new InstantCommand(driveTrain::pointWheelsInward, driveTrain), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(15), angleShooterSubsystem), - new WaitUntil(()->(Math.abs(shooter.getLSpeed()-SmartDashboard.getNumber("amp RPS", shooterAmpSpeed))<1)&&(Math.abs(shooter.getRSpeed()-SmartDashboard.getNumber("amp RPS", shooterAmpSpeed))<1)), + new WaitUntil(()->(Math.abs(shooter.getLSpeed()-shooterAmpSpeed)<1)&&(Math.abs(shooter.getRSpeed()-shooterAmpSpeed)<1)), new InstantCommand(()->angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE)), new WaitCommand(0.1), - new InstantCommand(()->indexer.magicRPSSupplier(()->SmartDashboard.getNumber("Indexer Amp Speed", 150)), indexer), + new InstantCommand(()->indexer.magicRPSSupplier(()->indexerAmpSpeed), indexer), new WaitCommand(0.5), new InstantCommand(()->intake.setNoteHeld(false)) ); From a9995d241a207dd3530426ed4636506d32aff403 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 10:50:25 -0500 Subject: [PATCH 05/24] callibrated shooting for event fiekd --- src/main/java/frc/robot/settings/Constants.java | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index 927a5a2c..c818abff 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -455,10 +455,10 @@ public final class PS4Operator{ public final class Field{ public static final double ROBOT_CENTER_TO_BUMBER = 0.419; - public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.48;//NOT fOUND - public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.89; //NOT FOUND - public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 14.11; - public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y = 4.02; + public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X = 2.945;//NOT fOUND + public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_Y = 4.585; //NOT FOUND + public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X = 13.94; + public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y = 3.99; public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_X-ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_RED_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_ROBOT_Y+ROBOT_CENTER_TO_BUMBER; public static final double AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_ROBOT_X+ROBOT_CENTER_TO_BUMBER; @@ -466,14 +466,14 @@ public final class Field{ public static final double CALCULATED_SHOOTER_RED_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_RED_X+3.213; public static final double CALCULATED_RED_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_RED_Y+1.263; - public static final double RED_SPEAKER_Y = CALCULATED_RED_SPEAKER_Y; - public static final double SHOOTER_RED_SPEAKER_X = CALCULATED_SHOOTER_RED_SPEAKER_X;//changed so that shots from the side wil aim to the opposite side, and bank in + public static final double RED_SPEAKER_Y = 5.58; + public static final double SHOOTER_RED_SPEAKER_X = 16.45;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double ROBOT_RED_SPEAKER_X = SHOOTER_RED_SPEAKER_X-0.165;//changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_SHOOTER_BLUE_SPEAKER_X = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_X-3.213; //changed so that shots from the side wil aim to the opposite side, and bank in public static final double CALCULATED_BLUE_SPEAKER_Y = AMP_SIDE_OUTER_TAPE_CORNER_BLUE_Y+1.263; - public static final double BLUE_SPEAKER_Y = CALCULATED_BLUE_SPEAKER_Y; - public static final double SHOOTER_BLUE_SPEAKER_X = CALCULATED_SHOOTER_BLUE_SPEAKER_X; + public static final double BLUE_SPEAKER_Y = 5.39; + public static final double SHOOTER_BLUE_SPEAKER_X = 0; public static final double ROBOT_BLUE_SPEAKER_X =SHOOTER_BLUE_SPEAKER_X+0.6; public static final double SPEAKER_Z = 2.08;//1.5;//1.8;//2.08; //height of opening. Changed so that the smaller spekeaker_x shots will still go in From 9a0f44138f69b6a176cb6139b9b8cabe38457e26 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 10:53:38 -0500 Subject: [PATCH 06/24] yellow lights more yellow --- src/main/java/frc/robot/commands/IndicatorLights.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index ebded616..bb24ae32 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -44,7 +44,7 @@ public void execute() { if(!noteInRobot) { if(noteSeenByLimelight) { - lights.setSides(50, 50, 0); + lights.setSides(50, 43, 0); } else { lights.setSides(0, 0, 0); } From b94191294dede7186ae3c8bba645e20ff98cdb50 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 11:51:26 -0500 Subject: [PATCH 07/24] better yellow --- src/main/java/frc/robot/commands/IndicatorLights.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index bb24ae32..78a402ce 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -44,7 +44,7 @@ public void execute() { if(!noteInRobot) { if(noteSeenByLimelight) { - lights.setSides(50, 43, 0); + lights.setSides(70, 35, 0); } else { lights.setSides(0, 0, 0); } From 1b26d3d9d80dc40befe03ae953973e877e5f0ecc Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 13:00:16 -0500 Subject: [PATCH 08/24] removed CLoseMidPickupAuto and hardStop from (AP)StartMidScore3Close --- .../pathplanner/autos/(AP)StartMidScore3Close.auto | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto index fb5a0c7e..74797177 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartMidScore3Close.auto @@ -23,18 +23,6 @@ "name": "shooterOn" } }, - { - "type": "path", - "data": { - "pathName": "CloseMidPickupAuto" - } - }, - { - "type": "named", - "data": { - "name": "hardStop" - } - }, { "type": "named", "data": { From 805f381bc26e3e3f1a6d1d8f5dc231ffa7798ed3 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 13:35:30 -0500 Subject: [PATCH 09/24] new methods in Lights.Java --- .../java/frc/robot/subsystems/Lights.java | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Lights.java b/src/main/java/frc/robot/subsystems/Lights.java index 61c343d1..e07c6b85 100644 --- a/src/main/java/frc/robot/subsystems/Lights.java +++ b/src/main/java/frc/robot/subsystems/Lights.java @@ -54,6 +54,55 @@ public void setRight(int R, int G, int B){ setLights(55, 60, R, G, B); } + /** + * Set one light in a horizontal line on all 4 strips + * @param pos 0-14 where 0 is the far left LED and 14 is the far right LED + */ + public void setHorizontal(int pos, int R, int G, int B){ + setOneLightRGB((14 - pos), R, G, B); + setOneLightRGB((15 + pos), R, G, B); + setOneLightRGB((44 - pos), R, G, B); + setOneLightRGB((45 + pos), R, G, B); + } + + /** + * Set each side to 2 seperate colors based on a given position + * 11122-----22111 + * ^pos + * @param pos the number of lights to the left of the split + */ + public void setSplit(int pos, int R1, int G1, int B1, int R2, int G2, int B2){ + for(int i = 0; i < pos; i++){ + setHorizontal(i, R1, G1, B1); + setHorizontal(14 - i, R1, G1, B1); + } + for(int i = pos; i < 5; i++){ + setHorizontal(i, R2, G2, B2); + setHorizontal(14 - i, R2, G2, B2); + } + + } + + /** + * set lights based on a distance from 0 + * when the absolute value of progress is greater than 1 all lights are color 1 + * when the absolute value of progress is near 0 all lights are color 2 + * when the absolute value of progress is in between 0 and 1 some lights will be color 1 and some will be color 2 + */ + public void setProgress(double progress, int R1, int G1, int B1, int R2, int G2, int B2){ + int count = (int)Math.abs(progress * 5); + if(count > 5){ + count = 5; + } + + if (progress < 0){ + setSplit(count, R1, G1, B1, R2, G2, B2); + } + else{ + setSplit(5 - count, R2, G2, B2, R1, G1, B1); + } + } + public void setSides(int R, int G, int B){ setLeft(R, G, B); setRight(R, G, B); From 5d3586c827dfc89d6c33045bf918f2126ecec746 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 14:43:52 -0500 Subject: [PATCH 10/24] added state --- src/main/java/frc/robot/subsystems/RobotState.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/RobotState.java b/src/main/java/frc/robot/subsystems/RobotState.java index aa522589..34e22519 100644 --- a/src/main/java/frc/robot/subsystems/RobotState.java +++ b/src/main/java/frc/robot/subsystems/RobotState.java @@ -6,6 +6,8 @@ public class RobotState { public boolean IsNoteHeld; public boolean ShooterInRange; public boolean ShooterReady; + public double ShooterError; + public double ShooterVelocity; public boolean LimelightsUpdated; private RobotState() { From 5d3b7ba2b379d4b56b95615f3285e57557783a18 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 14:44:20 -0500 Subject: [PATCH 11/24] curved source path --- .../pathplanner/paths/Blue5AutoPickupFromStart.path | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path index 54b62dc4..65ff1d5c 100644 --- a/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path +++ b/src/main/deploy/pathplanner/paths/Blue5AutoPickupFromStart.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.951779274060725, - "y": 1.6143455862325808 + "x": 2.7779158761164506, + "y": 2.34690443283615 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.412869986233799 }, "prevControl": { - "x": 4.733816875041158, - "y": 2.313223391878882 + "x": 4.783983565594652, + "y": 1.645754560785515 }, "nextControl": null, "isLocked": false, From b3e737f02d6fb25759ed9ee04298e73b47409bf9 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 14:46:18 -0500 Subject: [PATCH 12/24] fixed blue setpoints --- src/main/java/frc/robot/settings/Constants.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index c818abff..05414745 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -490,12 +490,12 @@ public final class Field{ public static final double BLUE_PODIUM_ROBOT_ANGLE = 149; public static final double RED_PODIUM_ROBOT_ANGLE = 31; - public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 184; + public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 186; public static final double RED_FAR_STAGE_ROBOT_ANGLE = -4; public static final double RED_OPPOSITE_STAGE_ROBOT_ANGLE = 32;//31 - public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 149; + public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 148; public static final double RED_OVER_STAGE_PASS_ANGLE = 45; - public static final double BLUE_OVER_STAGE_PASS_ANGLE = -115; + public static final double BLUE_OVER_STAGE_PASS_ANGLE = -135; //angle at 60 for bounce techinque, didn't work } From f1333e3c7dbe0c3b2cb115114207d0a9c06e733d Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 15:06:43 -0500 Subject: [PATCH 13/24] lights flash white once when the note enters the robot for 2-4 seconds --- .../frc/robot/commands/IndicatorLights.java | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index 78a402ce..7bc4eb30 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -4,6 +4,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.Lights; import frc.robot.subsystems.RobotState; @@ -14,6 +15,8 @@ public class IndicatorLights extends Command { Lights lights; + Timer timer; + boolean noteWasIn; /** * This command controls the lights. Middle Lights: if limelights are updating the odometry, these lights are green, otherwise they are red. * Side Lights: If a note is held, they are red if the shooter isn't rev'ed up, and green if they are rev'ed up. If a note is not held, they are off unless a note is seen by the intake @@ -22,6 +25,8 @@ public class IndicatorLights extends Command { public IndicatorLights(Lights lights) { addRequirements(lights); this.lights = lights; + timer = new Timer(); + timer.start(); } @@ -42,14 +47,22 @@ public void execute() { lights.setMid(50, 0, 0); } - if(!noteInRobot) { + if(timer.get() < 2){ + lights.setSides(255, 255, 255); + } + else if(!noteInRobot) { + noteWasIn = false; if(noteSeenByLimelight) { lights.setSides(70, 35, 0); } else { lights.setSides(0, 0, 0); } } - if(noteInRobot) { + else{ + if(!noteWasIn){ + noteWasIn = true; + timer.reset(); + } if(allSystemsGoodToGo) { lights.setSides(0, 50, 0); } else { From 8786d6f8c83af17984fa3f2974b49cd8250cb48d Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 15:20:33 -0500 Subject: [PATCH 14/24] progress bar lights done --- src/main/java/frc/robot/RobotContainer.java | 1 - src/main/java/frc/robot/commands/IndexCommand.java | 6 ------ src/main/java/frc/robot/commands/IndicatorLights.java | 8 ++------ src/main/java/frc/robot/subsystems/RobotState.java | 2 -- .../java/frc/robot/subsystems/ShooterSubsystem.java | 10 ++++++++-- 5 files changed, 10 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61c33e1b..c7e17de1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -604,7 +604,6 @@ public void teleopPeriodic() { } SmartDashboard.putBoolean("is note seen", RobotState.getInstance().IsNoteSeen); SmartDashboard.putBoolean("shooter in range", RobotState.getInstance().ShooterInRange); - SmartDashboard.putBoolean("shooter ready", RobotState.getInstance().ShooterReady); } public void logPower(){ diff --git a/src/main/java/frc/robot/commands/IndexCommand.java b/src/main/java/frc/robot/commands/IndexCommand.java index 38262be4..5850aa40 100644 --- a/src/main/java/frc/robot/commands/IndexCommand.java +++ b/src/main/java/frc/robot/commands/IndexCommand.java @@ -153,11 +153,6 @@ public void execute() { } } boolean indexer = false; - if(shooter.validShot()&&!idleReving) { - RobotState.getInstance().ShooterReady = true; - } else { - RobotState.getInstance().ShooterReady = false; - } if(angleShooterSubsytem.validShot() && drivetrain.validShot() && shooter.validShot() && shooter.isReving() && !idleReving) { if (shootIfReadySupplier.getAsBoolean()) { indexer = true; @@ -184,7 +179,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - RobotState.getInstance().ShooterReady = false; } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index 7bc4eb30..bbe6137b 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -39,7 +39,6 @@ public void execute() { boolean noteSeenByLimelight = RobotState.getInstance().IsNoteSeen; boolean limelightsUpdated = RobotState.getInstance().LimelightsUpdated; boolean readyToShoot = noteInRobot&&limelightsUpdated; - boolean allSystemsGoodToGo = RobotState.getInstance().ShooterReady; if(limelightsUpdated) { lights.setMid(0, 40, 0); @@ -63,11 +62,8 @@ else if(!noteInRobot) { noteWasIn = true; timer.reset(); } - if(allSystemsGoodToGo) { - lights.setSides(0, 50, 0); - } else { - lights.setSides(50, 0, 0); - } + lights.setProgress(RobotState.getInstance().ShooterError / 10, 50, 0, 0, 0, 50, 0); + } lights.dataSetter(); diff --git a/src/main/java/frc/robot/subsystems/RobotState.java b/src/main/java/frc/robot/subsystems/RobotState.java index 34e22519..323a5850 100644 --- a/src/main/java/frc/robot/subsystems/RobotState.java +++ b/src/main/java/frc/robot/subsystems/RobotState.java @@ -5,9 +5,7 @@ public class RobotState { public boolean IsNoteSeen; public boolean IsNoteHeld; public boolean ShooterInRange; - public boolean ShooterReady; public double ShooterError; - public double ShooterVelocity; public boolean LimelightsUpdated; private RobotState() { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index c0bb3708..8c7b1b0c 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -159,7 +159,11 @@ public void shootRPSWithCurrent(double RPS, double supplyLimit, double statorLim * @return the speed error of the right shooter motor */ private double getError() { - return Math.abs(shooterR.getClosedLoopError().getValueAsDouble()); + return Math.abs(getSignedError()); + } + + private double getSignedError(){ + return shooterR.getClosedLoopError().getValueAsDouble(); } /** * checks if the right shooter motor is attempting to rev up @@ -242,7 +246,9 @@ public void periodic() { SmartDashboard.putNumber("shooter current left", shooterL.getSupplyCurrent().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Right shooter speed", shooterR.getVelocity().getValueAsDouble()); SmartDashboard.putNumber("Shooter/Left shooter speed", shooterL.getVelocity().getValueAsDouble()); - if(getError() Date: Thu, 4 Apr 2024 15:56:03 -0500 Subject: [PATCH 15/24] modified auto path --- .../deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path b/src/main/deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path index bdcd84e3..16c3941d 100644 --- a/src/main/deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path +++ b/src/main/deploy/pathplanner/paths/CloseAmpPickupFromMidAuto.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.15, - "rotationDegrees": -95.24734689547573, + "rotationDegrees": -91.0, "rotateFast": true } ], @@ -56,7 +56,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -101.30993247402034, + "rotation": -91.98723203197048, "rotateFast": false }, "reversed": false, From 54d6fa78847f1dde6520ff5dc8e056cab476688d Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 16:20:13 -0500 Subject: [PATCH 16/24] scaled indexer speed with same multiplier as intake speed --- src/main/java/frc/robot/settings/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index c818abff..88c5ce5d 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -330,7 +330,7 @@ public static final class ClimberConstants{ public static final class IndexerConstants{ public static final int INDEXER_MOTOR = 11; public static final int CURRENT_LIMIT = 50; - public static final double INDEXER_INTAKE_SPEED = 1;//0.903 ;//speed to pick up at 10 ft/s + public static final double INDEXER_INTAKE_SPEED = 1*0.6;//0.903 ;//speed to pick up at 10 ft/s public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive public static final double INDEXER_SHOOTING_RPS = 90; public static final double INDEXER_SHOOTING_POWER = 1*0.6; From ff507aef89f85db572cb1293d32e3dcb14007fe4 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 16:32:11 -0500 Subject: [PATCH 17/24] fix error --- src/main/java/frc/robot/commands/GroundIntake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index 6e03b43c..d480a048 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -45,7 +45,7 @@ public void execute() { // double rollerSpeed = ( - (mult * 9000)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * 10000); double sideSpeed = (IntakeConstants.INTAKE_SIDE_SPEED - (mult * IntakeConstants.INTAKE_SIDE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SIDE_SPEED); double indexerSpeed = (IndexerConstants.INDEXER_INTAKE_SPEED- (mult * IndexerConstants.INDEXER_INTAKE_SPEED)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IndexerConstants.INDEXER_INTAKE_SPEED); - // intake.intakeYes(rollerSpeed, sideSpeed); + // intake.intakeYes(rollerSpeed, sideSpeed); intake.setVelocity(rollerSpeed); SmartDashboard.putNumber("GROUNDINTAKE/roller speed", rollerSpeed); intake.intakeSideWheels(sideSpeed); From 1faafc1bdc02e2970a29ad8e507ffc76b6644d3d Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 17:17:16 -0500 Subject: [PATCH 18/24] fixed lineup for amp side auto --- src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto index 372dacea..ba96f6ea 100644 --- a/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto +++ b/src/main/deploy/pathplanner/autos/(AP)StartAmpScore3FarAmp.auto @@ -5,7 +5,7 @@ "x": 0.6842044526319189, "y": 6.690138362483131 }, - "rotation": -125.0 + "rotation": -116.0 }, "command": { "type": "sequential", From 8c4d690293af84549250023bb87f3ff6f69061ea Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 18:15:57 -0500 Subject: [PATCH 19/24] fixed pass angle for blue --- src/main/java/frc/robot/settings/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index f8005a86..eaf59886 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -495,7 +495,7 @@ public final class Field{ public static final double RED_OPPOSITE_STAGE_ROBOT_ANGLE = 32;//31 public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 148; public static final double RED_OVER_STAGE_PASS_ANGLE = 45; - public static final double BLUE_OVER_STAGE_PASS_ANGLE = -135; + public static final double BLUE_OVER_STAGE_PASS_ANGLE = 135; //angle at 60 for bounce techinque, didn't work } From 510f264c16519cc58e5c95755ae72a1dd635f0c8 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Thu, 4 Apr 2024 18:32:52 -0500 Subject: [PATCH 20/24] finished lights code --- .../java/frc/robot/commands/IndicatorLights.java | 12 ++++++++---- .../java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index bbe6137b..47b2d389 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -45,9 +45,13 @@ public void execute() { } else { lights.setMid(50, 0, 0); } - - if(timer.get() < 2){ - lights.setSides(255, 255, 255); + double time = timer.get(); + if(time < 2){ + if(time%0.2<0.1) { + lights.setSides(255, 255, 255); + } else { + lights.setSides(0, 0, 0); + } } else if(!noteInRobot) { noteWasIn = false; @@ -62,7 +66,7 @@ else if(!noteInRobot) { noteWasIn = true; timer.reset(); } - lights.setProgress(RobotState.getInstance().ShooterError / 10, 50, 0, 0, 0, 50, 0); + lights.setProgress((RobotState.getInstance().ShooterError / 50)-0.2, 50, 0, 0, 0, 50, 0); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8c7b1b0c..70ca4c4f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -163,7 +163,7 @@ private double getError() { } private double getSignedError(){ - return shooterR.getClosedLoopError().getValueAsDouble(); + return shooterR.getVelocity().getValueAsDouble()-targetVelocityR; } /** * checks if the right shooter motor is attempting to rev up From 7d099cad264291cd4be2db4fcc0d64d86a568742 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 5 Apr 2024 12:23:48 -0500 Subject: [PATCH 21/24] fixed pass angle --- src/main/java/frc/robot/settings/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index eaf59886..6762db47 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -490,7 +490,7 @@ public final class Field{ public static final double BLUE_PODIUM_ROBOT_ANGLE = 149; public static final double RED_PODIUM_ROBOT_ANGLE = 31; - public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 186; + public static final double BLUE_FAR_STAGE_ROBOT_ANGLE = 184; public static final double RED_FAR_STAGE_ROBOT_ANGLE = -4; public static final double RED_OPPOSITE_STAGE_ROBOT_ANGLE = 32;//31 public static final double BLUE_OPPOSITE_STAGE_ROBOT_ANGLE = 148; From 1e40388d730f9371c68e6ecfa81a5c4fd1265799 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 5 Apr 2024 12:31:17 -0500 Subject: [PATCH 22/24] added lights reset when you try to intake --- src/main/java/frc/robot/commands/AimShooter.java | 3 +++ src/main/java/frc/robot/commands/GroundIntake.java | 3 +++ src/main/java/frc/robot/commands/IndicatorLights.java | 8 +++++--- src/main/java/frc/robot/subsystems/RobotState.java | 1 + 4 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShooter.java b/src/main/java/frc/robot/commands/AimShooter.java index 4962456e..7fee2e42 100644 --- a/src/main/java/frc/robot/commands/AimShooter.java +++ b/src/main/java/frc/robot/commands/AimShooter.java @@ -3,6 +3,7 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.Field; @@ -61,7 +62,9 @@ public void execute() { } else if (AmpSup.getAsBoolean()) { angleShooterSubsystem.setDesiredShooterAngle(Field.AMPLIFIER_SHOOTER_ANGLE); } else if(humanPlayerSupplier.getAsBoolean()) { + frc.robot.subsystems.RobotState.getInstance().lightsReset = true; angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE); + frc.robot.subsystems.RobotState.getInstance().lightsReset = false; } else if (groundIntakeSup.getAsBoolean()){ angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE); } else if(OverStageAngleSup.getAsBoolean()) { diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index d480a048..bd17ef23 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -14,6 +14,7 @@ import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RobotState; public class GroundIntake extends Command { /** Creates a new GroundIntake. */ @@ -33,12 +34,14 @@ public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer, Drivetrain // Called when the command is initially scheduled. @Override public void initialize() { + RobotState.getInstance().lightsReset = true; angleShooter.setDesiredShooterAngle(30); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + RobotState.getInstance().lightsReset = false; double mult = 0.43; double robotSpeed = Math.sqrt(Math.pow(driveTrain.getChassisSpeeds().vxMetersPerSecond, 2) + Math.pow(driveTrain.getChassisSpeeds().vyMetersPerSecond, 2)); double rollerSpeed = (IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY - (mult * IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY)) * (robotSpeed / /*DriveConstants.MAX_VELOCITY_METERS_PER_SECOND*/2.4) + (mult * IntakeConstants.INTAKE_SPEED*IntakeConstants.INTAKE_MAX_VELOCITY); diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index 47b2d389..aeaed102 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -39,6 +39,7 @@ public void execute() { boolean noteSeenByLimelight = RobotState.getInstance().IsNoteSeen; boolean limelightsUpdated = RobotState.getInstance().LimelightsUpdated; boolean readyToShoot = noteInRobot&&limelightsUpdated; + boolean timerReset = RobotState.getInstance().lightsReset; if(limelightsUpdated) { lights.setMid(0, 40, 0); @@ -57,7 +58,7 @@ else if(!noteInRobot) { noteWasIn = false; if(noteSeenByLimelight) { lights.setSides(70, 35, 0); - } else { + } else { lights.setSides(0, 0, 0); } } @@ -67,9 +68,10 @@ else if(!noteInRobot) { timer.reset(); } lights.setProgress((RobotState.getInstance().ShooterError / 50)-0.2, 50, 0, 0, 0, 50, 0); - } - + if(noteInRobot&&timerReset) { + timer.reset(); + } lights.dataSetter(); } diff --git a/src/main/java/frc/robot/subsystems/RobotState.java b/src/main/java/frc/robot/subsystems/RobotState.java index 323a5850..8d5dafad 100644 --- a/src/main/java/frc/robot/subsystems/RobotState.java +++ b/src/main/java/frc/robot/subsystems/RobotState.java @@ -7,6 +7,7 @@ public class RobotState { public boolean ShooterInRange; public double ShooterError; public boolean LimelightsUpdated; + public boolean lightsReset; private RobotState() { From dd56cab8bbceca91d39a2bb8d5dc53c41c1ef4ad Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 5 Apr 2024 13:20:17 -0500 Subject: [PATCH 23/24] Adjusted timing on bright flashing lights --- src/main/java/frc/robot/commands/AimShooter.java | 1 - src/main/java/frc/robot/commands/GroundIntake.java | 4 +++- src/main/java/frc/robot/commands/IndicatorLights.java | 7 ++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/AimShooter.java b/src/main/java/frc/robot/commands/AimShooter.java index 7fee2e42..431d46aa 100644 --- a/src/main/java/frc/robot/commands/AimShooter.java +++ b/src/main/java/frc/robot/commands/AimShooter.java @@ -64,7 +64,6 @@ public void execute() { } else if(humanPlayerSupplier.getAsBoolean()) { frc.robot.subsystems.RobotState.getInstance().lightsReset = true; angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.HUMAN_PLAYER_ANGLE); - frc.robot.subsystems.RobotState.getInstance().lightsReset = false; } else if (groundIntakeSup.getAsBoolean()){ angleShooterSubsystem.setDesiredShooterAngle(ShooterConstants.GROUND_INTAKE_SHOOTER_ANGLE); } else if(OverStageAngleSup.getAsBoolean()) { diff --git a/src/main/java/frc/robot/commands/GroundIntake.java b/src/main/java/frc/robot/commands/GroundIntake.java index bd17ef23..1ce45630 100644 --- a/src/main/java/frc/robot/commands/GroundIntake.java +++ b/src/main/java/frc/robot/commands/GroundIntake.java @@ -34,7 +34,9 @@ public GroundIntake(IntakeSubsystem intake, IndexerSubsystem indexer, Drivetrain // Called when the command is initially scheduled. @Override public void initialize() { - RobotState.getInstance().lightsReset = true; + if (intake.isNoteSeen()){ + RobotState.getInstance().lightsReset = true; + } angleShooter.setDesiredShooterAngle(30); } diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index aeaed102..915a199a 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -39,14 +39,14 @@ public void execute() { boolean noteSeenByLimelight = RobotState.getInstance().IsNoteSeen; boolean limelightsUpdated = RobotState.getInstance().LimelightsUpdated; boolean readyToShoot = noteInRobot&&limelightsUpdated; - boolean timerReset = RobotState.getInstance().lightsReset; - + if(limelightsUpdated) { lights.setMid(0, 40, 0); } else { lights.setMid(50, 0, 0); } double time = timer.get(); + boolean timerReset = RobotState.getInstance().lightsReset; if(time < 2){ if(time%0.2<0.1) { lights.setSides(255, 255, 255); @@ -70,7 +70,8 @@ else if(!noteInRobot) { lights.setProgress((RobotState.getInstance().ShooterError / 50)-0.2, 50, 0, 0, 0, 50, 0); } if(noteInRobot&&timerReset) { - timer.reset(); + timer.reset(); + RobotState.getInstance().lightsReset = false; } lights.dataSetter(); } From 125258e136b098f90e67299cf07f95dc278dd520 Mon Sep 17 00:00:00 2001 From: Xiaohan Date: Fri, 5 Apr 2024 13:26:08 -0500 Subject: [PATCH 24/24] Another change --- src/main/java/frc/robot/commands/IndicatorLights.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/IndicatorLights.java b/src/main/java/frc/robot/commands/IndicatorLights.java index 915a199a..c2211863 100644 --- a/src/main/java/frc/robot/commands/IndicatorLights.java +++ b/src/main/java/frc/robot/commands/IndicatorLights.java @@ -70,7 +70,9 @@ else if(!noteInRobot) { lights.setProgress((RobotState.getInstance().ShooterError / 50)-0.2, 50, 0, 0, 0, 50, 0); } if(noteInRobot&&timerReset) { - timer.reset(); + if (time > 2){ + timer.reset(); + } RobotState.getInstance().lightsReset = false; } lights.dataSetter();