From 13d56c8251c099cabc6fc878c808b6d589160b5d Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sat, 30 Mar 2024 11:12:10 -0700 Subject: [PATCH 01/10] driverOI handlePre --- src/main/java/com/team766/robot/reva/DriverOI.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index d513e81d1..e25621eaa 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -59,8 +59,8 @@ public DriverOI( > 0); } - public void handleOI(Context context) { - + @Override + protected void handlePre() { // Negative because forward is negative in driver station leftJoystickX = -createJoystickDeadzone(leftJoystick.getAxis(InputConstants.AXIS_FORWARD_BACKWARD)) @@ -74,6 +74,11 @@ public void handleOI(Context context) { -createJoystickDeadzone(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer + } + + @Override + protected void handleOI(Context context) { + if (leftJoystick.getButtonPressed(InputConstants.BUTTON_RESET_GYRO)) { drive.resetGyro(); } From 8cd63105a16de30509a8dd22a578d3b3abd68372 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sat, 30 Mar 2024 11:14:45 -0700 Subject: [PATCH 02/10] changed autons to old shootNow --- .../team766/robot/reva/procedures/FourPieceAmpSide.java | 8 ++++---- .../robot/reva/procedures/MidfieldAutonSourceSide.java | 4 ++-- .../team766/robot/reva/procedures/OldThreePieceAuton.java | 6 +++--- .../robot/reva/procedures/OneShotBackupSourceSide.java | 2 +- .../com/team766/robot/reva/procedures/ShootOnePiece.java | 2 +- .../robot/reva/procedures/ThreePieceAutonAmpSide.java | 6 +++--- 6 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/procedures/FourPieceAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/FourPieceAmpSide.java index 64b8b83ae..33bc91253 100644 --- a/src/main/java/com/team766/robot/reva/procedures/FourPieceAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/FourPieceAmpSide.java @@ -8,15 +8,15 @@ public class FourPieceAmpSide extends PathSequenceAuto { public FourPieceAmpSide() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("3 Piece 1"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("Alternate 3 Piece 2"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("4 Piece 3"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java b/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java index feac5d02b..ec58d4bd5 100644 --- a/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/MidfieldAutonSourceSide.java @@ -8,10 +8,10 @@ public class MidfieldAutonSourceSide extends PathSequenceAuto { public MidfieldAutonSourceSide() { super(Robot.drive, new Pose2d(0.71, 4.39, Rotation2d.fromDegrees(-60))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("MidfieldSource 1"); addPath("MidfieldSource 2"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/OldThreePieceAuton.java b/src/main/java/com/team766/robot/reva/procedures/OldThreePieceAuton.java index e888137ce..4fe9ea523 100644 --- a/src/main/java/com/team766/robot/reva/procedures/OldThreePieceAuton.java +++ b/src/main/java/com/team766/robot/reva/procedures/OldThreePieceAuton.java @@ -8,13 +8,13 @@ public class OldThreePieceAuton extends PathSequenceAuto { public OldThreePieceAuton() { super(Robot.drive, new Pose2d(2.00, 6.75, Rotation2d.fromDegrees(33))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("3 Piece 1"); addPath("3 Piece 2"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("3 Piece 3"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/OneShotBackupSourceSide.java b/src/main/java/com/team766/robot/reva/procedures/OneShotBackupSourceSide.java index c696c698a..2f75a444c 100644 --- a/src/main/java/com/team766/robot/reva/procedures/OneShotBackupSourceSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/OneShotBackupSourceSide.java @@ -8,7 +8,7 @@ public class OneShotBackupSourceSide extends PathSequenceAuto { public OneShotBackupSourceSide() { super(Robot.drive, new Pose2d(0.72, 4.41, Rotation2d.fromDegrees(-60))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addPath("BackupSource"); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootOnePiece.java b/src/main/java/com/team766/robot/reva/procedures/ShootOnePiece.java index 98389c5dc..2448edfd3 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootOnePiece.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootOnePiece.java @@ -8,6 +8,6 @@ public class ShootOnePiece extends PathSequenceAuto { public ShootOnePiece() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java index b950d7c6a..b74213982 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java @@ -8,12 +8,12 @@ public class ThreePieceAutonAmpSide extends PathSequenceAuto { public ThreePieceAutonAmpSide() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("3 Piece 1"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("Alternate 3 Piece 2"); - addProcedure(new RotateAndShootNow()); + addProcedure(new ShootNow()); } } From 1f54295001e892cbc15d7b907bac6d300c91cd97 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sat, 30 Mar 2024 14:00:29 -0700 Subject: [PATCH 03/10] adjusted auton path --- .../pathplanner/paths/3 Piece 1 Stop 1.path | 58 +++++++++++++++++++ .../pathplanner/paths/3 Piece 1 Stop 2.path | 52 +++++++++++++++++ .../deploy/pathplanner/paths/3 Piece 1.path | 8 +-- .../deploy/pathplanner/paths/3 Piece 2.path | 8 +-- .../deploy/pathplanner/paths/4 Piece 3.path | 4 +- .../paths/Alternate 3 Piece 2.path | 24 ++++---- .../common/procedures/PathSequenceAuto.java | 6 +- .../team766/robot/reva/AutonomousModes.java | 3 +- .../reva/VisionUtil/VisionPIDProcedure.java | 4 +- .../mechanisms/ForwardApriltagCamera.java | 28 ++++----- .../robot/reva/mechanisms/Shooter.java | 5 +- .../robot/reva/mechanisms/Shoulder.java | 3 +- .../robot/reva/procedures/DoNothing.java | 9 +++ .../robot/reva/procedures/ShootNow.java | 2 +- .../procedures/ThreePieceAutonAmpSide.java | 3 +- 15 files changed, 170 insertions(+), 47 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/3 Piece 1 Stop 1.path create mode 100644 src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path create mode 100644 src/main/java/com/team766/robot/reva/procedures/DoNothing.java diff --git a/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 1.path b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 1.path new file mode 100644 index 000000000..8b2dfd13c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 1.path @@ -0,0 +1,58 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.7483342975945213, + "y": 6.675397934129203 + }, + "prevControl": null, + "nextControl": { + "x": 1.406194437388875, + "y": 7.089841508386721 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1015855040969345, + "y": 6.522126854438895 + }, + "prevControl": { + "x": 1.7969436448132157, + "y": 6.6332944101073394 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 Piece 1 Stop 1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 37.38608672087997, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 35.26397392088418, + "rotateFast": true + }, + "reversed": false, + "folder": "RevA", + "previewStartingState": { + "rotation": 59.08161572041965, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path new file mode 100644 index 000000000..acfc5c89e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.1015855040969345, + "y": 6.522126854438895 + }, + "prevControl": null, + "nextControl": { + "x": 2.328968287566205, + "y": 6.561759988334791 + }, + "isLocked": false, + "linkedName": "3 Piece 1 Stop 1" + }, + { + "anchor": { + "x": 2.6682193623646513, + "y": 6.815497558772896 + }, + "prevControl": { + "x": 2.4941756272401436, + "y": 6.711028560946012 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "3 Piece 1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 36.0, + "rotateFast": false + }, + "reversed": false, + "folder": "RevA", + "previewStartingState": { + "rotation": 35.26, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/3 Piece 1.path b/src/main/deploy/pathplanner/paths/3 Piece 1.path index 6546313d3..f3feaf98f 100644 --- a/src/main/deploy/pathplanner/paths/3 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/3 Piece 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.8139011185429776, - "y": 7.04708475620655 + "x": 2.6682193623646513, + "y": 6.815497558772896 }, "prevControl": { - "x": 1.778950178270727, - "y": 7.121671535009737 + "x": 1.6332684220924008, + "y": 6.890084337576083 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/3 Piece 2.path b/src/main/deploy/pathplanner/paths/3 Piece 2.path index 104216349..260c6e9a7 100644 --- a/src/main/deploy/pathplanner/paths/3 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/3 Piece 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.8139011185429776, - "y": 7.04708475620655 + "x": 2.6682193623646513, + "y": 6.815497558772896 }, "prevControl": null, "nextControl": { - "x": 2.7965014957807695, - "y": 6.4770541115156295 + "x": 2.650819739602443, + "y": 6.245466914081976 }, "isLocked": false, "linkedName": "3 Piece 1" diff --git a/src/main/deploy/pathplanner/paths/4 Piece 3.path b/src/main/deploy/pathplanner/paths/4 Piece 3.path index 5870fa4ff..a1505de03 100644 --- a/src/main/deploy/pathplanner/paths/4 Piece 3.path +++ b/src/main/deploy/pathplanner/paths/4 Piece 3.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.623633571341217, + "x": 2.6682193623646513, "y": 5.646385431659753 }, "prevControl": null, "nextControl": { - "x": 1.6761661106788028, + "x": 1.7207519017022372, "y": 5.204122117306238 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path b/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path index a35e01423..548a9620c 100644 --- a/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path @@ -3,40 +3,40 @@ "waypoints": [ { "anchor": { - "x": 2.8139011185429776, - "y": 7.04708475620655 + "x": 2.6682193623646513, + "y": 6.815497558772896 }, "prevControl": null, "nextControl": { - "x": 2.196897430831899, - "y": 7.214845624135694 + "x": 2.0512156746535726, + "y": 6.98325842670204 }, "isLocked": false, "linkedName": "3 Piece 1" }, { "anchor": { - "x": 2.8438902294471258, - "y": 5.646385431659753 + "x": 2.9128948767156797, + "y": 5.502794005588011 }, "prevControl": { - "x": 2.5489874081600017, - "y": 5.752742852707928 + "x": 2.6179920554285556, + "y": 5.609151426636186 }, "nextControl": { - "x": 2.8457928338581193, - "y": 5.645699252743102 + "x": 2.9147974811266733, + "y": 5.5021078266713594 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.623633571341217, + "x": 2.6682193623646513, "y": 5.646385431659753 }, "prevControl": { - "x": 2.636163675105128, + "x": 2.6807494661285625, "y": 5.638427925003999 }, "nextControl": null, diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index a0fec9a3a..1f9f5923d 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -102,7 +102,7 @@ public final void run(Context context) { return; } - context.startAsync(new MoveClimbersToBottom()); + // context.startAsync(new MoveClimbersToBottom()); context.takeOwnership(drive); // if (!visionSpeakerHelper.updateTarget(context)) { drive.setCurrentPosition( @@ -110,10 +110,10 @@ public final void run(Context context) { // } context.takeOwnership(drive); drive.resetGyro( - (shouldFlipAuton + ((shouldFlipAuton ? GeometryUtil.flipFieldRotation(initialPosition.getRotation()) : initialPosition.getRotation()) - .getDegrees()); + .getDegrees())); for (RunnableWithContext pathItem : pathItems) { pathItem.run(context); context.yield(); diff --git a/src/main/java/com/team766/robot/reva/AutonomousModes.java b/src/main/java/com/team766/robot/reva/AutonomousModes.java index 4c21521e4..713490ca1 100644 --- a/src/main/java/com/team766/robot/reva/AutonomousModes.java +++ b/src/main/java/com/team766/robot/reva/AutonomousModes.java @@ -21,6 +21,7 @@ public class AutonomousModes { new AutonomousMode("ClimbersDown", () -> new MoveClimbersToBottom()), new AutonomousMode("ShootOnePiece", () -> new ShootOnePiece()), new AutonomousMode("AltThreePiece", () -> new ThreePieceAutonAmpSide()), - new AutonomousMode("ShootNow", () -> new RotateAndShootNow()) + new AutonomousMode("ShootNow", () -> new RotateAndShootNow()), + new AutonomousMode("DoNothing", () -> new RotateAndShootNow()) }; } diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java index 1a3ba9314..f9743d187 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java @@ -23,8 +23,8 @@ public abstract class VisionPIDProcedure extends Procedure { new AnywhereScoringPosition(1.7201, 5600, 22.205946); private static AnywhereScoringPosition a3 = new AnywhereScoringPosition(1.9506, 5600, 23.516); private static AnywhereScoringPosition a4 = new AnywhereScoringPosition(2.072, 5600, 27.32); - private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 30.109); - private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 30.8987); + private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 29.109); + private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 29.8987); private static AnywhereScoringPosition a7 = new AnywhereScoringPosition(2.6942, 5600, 32.699); private static AnywhereScoringPosition a8 = new AnywhereScoringPosition(2.8657, 5600, 34.103733); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 89fc356ff..5497e4620 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -37,21 +37,21 @@ public GrayScaleCamera getCamera() { } public void run() { - if (tagId == -1) { - Optional alliance = DriverStation.getAlliance(); + // if (tagId == -1) { + // Optional alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) { - if (alliance.get().equals(Alliance.Blue)) { - tagId = 7; - } else { - tagId = 4; - } - Robot.lights.signalCameraConnected(); - } else { - LoggerExceptionUtils.logException( - new AprilTagGeneralCheckedException("Couldn't find alliance correctly")); - } - } + // if (alliance.isPresent()) { + // if (alliance.get().equals(Alliance.Blue)) { + // tagId = 7; + // } else { + // tagId = 4; + // } + // Robot.lights.signalCameraConnected(); + // } else { + // LoggerExceptionUtils.logException( + // new AprilTagGeneralCheckedException("Couldn't find alliance correctly")); + // } + // } try { if (tagId == -1) { diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index 9b91e3f9c..aa3dddb55 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java @@ -9,6 +9,7 @@ import com.team766.hal.MotorController.ControlMode; import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Shooter extends Mechanism { private static final double DEFAULT_SPEED = @@ -84,8 +85,8 @@ public void nudgeDown() { public void run() { if (speedUpdated || rateLimiter.next()) { // SmartDashboard.putNumber("[SHOOTER TARGET SPEED]", shouldRun ? targetSpeed : 0.0); - // SmartDashboard.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); - // SmartDashboard.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); + SmartDashboard.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); + SmartDashboard.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); // SmartDashboard.putNumber( // "[SHOOTER] Top Motor Current", MotorUtil.getCurrentUsage(shooterMotorTop)); // SmartDashboard.putNumber( diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 2d60341d0..11a1dd519 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -13,6 +13,7 @@ import com.team766.hal.RobotProvider; import com.team766.hal.wpilib.REVThroughBoreDutyCycleEncoder; import com.team766.library.ValueProvider; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Shoulder extends Mechanism { public enum ShoulderPosition { @@ -152,7 +153,7 @@ public void run() { leftMotor.setSensorPosition(convertedPos); encoderInitializationCount++; } - // SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); + SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); // SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle); // SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); // SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations); diff --git a/src/main/java/com/team766/robot/reva/procedures/DoNothing.java b/src/main/java/com/team766/robot/reva/procedures/DoNothing.java new file mode 100644 index 000000000..6861af213 --- /dev/null +++ b/src/main/java/com/team766/robot/reva/procedures/DoNothing.java @@ -0,0 +1,9 @@ +package com.team766.robot.reva.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; + +public class DoNothing extends Procedure { + + public void run(final Context context) {} +} diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java index 32ed349a2..ade80594c 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java @@ -106,7 +106,7 @@ public void run(Context context) { // SmartDashboard.putNumber("[ANGLE PID OUTPUT]", anglePID.getOutput()); // SmartDashboard.putNumber("[ANGLE PID ROTATION]", angle); - context.waitFor(() -> Robot.shoulder.isFinished()); + context.waitForConditionOrTimeout(() -> Robot.shoulder.isFinished(), 1); context.releaseOwnership(Robot.shooter); Robot.lights.signalFinishingShootingProcedure(); diff --git a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java index b74213982..66595716e 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java @@ -8,9 +8,10 @@ public class ThreePieceAutonAmpSide extends PathSequenceAuto { public ThreePieceAutonAmpSide() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); + addPath("3 Piece 1 Stop 1"); addProcedure(new ShootNow()); addProcedure(new AutoIntake()); - addPath("3 Piece 1"); + addPath("3 Piece 1 Stop 2"); addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("Alternate 3 Piece 2"); From 2fef02a83b2e691f7579f5870bf2ff7ab81b3815 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sat, 30 Mar 2024 16:27:03 -0700 Subject: [PATCH 04/10] re-added DriverShootVelocityIntake in driverOI changed auton to shoot off of subwoofer for first shot --- .../pathplanner/paths/3 Piece 1 Stop 2.path | 8 ++++---- .../deploy/pathplanner/paths/3 Piece 1.path | 14 +++++++------- .../deploy/pathplanner/paths/3 Piece 2.path | 8 ++++---- .../pathplanner/paths/Alternate 3 Piece 2.path | 8 ++++---- .../java/com/team766/robot/reva/DriverOI.java | 17 ++++++++++++++++- .../reva/VisionUtil/VisionPIDProcedure.java | 4 ++-- .../team766/robot/reva/mechanisms/Intake.java | 3 ++- .../team766/robot/reva/mechanisms/Shoulder.java | 4 ++-- .../robot/reva/procedures/ShootAtSubwoofer.java | 15 +++++++++++++++ .../team766/robot/reva/procedures/ShootNow.java | 14 ++++++++++++++ .../reva/procedures/ShootVelocityAndIntake.java | 2 +- .../reva/procedures/ThreePieceAutonAmpSide.java | 6 +++--- 12 files changed, 74 insertions(+), 29 deletions(-) create mode 100644 src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java diff --git a/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path index acfc5c89e..4de4ddbc6 100644 --- a/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path +++ b/src/main/deploy/pathplanner/paths/3 Piece 1 Stop 2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.6682193623646513, - "y": 6.815497558772896 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": { - "x": 2.4941756272401436, - "y": 6.711028560946012 + "x": 2.6349878644201734, + "y": 6.817492873874638 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/3 Piece 1.path b/src/main/deploy/pathplanner/paths/3 Piece 1.path index f3feaf98f..040e9239c 100644 --- a/src/main/deploy/pathplanner/paths/3 Piece 1.path +++ b/src/main/deploy/pathplanner/paths/3 Piece 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 1.406194437388875, - "y": 7.089841508386721 + "x": 2.3770102493908696, + "y": 6.456447732339909 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.6682193623646513, - "y": 6.815497558772896 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": { - "x": 1.6332684220924008, - "y": 6.890084337576083 + "x": 2.2404261799264344, + "y": 6.638104544727609 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -8.825474574526524, + "rotation": 36.0, "rotateFast": true }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/3 Piece 2.path b/src/main/deploy/pathplanner/paths/3 Piece 2.path index 260c6e9a7..d0954b51d 100644 --- a/src/main/deploy/pathplanner/paths/3 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/3 Piece 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.6682193623646513, - "y": 6.815497558772896 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": null, "nextControl": { - "x": 2.650819739602443, - "y": 6.245466914081976 + "x": 2.791631976782473, + "y": 6.351931227010602 }, "isLocked": false, "linkedName": "3 Piece 1" diff --git a/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path b/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path index 548a9620c..c79c3bae8 100644 --- a/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path +++ b/src/main/deploy/pathplanner/paths/Alternate 3 Piece 2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.6682193623646513, - "y": 6.815497558772896 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": null, "nextControl": { - "x": 2.0512156746535726, - "y": 6.98325842670204 + "x": 2.1920279118336023, + "y": 7.089722739630666 }, "isLocked": false, "linkedName": "3 Piece 1" diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index e25621eaa..fa7f4290e 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -12,6 +12,7 @@ import com.team766.robot.reva.mechanisms.Shooter; import com.team766.robot.reva.mechanisms.Shoulder; import com.team766.robot.reva.procedures.DriverShootNow; +import com.team766.robot.reva.procedures.DriverShootVelocityAndIntake; public class DriverOI extends OIFragment { @@ -108,13 +109,27 @@ protected void handleOI(Context context) { context.takeOwnership(drive); context.takeOwnership(intake); - Robot.intake.stop(); + intake.stop(); drive.stopDrive(); context.releaseOwnership(drive); context.releaseOwnership(intake); } + if (rightJoystick.getButtonPressed(InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) { + + visionContext = context.startAsync(new DriverShootVelocityAndIntake()); + + } else if (rightJoystick.getButtonReleased( + InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) { + + visionContext.stop(); + + context.takeOwnership(intake); + intake.stop(); + context.releaseOwnership(intake); + } + // Moves the robot if there are joystick inputs if (movingJoysticks.isTriggering()) { double drivingCoefficient = 1; diff --git a/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java b/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java index f9743d187..d43ea3570 100644 --- a/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java +++ b/src/main/java/com/team766/robot/reva/VisionUtil/VisionPIDProcedure.java @@ -23,8 +23,8 @@ public abstract class VisionPIDProcedure extends Procedure { new AnywhereScoringPosition(1.7201, 5600, 22.205946); private static AnywhereScoringPosition a3 = new AnywhereScoringPosition(1.9506, 5600, 23.516); private static AnywhereScoringPosition a4 = new AnywhereScoringPosition(2.072, 5600, 27.32); - private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 29.109); - private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 29.8987); + private static AnywhereScoringPosition a5 = new AnywhereScoringPosition(2.29161, 5600, 31.109); + private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 31.8987); private static AnywhereScoringPosition a7 = new AnywhereScoringPosition(2.6942, 5600, 32.699); private static AnywhereScoringPosition a8 = new AnywhereScoringPosition(2.8657, 5600, 34.103733); diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java index 4868d1a5a..84c26e14c 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java @@ -10,6 +10,7 @@ import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.ValueProvider; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Intake extends Mechanism { @@ -98,7 +99,7 @@ public void nudgeDown() { public void run() { // SmartDashboard.putString("[INTAKE]", state.toString()); - // SmartDashboard.putNumber("[INTAKE POWER]", intakePower); + SmartDashboard.putNumber("[INTAKE POWER]", intakePower); // SmartDashboard.putNumber("[INTAKE] Current", MotorUtil.getCurrentUsage(intakeMotor)); // SmartDashboard.putNumber("Prox Sensor", sensor.getRange()); } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 11a1dd519..13c0fbbc6 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -137,7 +137,7 @@ public void rotate(double angle) { } public boolean isFinished() { - return Math.abs(getAngle() - targetAngle) < 1; + return Math.abs(getAngle() - targetAngle) < 2.5; } @Override @@ -170,7 +170,7 @@ public void run() { // SmartDashboard.putNumber( // "[SHOULDER] Right Motor Stator Current", // MotorUtil.getStatorCurrentUsage(rightMotor)); - // SmartDashboard.putBoolean("Shoulder at angle", isFinished()); + SmartDashboard.putBoolean("Shoulder at angle", isFinished()); TalonFX leftTalon = (TalonFX) leftMotor; // SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get()); diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java b/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java new file mode 100644 index 000000000..f0511ebf3 --- /dev/null +++ b/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java @@ -0,0 +1,15 @@ +package com.team766.robot.reva.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.reva.mechanisms.Shoulder.ShoulderPosition; +import com.team766.robot.reva.Robot; + +public class ShootAtSubwoofer extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.shoulder); + Robot.shoulder.rotate(ShoulderPosition.SHOOT_LOW); + context.releaseOwnership(Robot.shoulder); + new ShootVelocityAndIntake().run(context); + } +} diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java index ade80594c..c37dc2076 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootNow.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootNow.java @@ -40,6 +40,9 @@ public void run(Context context) { Robot.drive.stopDrive(); Transform3d toUse; + + context.waitForConditionOrTimeout(() -> seesTarget(), 1.0); + try { toUse = getTransform3dOfRobotToTag(); @@ -119,4 +122,15 @@ private Transform3d getTransform3dOfRobotToTag() throws AprilTagGeneralCheckedEx return GrayScaleCamera.getBestTargetTransform3d(toUse.getTrackedTargetWithID(tagId)); } + + private boolean seesTarget() { + GrayScaleCamera toUse = Robot.forwardApriltagCamera.getCamera(); + + try { + toUse.getTrackedTargetWithID(tagId); + return true; + } catch (Exception e) { + return false; + } + } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java index 3104003af..7fc0d407a 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java @@ -27,7 +27,7 @@ public void run(Context context) { context.waitForSeconds(1.5); new IntakeStop().run(context); - Robot.shooter.stop(); + // Robot.shooter.stop(); Robot.lights.signalFinishedShootingProcedure(); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java index 66595716e..240995e55 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java +++ b/src/main/java/com/team766/robot/reva/procedures/ThreePieceAutonAmpSide.java @@ -8,10 +8,10 @@ public class ThreePieceAutonAmpSide extends PathSequenceAuto { public ThreePieceAutonAmpSide() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); - addPath("3 Piece 1 Stop 1"); - addProcedure(new ShootNow()); + // addPath("3 Piece 1"); + addProcedure(new ShootAtSubwoofer()); addProcedure(new AutoIntake()); - addPath("3 Piece 1 Stop 2"); + addPath("3 Piece 1"); addProcedure(new ShootNow()); addProcedure(new AutoIntake()); addPath("Alternate 3 Piece 2"); From fa451e16d4b4389d834a5bc432f06d16139fa6aa Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 13:25:10 -0700 Subject: [PATCH 05/10] added timeout to auto intake and removed incorrect context ownership --- .../com/team766/robot/common/procedures/PathSequenceAuto.java | 2 +- src/main/java/com/team766/robot/reva/procedures/AutoIntake.java | 2 +- .../team766/robot/reva/procedures/ShootVelocityAndIntake.java | 1 - 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 1f9f5923d..2bfd70679 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -102,7 +102,7 @@ public final void run(Context context) { return; } - // context.startAsync(new MoveClimbersToBottom()); + context.startAsync(new MoveClimbersToBottom()); context.takeOwnership(drive); // if (!visionSpeakerHelper.updateTarget(context)) { drive.setCurrentPosition( diff --git a/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java b/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java index fdb871150..675da24bc 100644 --- a/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java @@ -9,7 +9,7 @@ public class AutoIntake extends Procedure { public void run(Context context) { context.takeOwnership(Robot.shoulder); Robot.shoulder.rotate(ShoulderPosition.BOTTOM); - context.waitFor(Robot.shoulder::isFinished); + context.waitForConditionOrTimeout(Robot.shoulder::isFinished, 1.5); context.startAsync(new IntakeUntilIn()); } } diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java index 7fc0d407a..1f37151fb 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java @@ -22,7 +22,6 @@ public void run(Context context) { Robot.shooter.shoot(speed); context.waitForConditionOrTimeout(Robot.shooter::isCloseToExpectedSpeed, 1.5); - context.takeOwnership(Robot.intake); new IntakeIn().run(context); context.waitForSeconds(1.5); From 0593830fff541a0345d99d5afcce2306a34a2f91 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 14:16:32 -0700 Subject: [PATCH 06/10] resets gyro to 180 degrees of current after auton is done --- .../robot/common/procedures/PathSequenceAuto.java | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 2bfd70679..228dee806 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -108,15 +108,21 @@ public final void run(Context context) { drive.setCurrentPosition( shouldFlipAuton ? GeometryUtil.flipFieldPose(initialPosition) : initialPosition); // } - context.takeOwnership(drive); + // context.takeOwnership(drive); drive.resetGyro( - ((shouldFlipAuton + (shouldFlipAuton ? GeometryUtil.flipFieldRotation(initialPosition.getRotation()) : initialPosition.getRotation()) - .getDegrees())); + .getDegrees()); for (RunnableWithContext pathItem : pathItems) { pathItem.run(context); context.yield(); } + + // TODO: For some reason, the gyro is consistenty 180 degrees from expected in teleop + // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of current angle + context.takeOwnership(drive); + drive.resetGyro(180 + drive.getHeading()); + context.releaseOwnership(drive); } } From e8f4e73657a6fe65cf1366f46f52906fdd26c593 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 14:31:42 -0700 Subject: [PATCH 07/10] Spotless --- .../robot/common/procedures/PathSequenceAuto.java | 3 ++- src/main/java/com/team766/robot/reva/DriverOI.java | 5 ++--- .../robot/reva/procedures/ShootAtSubwoofer.java | 14 +++++++------- 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index 228dee806..b424a29b0 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -120,7 +120,8 @@ public final void run(Context context) { } // TODO: For some reason, the gyro is consistenty 180 degrees from expected in teleop - // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of current angle + // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of + // current angle context.takeOwnership(drive); drive.resetGyro(180 + drive.getHeading()); context.releaseOwnership(drive); diff --git a/src/main/java/com/team766/robot/reva/DriverOI.java b/src/main/java/com/team766/robot/reva/DriverOI.java index fa7f4290e..0423d2287 100644 --- a/src/main/java/com/team766/robot/reva/DriverOI.java +++ b/src/main/java/com/team766/robot/reva/DriverOI.java @@ -74,8 +74,7 @@ protected void handlePre() { rightJoystickY = -createJoystickDeadzone(rightJoystick.getAxis(InputConstants.AXIS_LEFT_RIGHT)) * ControlConstants.MAX_ROTATIONAL_VELOCITY; // For steer - - } + } @Override protected void handleOI(Context context) { @@ -121,7 +120,7 @@ protected void handleOI(Context context) { visionContext = context.startAsync(new DriverShootVelocityAndIntake()); } else if (rightJoystick.getButtonReleased( - InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) { + InputConstants.BUTTON_START_SHOOTING_PROCEDURE)) { visionContext.stop(); diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java b/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java index f0511ebf3..a7631d3c6 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java @@ -2,14 +2,14 @@ import com.team766.framework.Context; import com.team766.framework.Procedure; -import com.team766.robot.reva.mechanisms.Shoulder.ShoulderPosition; import com.team766.robot.reva.Robot; +import com.team766.robot.reva.mechanisms.Shoulder.ShoulderPosition; public class ShootAtSubwoofer extends Procedure { - public void run(Context context) { - context.takeOwnership(Robot.shoulder); - Robot.shoulder.rotate(ShoulderPosition.SHOOT_LOW); - context.releaseOwnership(Robot.shoulder); - new ShootVelocityAndIntake().run(context); - } + public void run(Context context) { + context.takeOwnership(Robot.shoulder); + Robot.shoulder.rotate(ShoulderPosition.SHOOT_LOW); + context.releaseOwnership(Robot.shoulder); + new ShootVelocityAndIntake().run(context); + } } From c67147fd56f0f9e4044b24bfb7fc6768d0c55185 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 18:50:26 -0700 Subject: [PATCH 08/10] stop shooter at the end of an auton and other context stuff --- .../team766/robot/common/procedures/PathSequenceAuto.java | 5 +++++ src/main/java/com/team766/robot/reva/AutonomousModes.java | 3 +-- .../java/com/team766/robot/reva/procedures/AutoIntake.java | 1 + .../robot/reva/procedures/DriverShootVelocityAndIntake.java | 1 - .../java/com/team766/robot/reva/procedures/IntakeIn.java | 1 + 5 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java index b424a29b0..ac3d336d7 100644 --- a/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java +++ b/src/main/java/com/team766/robot/common/procedures/PathSequenceAuto.java @@ -10,6 +10,7 @@ import com.team766.robot.common.constants.ConfigConstants; import com.team766.robot.common.constants.PathPlannerConstants; import com.team766.robot.common.mechanisms.Drive; +import com.team766.robot.reva.Robot; import com.team766.robot.reva.VisionUtil.VisionSpeakerHelper; import com.team766.robot.reva.procedures.MoveClimbersToBottom; import edu.wpi.first.math.geometry.Pose2d; @@ -119,6 +120,10 @@ public final void run(Context context) { context.yield(); } + context.takeOwnership(Robot.shooter); + Robot.shooter.stop(); + context.releaseOwnership(Robot.shooter); + // TODO: For some reason, the gyro is consistenty 180 degrees from expected in teleop // TODO: We should figure out why after EBR but for now we can just reset the gyro to 180 of // current angle diff --git a/src/main/java/com/team766/robot/reva/AutonomousModes.java b/src/main/java/com/team766/robot/reva/AutonomousModes.java index 713490ca1..4c21521e4 100644 --- a/src/main/java/com/team766/robot/reva/AutonomousModes.java +++ b/src/main/java/com/team766/robot/reva/AutonomousModes.java @@ -21,7 +21,6 @@ public class AutonomousModes { new AutonomousMode("ClimbersDown", () -> new MoveClimbersToBottom()), new AutonomousMode("ShootOnePiece", () -> new ShootOnePiece()), new AutonomousMode("AltThreePiece", () -> new ThreePieceAutonAmpSide()), - new AutonomousMode("ShootNow", () -> new RotateAndShootNow()), - new AutonomousMode("DoNothing", () -> new RotateAndShootNow()) + new AutonomousMode("ShootNow", () -> new RotateAndShootNow()) }; } diff --git a/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java b/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java index 675da24bc..fdfb56a31 100644 --- a/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/AutoIntake.java @@ -9,6 +9,7 @@ public class AutoIntake extends Procedure { public void run(Context context) { context.takeOwnership(Robot.shoulder); Robot.shoulder.rotate(ShoulderPosition.BOTTOM); + context.releaseOwnership(Robot.shoulder); context.waitForConditionOrTimeout(Robot.shoulder::isFinished, 1.5); context.startAsync(new IntakeUntilIn()); } diff --git a/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java index 83da59eb4..1a425f9cd 100644 --- a/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/DriverShootVelocityAndIntake.java @@ -10,7 +10,6 @@ public void run(Context context) { context.waitForConditionOrTimeout(Robot.shooter::isCloseToExpectedSpeed, 1); - context.takeOwnership(Robot.intake); new IntakeIn().run(context); // Does not stop intake here so driver can stop when button released diff --git a/src/main/java/com/team766/robot/reva/procedures/IntakeIn.java b/src/main/java/com/team766/robot/reva/procedures/IntakeIn.java index ea2468eae..776a9605b 100644 --- a/src/main/java/com/team766/robot/reva/procedures/IntakeIn.java +++ b/src/main/java/com/team766/robot/reva/procedures/IntakeIn.java @@ -8,5 +8,6 @@ public class IntakeIn extends Procedure { public void run(Context context) { context.takeOwnership(Robot.intake); Robot.intake.in(); + context.releaseOwnership(Robot.intake); } } From c4ded816ac613e24fa0f2fee3a18372b36a8ea12 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 18:56:29 -0700 Subject: [PATCH 09/10] comments and used constants --- .../team766/robot/reva/mechanisms/ForwardApriltagCamera.java | 5 +++-- .../robot/reva/procedures/ShootVelocityAndIntake.java | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 5497e4620..1c2f90e6c 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -5,6 +5,7 @@ import com.team766.framework.Mechanism; import com.team766.logging.LoggerExceptionUtils; import com.team766.robot.reva.Robot; +import com.team766.robot.reva.constants.VisionConstants; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -59,9 +60,9 @@ public void run() { if (alliance.isPresent()) { if (alliance.get().equals(Alliance.Blue)) { - tagId = 7; + tagId = VisionConstants.MAIN_BLUE_SPEAKER_TAG; } else { - tagId = 4; + tagId = VisionConstants.MAIN_RED_SPEAKER_TAG; } Robot.lights.signalCameraConnected(); } else { diff --git a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java index 1f37151fb..03c07a091 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java @@ -26,7 +26,8 @@ public void run(Context context) { context.waitForSeconds(1.5); new IntakeStop().run(context); - // Robot.shooter.stop(); Robot.lights.signalFinishedShootingProcedure(); + + // Shooter stopped at the end of auton } } From ce2a1a9476f62ca6c4c8e22fe0f3034e8d1e89e3 Mon Sep 17 00:00:00 2001 From: qntmcube <100043837+qntmcube@users.noreply.github.com> Date: Sun, 31 Mar 2024 22:25:49 -0700 Subject: [PATCH 10/10] removed logging and deleted commented out stuff --- .../reva/mechanisms/ForwardApriltagCamera.java | 15 --------------- .../com/team766/robot/reva/mechanisms/Intake.java | 3 +-- .../team766/robot/reva/mechanisms/Shooter.java | 5 ++--- .../team766/robot/reva/mechanisms/Shoulder.java | 5 ++--- 4 files changed, 5 insertions(+), 23 deletions(-) diff --git a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 1c2f90e6c..7caa39d3f 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java @@ -38,21 +38,6 @@ public GrayScaleCamera getCamera() { } public void run() { - // if (tagId == -1) { - // Optional alliance = DriverStation.getAlliance(); - - // if (alliance.isPresent()) { - // if (alliance.get().equals(Alliance.Blue)) { - // tagId = 7; - // } else { - // tagId = 4; - // } - // Robot.lights.signalCameraConnected(); - // } else { - // LoggerExceptionUtils.logException( - // new AprilTagGeneralCheckedException("Couldn't find alliance correctly")); - // } - // } try { if (tagId == -1) { diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java index 84c26e14c..4868d1a5a 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Intake.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Intake.java @@ -10,7 +10,6 @@ import com.team766.hal.MotorController; import com.team766.hal.RobotProvider; import com.team766.library.ValueProvider; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Intake extends Mechanism { @@ -99,7 +98,7 @@ public void nudgeDown() { public void run() { // SmartDashboard.putString("[INTAKE]", state.toString()); - SmartDashboard.putNumber("[INTAKE POWER]", intakePower); + // SmartDashboard.putNumber("[INTAKE POWER]", intakePower); // SmartDashboard.putNumber("[INTAKE] Current", MotorUtil.getCurrentUsage(intakeMotor)); // SmartDashboard.putNumber("Prox Sensor", sensor.getRange()); } diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java index aa3dddb55..9b91e3f9c 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shooter.java @@ -9,7 +9,6 @@ import com.team766.hal.MotorController.ControlMode; import com.team766.hal.RobotProvider; import com.team766.library.RateLimiter; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Shooter extends Mechanism { private static final double DEFAULT_SPEED = @@ -85,8 +84,8 @@ public void nudgeDown() { public void run() { if (speedUpdated || rateLimiter.next()) { // SmartDashboard.putNumber("[SHOOTER TARGET SPEED]", shouldRun ? targetSpeed : 0.0); - SmartDashboard.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); - SmartDashboard.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); + // SmartDashboard.putNumber("[SHOOTER TOP MOTOR SPEED]", getShooterSpeedTop()); + // SmartDashboard.putNumber("[SHOOTER BOTTOM MOTOR SPEED]", getShooterSpeedBottom()); // SmartDashboard.putNumber( // "[SHOOTER] Top Motor Current", MotorUtil.getCurrentUsage(shooterMotorTop)); // SmartDashboard.putNumber( diff --git a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 13c0fbbc6..b936558ae 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -13,7 +13,6 @@ import com.team766.hal.RobotProvider; import com.team766.hal.wpilib.REVThroughBoreDutyCycleEncoder; import com.team766.library.ValueProvider; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Shoulder extends Mechanism { public enum ShoulderPosition { @@ -153,7 +152,7 @@ public void run() { leftMotor.setSensorPosition(convertedPos); encoderInitializationCount++; } - SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); + // SmartDashboard.putNumber("[SHOULDER] Angle", getAngle()); // SmartDashboard.putNumber("[SHOULDER] Target Angle", targetAngle); // SmartDashboard.putNumber("[SHOULDER] Rotations", getRotations()); // SmartDashboard.putNumber("[SHOULDER] Target Rotations", targetRotations); @@ -170,7 +169,7 @@ public void run() { // SmartDashboard.putNumber( // "[SHOULDER] Right Motor Stator Current", // MotorUtil.getStatorCurrentUsage(rightMotor)); - SmartDashboard.putBoolean("Shoulder at angle", isFinished()); + // SmartDashboard.putBoolean("Shoulder at angle", isFinished()); TalonFX leftTalon = (TalonFX) leftMotor; // SmartDashboard.putNumber("[SHOULDER] ffGain", ffGain.get());