diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index ce18629b..350967fd 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,7 +4,7 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, + "defaultMaxVel": 4.12, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, diff --git a/src/main/deploy/pathplanner/autos/A1456.auto b/src/main/deploy/pathplanner/autos/A1456.auto new file mode 100644 index 00000000..06aa2f17 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A1456.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6582127857550991, + "y": 6.628333484912334 + }, + "rotation": 59.91997316061384 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A1456" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/A154.auto b/src/main/deploy/pathplanner/autos/A154.auto new file mode 100644 index 00000000..ac1fc59e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/A154.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6582127857550991, + "y": 6.628333484912334 + }, + "rotation": 57.215719134130836 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "A154" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A1456.path b/src/main/deploy/pathplanner/paths/A1456.path new file mode 100644 index 00000000..6d4ba9b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A1456.path @@ -0,0 +1,325 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6582127857550991, + "y": 6.628333484912334 + }, + "prevControl": null, + "nextControl": { + "x": 1.6100701432172113, + "y": 6.847658310701237 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.44786151110764, + "y": 7.341005451241672 + }, + "prevControl": { + "x": 8.388659854242785, + "y": 7.518610421836227 + }, + "nextControl": { + "x": 8.487329282350876, + "y": 7.222602137511967 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8035208378109076, + "y": 6.5220491979445505 + }, + "prevControl": { + "x": 5.823254723432525, + "y": 7.0176484224404785 + }, + "nextControl": { + "x": 5.794188685334656, + "y": 6.287680375767852 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28012348332801, + "y": 5.742560715895044 + }, + "prevControl": { + "x": 8.023582970242865, + "y": 5.338016060647505 + }, + "nextControl": { + "x": 8.445543936244938, + "y": 6.003416045493453 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.8035208378109076, + "y": 6.5220491979445505 + }, + "prevControl": { + "x": 5.931791094347303, + "y": 6.662438481246984 + }, + "nextControl": { + "x": 5.649643309381696, + "y": 6.353633257134473 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.28012348332801, + "y": 4.124382094918037 + }, + "prevControl": { + "x": 8.319591254567126, + "y": 4.380922607999063 + }, + "nextControl": { + "x": 8.239586541181037, + "y": 3.860891970935212 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.274144702135559, + "y": 5.071608604755672 + }, + "prevControl": { + "x": 5.8035208378109004, + "y": 4.153982923350462 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2, + "rotationDegrees": 10.0, + "rotateFast": false + }, + { + "waypointRelativePos": 4.7, + "rotationDegrees": -42.506287523797496, + "rotateFast": false + }, + { + "waypointRelativePos": 4.199999999999999, + "rotationDegrees": 11.624631750657304, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Shoot A", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.2, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + }, + { + "name": "Shoot 1", + "minWaypointRelativePos": 0.5, + "maxWaypointRelativePos": 0.6000000000000001, + "constraints": { + "maxVelocity": 1.3, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Shoot A", + "waypointRelativePos": 0.1, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 1", + "waypointRelativePos": 0.2, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 1", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 4", + "waypointRelativePos": 0.7, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 4", + "waypointRelativePos": 1.75, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 5", + "waypointRelativePos": 2.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 5", + "waypointRelativePos": 3.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 6", + "waypointRelativePos": 4.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 6", + "waypointRelativePos": 5.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.12, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -9.619727799698838, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.124007308310574, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A154.path b/src/main/deploy/pathplanner/paths/A154.path new file mode 100644 index 00000000..1f8a33aa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A154.path @@ -0,0 +1,334 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.6582127857550991, + "y": 6.628333484912334 + }, + "prevControl": null, + "nextControl": { + "x": 1.6419230556007463, + "y": 6.885971888919526 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.619028587557887, + "y": 6.253586715447326 + }, + "prevControl": { + "x": 5.670450827349586, + "y": 7.553489572029072 + }, + "nextControl": { + "x": 7.8202605433042205, + "y": 4.607454035350498 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27025654051308, + "y": 6.253586715447326 + }, + "prevControl": { + "x": 8.527894944520272, + "y": 5.656334051612467 + }, + "nextControl": { + "x": 7.849142888394471, + "y": 7.229804727176832 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.191770383540769, + "y": 6.768863523461712 + }, + "prevControl": { + "x": 5.108218155926128, + "y": 7.911141267707331 + }, + "nextControl": { + "x": 5.221371211973195, + "y": 6.3641781129716115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.27025654051308, + "y": 6.936460795998516 + }, + "prevControl": { + "x": 7.509052165037282, + "y": 5.460895391230046 + }, + "nextControl": { + "x": 8.866258760172132, + "y": 8.091788175645291 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.191770383540769, + "y": 6.768863523461712 + }, + "prevControl": { + "x": 5.053633184189446, + "y": 7.794884820538872 + }, + "nextControl": { + "x": 5.329907582892091, + "y": 5.7428422263845516 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.540102772513238, + "y": 5.407084660323167 + }, + "prevControl": { + "x": 6.839368406783638, + "y": 6.533264890960021 + }, + "nextControl": { + "x": 7.8163771712158825, + "y": 4.963072233836776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.092651569918525, + "y": 4.676930892323324 + }, + "prevControl": { + "x": 7.993982141810438, + "y": 4.903870576971923 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3, + "rotationDegrees": 12.0, + "rotateFast": false + }, + { + "waypointRelativePos": 1.3, + "rotationDegrees": 31.50496131664116, + "rotateFast": false + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 69.59011716619598, + "rotateFast": false + }, + { + "waypointRelativePos": 2.6500000000000004, + "rotationDegrees": 20.0, + "rotateFast": false + } + ], + "constraintZones": [ + { + "name": "Shoot A", + "minWaypointRelativePos": 0.15, + "maxWaypointRelativePos": 0.25, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + }, + { + "name": "Shoot 1", + "minWaypointRelativePos": 0.4, + "maxWaypointRelativePos": 0.6, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + }, + { + "name": "Shoot 5", + "minWaypointRelativePos": 2.8000000000000003, + "maxWaypointRelativePos": 3.15, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + }, + { + "name": "Shoot 4", + "minWaypointRelativePos": 4.75, + "maxWaypointRelativePos": 5.1, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], + "eventMarkers": [ + { + "name": "Shoot A", + "waypointRelativePos": 0.15, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 1", + "waypointRelativePos": 0.25, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 1", + "waypointRelativePos": 0.5, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 5", + "waypointRelativePos": 1.3, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 5", + "waypointRelativePos": 2.8000000000000003, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + }, + { + "name": "Intake 4", + "waypointRelativePos": 3.95, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + }, + { + "name": "Shoot 4", + "waypointRelativePos": 4.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.12, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -58.212746878335274, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 60.124007308310574, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 0ebde615..00000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.0, - "y": 6.0 - }, - "prevControl": { - "x": 3.0, - "y": 6.0 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc4f7f39..0f7d2548 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,10 @@ import java.util.function.BooleanSupplier; import com.choreo.lib.ChoreoTrajectory; +import com.fasterxml.jackson.databind.util.Named; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.cscore.MjpegServer; import edu.wpi.first.cscore.UsbCamera; import edu.wpi.first.math.MathUtil; @@ -21,6 +25,7 @@ import edu.wpi.first.util.PixelFormat; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.event.EventLoop; import edu.wpi.first.wpilibj.XboxController; @@ -32,13 +37,16 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import frc.robot.Constants.SwerveConstants; import frc.robot.commands.auton.AutonBuilder; +import frc.robot.commands.auton.SwerveAimCommand; import frc.robot.commands.climb.ClimbLowerCommand; import frc.robot.commands.climb.ClimbRaiseCommand; import frc.robot.commands.elevator.ElevatorToAmpCommand; @@ -47,9 +55,12 @@ import frc.robot.commands.elevator.ElevatorToZeroCommand; import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand; import frc.robot.commands.intake.roller.IntakeRollerAmpIntakeCommand; +import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand; +import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand; import frc.robot.commands.shooter.flywheel.ShooterFlywheelShuttleCommand; +import frc.robot.commands.shooter.pivot.ShooterPivotAimCommand; import frc.robot.commands.swerve.AlignCommand; import frc.robot.commands.swerve.AutoIntakeSequence; import frc.robot.commands.swerve.NoteAlignCommand; @@ -143,6 +154,7 @@ public class RobotContainer { private boolean isNoteTrapReady = false; private boolean noteInBack = false; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -224,6 +236,22 @@ public RobotContainer() { swerveCrauton.add(autonPathChooser); + NamedCommands.registerCommand("Shoot", new ParallelDeadlineGroup( + new SequentialCommandGroup( + new WaitCommand(.05), + new ConditionalWaitCommand(swerveSubsystem::atTargetAngle), + new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(() -> !intakeRollerSubsystem.getRockwellSensorValue()), + new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).withTimeout(.3) + ), + new SwerveAimCommand(swerveSubsystem) + )); + NamedCommands.registerCommand( + "Intake", + new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem).andThen(new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem)) + ); + + + configureBindings(); } @@ -596,6 +624,10 @@ private void configureBindings() { * @return The selected autonomous command. */ public Command getAutonomousCommand() { - return autonBuilder.getMiddleFourPiece();//autonPathChooser.getSelected(); + return AutoBuilder.buildAuto("A1456").alongWith( + new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem), + new InstantCommand(() -> {shooterPivotSubsystem.setAutoAimBoolean(true);}), + new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) + ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/AutonBuilder.java b/src/main/java/frc/robot/commands/auton/AutonBuilder.java index ffdfbd51..39164ae8 100644 --- a/src/main/java/frc/robot/commands/auton/AutonBuilder.java +++ b/src/main/java/frc/robot/commands/auton/AutonBuilder.java @@ -132,7 +132,7 @@ public Command goAndIntake(ChoreoTrajectory intakeTrajectory) { */ public Command shoot() { return new ShooterPivotAimCommand(shooterPivotSubsystem) - .alongWith(new SetCalculatedAngleCommand(swerveSubsystem)).withTimeout(1) + .alongWith(new SwerveAimCommand(swerveSubsystem)).withTimeout(1) .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem).until( () -> !intakeRollerSubsystem.getRockwellSensorValue()) .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem)).withTimeout(.5) @@ -173,7 +173,7 @@ public SequentialCommandGroup buildAuton(Pose2d initPose, Command... commands) { autonSequence.addCommands( resetSwerve(GRTUtil.mirrorAcrossField(initPose, fmsSubsystem::isRedAlliance)), new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem).alongWith( - new SetCalculatedAngleCommand(swerveSubsystem), + new SwerveAimCommand(swerveSubsystem), new ShooterPivotAimCommand(shooterPivotSubsystem), new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) ).withTimeout(2) diff --git a/src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java b/src/main/java/frc/robot/commands/auton/SwerveAimCommand.java similarity index 50% rename from src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java rename to src/main/java/frc/robot/commands/auton/SwerveAimCommand.java index 7556622d..33265018 100644 --- a/src/main/java/frc/robot/commands/auton/SetCalculatedAngleCommand.java +++ b/src/main/java/frc/robot/commands/auton/SwerveAimCommand.java @@ -1,19 +1,15 @@ package frc.robot.commands.auton; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.SwerveConstants; import frc.robot.subsystems.swerve.SwerveSubsystem; /** SetCalculatedAngleCommand. */ -public class SetCalculatedAngleCommand extends Command { +public class SwerveAimCommand extends Command { private final SwerveSubsystem swerve; /** Sets the angle of the robot to aim at the speaker. */ - public SetCalculatedAngleCommand(SwerveSubsystem swerve) { + public SwerveAimCommand(SwerveSubsystem swerve) { this.swerve = swerve; - addRequirements(swerve); - } @Override @@ -22,19 +18,8 @@ public void initialize() { swerve.setAim(true); } - @Override - public void execute() { - swerve.setDrivePowers(0, 0, 0); - } - - @Override - public boolean isFinished() { - return Math.abs(swerve.getAngleError()) < Units.degreesToRadians(3); - } - @Override public void end(boolean interrupted) { - swerve.setRobotRelativeDrivePowers(0, 0, 0); swerve.setAim(false); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java index 2b25369d..6a2dccb9 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java @@ -9,6 +9,7 @@ public class IntakeRollerFeedCommand extends Command { private final IntakeRollerSubsystem intakeSubsystem; private final TrackingTimer timer; + private double speed = .6; /** * Sets all the rollers inwards to pass note into shooter. @@ -21,9 +22,22 @@ public IntakeRollerFeedCommand(IntakeRollerSubsystem intakeRollerSubsystem) { addRequirements(intakeRollerSubsystem); } + /** + * Sets all the rollers inwards to pass note into shooter. + * + * @param intakeRollerSubsystem The {@link IntakeRollerSubsystem} to set the powers to. + */ + public IntakeRollerFeedCommand(IntakeRollerSubsystem intakeRollerSubsystem, double speed) { + this.intakeSubsystem = intakeRollerSubsystem; + timer = new TrackingTimer(); + addRequirements(intakeRollerSubsystem); + + this.speed = speed; + } + @Override public void initialize() { - intakeSubsystem.setRollSpeeds(.6, .6); + intakeSubsystem.setRollSpeeds(speed, speed); timer.reset(); } diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java index 478908f4..999a8d11 100644 --- a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java +++ b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java @@ -9,6 +9,7 @@ public class IntakeRollerIntakeCommand extends Command { private final IntakeRollerSubsystem intakeSubsystem; private final LightBarSubsystem lightBarSubsystem; + private double speed = .7; /** * Runs the rollers on the intake until a note is detected. @@ -22,6 +23,13 @@ public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBar addRequirements(intakeSubsystem); } + public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed) { + this.intakeSubsystem = intakeSubsystem; + this.lightBarSubsystem = lightBarSubsystem; + addRequirements(intakeSubsystem); + this.speed = speed; + } + @Override public void initialize() { lightBarSubsystem.setLightBarStatus(LightBarStatus.INTAKING, 2); @@ -29,7 +37,7 @@ public void initialize() { @Override public void execute() { - intakeSubsystem.setRollSpeeds(.7, .7); + intakeSubsystem.setRollSpeeds(speed, speed); } @Override diff --git a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java index be0d60b1..eb6b6332 100644 --- a/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java +++ b/src/main/java/frc/robot/commands/shooter/flywheel/ShooterFlywheelReadyCommand.java @@ -52,9 +52,4 @@ public void execute() { lightBarSubsystem.updateShooterSpeedPercentage(avg); } - - @Override - public boolean isFinished() { - return shooterSubsystem.atSpeed(); - } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java index 136290e9..bc8d6afd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterFlywheelSubsystem.java @@ -6,6 +6,7 @@ package frc.robot.subsystems.shooter; import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.controls.ControlRequest; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; @@ -70,6 +71,7 @@ public ShooterFlywheelSubsystem(Pose2dSupplier poseSupplier, BooleanSupplier red shooterMotorTop.setNeutralMode(NeutralModeValue.Coast); shooterMotorBottom.setNeutralMode(NeutralModeValue.Coast); + request.EnableFOC = true; Slot0Configs configs = new Slot0Configs(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java index fccf39ac..64a671e9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java @@ -168,8 +168,8 @@ public double getPosition() { } /** Sets whether Auto-aim should be on or off. */ - public void setAutoAimBoolean(boolean autonAim) { - this.autoAim = autonAim; + public void setAutoAimBoolean(boolean autoAim) { + this.autoAim = autoAim; } @Override diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 188b431e..46508c33 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -425,6 +425,14 @@ public ChassisSpeeds getAimChassisSpeeds(ChassisSpeeds currentSpeeds){ return currentSpeeds; } + public double getAngleError(){ + return getRobotPosition().getRotation().getRadians() - getAngleToTarget(); + } + + public boolean atTargetAngle(){ + return Math.abs(getAngleError()) < Units.degreesToRadians(3); + } + public Translation2d getTargetPoint() { return targetPoint; } @@ -446,7 +454,7 @@ public double getAngleToTarget() { } /** Returns the PID error for the rotation controller. */ - public double getAngleError() { + public double getAnglePIDError() { return thetaController.getPositionError(); }