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..4de4ddbc6 --- /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.809031599544681, + "y": 6.921961871701522 + }, + "prevControl": { + "x": 2.6349878644201734, + "y": 6.817492873874638 + }, + "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..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.8139011185429776, - "y": 7.04708475620655 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": { - "x": 1.778950178270727, - "y": 7.121671535009737 + "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 104216349..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.8139011185429776, - "y": 7.04708475620655 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": null, "nextControl": { - "x": 2.7965014957807695, - "y": 6.4770541115156295 + "x": 2.791631976782473, + "y": 6.351931227010602 }, "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..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,40 +3,40 @@ "waypoints": [ { "anchor": { - "x": 2.8139011185429776, - "y": 7.04708475620655 + "x": 2.809031599544681, + "y": 6.921961871701522 }, "prevControl": null, "nextControl": { - "x": 2.196897430831899, - "y": 7.214845624135694 + "x": 2.1920279118336023, + "y": 7.089722739630666 }, "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..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; @@ -108,7 +109,7 @@ public final void run(Context context) { drive.setCurrentPosition( shouldFlipAuton ? GeometryUtil.flipFieldPose(initialPosition) : initialPosition); // } - context.takeOwnership(drive); + // context.takeOwnership(drive); drive.resetGyro( (shouldFlipAuton ? GeometryUtil.flipFieldRotation(initialPosition.getRotation()) @@ -118,5 +119,16 @@ public final void run(Context context) { pathItem.run(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 + 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 d513e81d1..0423d2287 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 { @@ -59,8 +60,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)) @@ -73,6 +74,10 @@ public void handleOI(Context context) { rightJoystickY = -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(); @@ -103,13 +108,27 @@ public 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 1a3ba9314..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, 30.109); - private static AnywhereScoringPosition a6 = new AnywhereScoringPosition(2.4616, 5600, 30.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/ForwardApriltagCamera.java b/src/main/java/com/team766/robot/reva/mechanisms/ForwardApriltagCamera.java index 89fc356ff..7caa39d3f 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; @@ -37,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) { @@ -59,9 +45,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/mechanisms/Shoulder.java b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java index 2d60341d0..b936558ae 100644 --- a/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java +++ b/src/main/java/com/team766/robot/reva/mechanisms/Shoulder.java @@ -136,7 +136,7 @@ public void rotate(double angle) { } public boolean isFinished() { - return Math.abs(getAngle() - targetAngle) < 1; + return Math.abs(getAngle() - targetAngle) < 2.5; } @Override 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..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,7 +9,8 @@ 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.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/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/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/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/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); } } 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/ShootAtSubwoofer.java b/src/main/java/com/team766/robot/reva/procedures/ShootAtSubwoofer.java new file mode 100644 index 000000000..a7631d3c6 --- /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.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); + } +} 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..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(); @@ -106,7 +109,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(); @@ -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/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/ShootVelocityAndIntake.java b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java index 3104003af..03c07a091 100644 --- a/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java +++ b/src/main/java/com/team766/robot/reva/procedures/ShootVelocityAndIntake.java @@ -22,12 +22,12 @@ 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); new IntakeStop().run(context); - Robot.shooter.stop(); Robot.lights.signalFinishedShootingProcedure(); + + // Shooter stopped at the end of auton } } 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..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,12 +8,13 @@ public class ThreePieceAutonAmpSide extends PathSequenceAuto { public ThreePieceAutonAmpSide() { super(Robot.drive, new Pose2d(0.75, 6.68, Rotation2d.fromDegrees(60))); - addProcedure(new RotateAndShootNow()); + // addPath("3 Piece 1"); + addProcedure(new ShootAtSubwoofer()); 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()); } }