diff --git a/Robot2024/.pathplanner/settings.json b/Robot2024/.pathplanner/settings.json index 7b45380..9798c5c 100644 --- a/Robot2024/.pathplanner/settings.json +++ b/Robot2024/.pathplanner/settings.json @@ -11,7 +11,7 @@ "B3 Stuff" ], "autoFolders": [], - "defaultMaxVel": 1.0, + "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, diff --git a/Robot2024/src/main/deploy/pathplanner/autos/Command Test.auto b/Robot2024/src/main/deploy/pathplanner/autos/Command Test.auto new file mode 100644 index 0000000..44c193a --- /dev/null +++ b/Robot2024/src/main/deploy/pathplanner/autos/Command Test.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3464015540130732, + "y": 5.52155524239874 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "shoot-side" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/Robot2024/src/main/deploy/pathplanner/autos/Rotation Test.auto b/Robot2024/src/main/deploy/pathplanner/autos/Rotation Test.auto index c9585bf..a1d307f 100644 --- a/Robot2024/src/main/deploy/pathplanner/autos/Rotation Test.auto +++ b/Robot2024/src/main/deploy/pathplanner/autos/Rotation Test.auto @@ -1,6 +1,12 @@ { "version": 1.0, - "startingPose": null, + "startingPose": { + "position": { + "x": 2.0, + "y": 7.0 + }, + "rotation": 0 + }, "command": { "type": "sequential", "data": { diff --git a/Robot2024/src/main/deploy/pathplanner/autos/Translation Test.auto b/Robot2024/src/main/deploy/pathplanner/autos/Translation Test.auto index 1880786..819083a 100644 --- a/Robot2024/src/main/deploy/pathplanner/autos/Translation Test.auto +++ b/Robot2024/src/main/deploy/pathplanner/autos/Translation Test.auto @@ -1,6 +1,12 @@ { "version": 1.0, - "startingPose": null, + "startingPose": { + "position": { + "x": 1.34, + "y": 5.52155524239874 + }, + "rotation": 0 + }, "command": { "type": "sequential", "data": { diff --git a/Robot2024/src/main/deploy/pathplanner/paths/B1 Score Intake.path b/Robot2024/src/main/deploy/pathplanner/paths/B1 Score Intake.path index 55a4dfa..19c0490 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/B1 Score Intake.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/B1 Score Intake.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.3883881694216544, - "y": 7.177047995851627 + "x": 2.3104826280826942, + "y": 7.595790280548532 }, "isLocked": false, "linkedName": null @@ -20,12 +20,12 @@ "y": 6.7972584818242 }, "prevControl": { - "x": 2.6513193714406413, - "y": 7.001760527838968 + "x": 2.5149846740974624, + "y": 7.6055284732159025 }, "nextControl": { - "x": 3.5277461266883083, - "y": 6.367106670590658 + "x": 3.2710988604838738, + "y": 6.146052252981667 }, "isLocked": false, "linkedName": null @@ -44,12 +44,30 @@ "linkedName": null } ], - "rotationTargets": [], - "constraintZones": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.45, + "rotationDegrees": -42.51088582857621, + "rotateFast": true + } + ], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.25, + "maxWaypointRelativePos": 1.4, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [ { - "name": "start intake", - "waypointRelativePos": 0.15, + "name": "stop intake", + "waypointRelativePos": 1.35, "command": { "type": "parallel", "data": { @@ -57,7 +75,7 @@ { "type": "named", "data": { - "name": "start-intake" + "name": "stop-intake" } } ] @@ -65,8 +83,8 @@ } }, { - "name": "New Event Marker", - "waypointRelativePos": 1.5, + "name": "start intake", + "waypointRelativePos": 0.15, "command": { "type": "parallel", "data": { @@ -74,7 +92,7 @@ { "type": "named", "data": { - "name": "stop-intake" + "name": "start-intake" } } ] @@ -83,7 +101,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/B1-Leave.path b/Robot2024/src/main/deploy/pathplanner/paths/B1-Leave.path index e65ef6c..d6ddb30 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/B1-Leave.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/B1-Leave.path @@ -67,7 +67,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/B2 Score Intake.path b/Robot2024/src/main/deploy/pathplanner/paths/B2 Score Intake.path index b277533..8eaba88 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/B2 Score Intake.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/B2 Score Intake.path @@ -53,7 +53,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/B3-Leave.path b/Robot2024/src/main/deploy/pathplanner/paths/B3-Leave.path index 431c411..54b0e51 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/B3-Leave.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/B3-Leave.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/Rotation Test.path b/Robot2024/src/main/deploy/pathplanner/paths/Rotation Test.path index f135e60..c19205c 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/Rotation Test.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/Rotation Test.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -40,7 +40,7 @@ "goalEndState": { "velocity": 0.0, "rotation": 180.0, - "rotateFast": false + "rotateFast": true }, "reversed": false, "folder": null, diff --git a/Robot2024/src/main/deploy/pathplanner/paths/Translation Test.path b/Robot2024/src/main/deploy/pathplanner/paths/Translation Test.path index f2584f2..5152c01 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/Translation Test.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/Translation Test.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/finalstretch B2 Score Intake.path b/Robot2024/src/main/deploy/pathplanner/paths/finalstretch B2 Score Intake.path index 1599367..82ae36a 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/finalstretch B2 Score Intake.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/finalstretch B2 Score Intake.path @@ -43,7 +43,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/outforfront B2 Score Intake.path b/Robot2024/src/main/deploy/pathplanner/paths/outforfront B2 Score Intake.path index 67042d4..bb32f66 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/outforfront B2 Score Intake.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/outforfront B2 Score Intake.path @@ -53,7 +53,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/deploy/pathplanner/paths/runback B2 Score Intake.path b/Robot2024/src/main/deploy/pathplanner/paths/runback B2 Score Intake.path index 3a7b23e..696bfaa 100644 --- a/Robot2024/src/main/deploy/pathplanner/paths/runback B2 Score Intake.path +++ b/Robot2024/src/main/deploy/pathplanner/paths/runback B2 Score Intake.path @@ -53,7 +53,7 @@ } ], "globalConstraints": { - "maxVelocity": 1.0, + "maxVelocity": 3.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 diff --git a/Robot2024/src/main/java/com/team2813/AutoCommands.java b/Robot2024/src/main/java/com/team2813/AutoCommands.java index e375955..752ea6e 100644 --- a/Robot2024/src/main/java/com/team2813/AutoCommands.java +++ b/Robot2024/src/main/java/com/team2813/AutoCommands.java @@ -77,7 +77,7 @@ public Command stopIntake() { private Command createShootFront() { // TODO: get correct angle and speed - return new ShootFromPosCommand(magazine, shooter, shooterPivot, Position.SUBWOOFER_FRONT, 26.5); + return new ShootFromPosCommand(magazine, shooter, shooterPivot, Position.SUBWOOFER_FRONT, 75); } public Command shootFront() { @@ -95,7 +95,7 @@ public Command shootFront() { private Command createShootSide() { // TODO: get correct angle and speed - return new ShootFromPosCommand(magazine, shooter, shooterPivot, Position.SUBWOOFER_SIDE, 25); + return new ShootFromPosCommand(magazine, shooter, shooterPivot, Position.SUBWOOFER_SIDE, 80); } public Command shootSide() { @@ -131,7 +131,7 @@ public Command shootAuto() { private Command createShootAmp() { return new SequentialCommandGroup( - new InstantCommand(() -> shooter.run(10), shooter), + new InstantCommand(() -> shooter.run(22.5), shooter), new WaitCommand(0.2), new InstantCommand(magazine::runMagKicker, magazine), new WaitCommand(1), diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index cd780f5..db9c738 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -35,8 +35,8 @@ public static class OperatorConstants { // public static final Trigger spoolAutoAimButton = operatorController.options(); public static final Trigger spoolPodiumButton = operatorController.cross(); // actually maps to circle public static final Trigger climbButton = operatorController.share(); - public static final Trigger climbManualUpButton = operatorController.povUp(); - public static final Trigger climbDownUpButton = operatorController.povDown(); + public static final Trigger climbUpButton = operatorController.povUp(); + public static final Trigger climbDownButton = operatorController.povDown(); public static final Trigger altOuttakeButton = new Trigger(() -> operatorXboxController.getLeftTriggerAxis() >= 0.5).and(() -> false); } diff --git a/Robot2024/src/main/java/com/team2813/RobotContainer.java b/Robot2024/src/main/java/com/team2813/RobotContainer.java index 915ffe1..ab93344 100644 --- a/Robot2024/src/main/java/com/team2813/RobotContainer.java +++ b/Robot2024/src/main/java/com/team2813/RobotContainer.java @@ -12,6 +12,8 @@ import static com.team2813.Constants.OperatorConstants.ampInButton; import static com.team2813.Constants.OperatorConstants.ampIntakeButton; import static com.team2813.Constants.OperatorConstants.ampOutButton; +import static com.team2813.Constants.OperatorConstants.climbDownButton; +import static com.team2813.Constants.OperatorConstants.climbUpButton; import static com.team2813.Constants.OperatorConstants.intakeButton; import static com.team2813.Constants.OperatorConstants.operatorControllerPort; import static com.team2813.Constants.OperatorConstants.outtakeButton; @@ -23,15 +25,15 @@ import com.team2813.commands.AutoAimCommand; import com.team2813.commands.DefaultDriveCommand; import com.team2813.commands.DefaultShooterCommand; -import com.team2813.commands.LockFunctionCommand; import com.team2813.commands.SaveSwerveOffsetsCommand; import com.team2813.commands.SpoolCommand; import com.team2813.subsystems.Amp; +import com.team2813.subsystems.Climber; import com.team2813.subsystems.Drive; import com.team2813.subsystems.Intake; import com.team2813.subsystems.IntakePivot; import com.team2813.subsystems.IntakePivot.Rotations; -import com.team2813.subsystems.ShooterPivot.Position; +import com.team2813.subsystems.LEDs; import com.team2813.subsystems.Magazine; import com.team2813.subsystems.Shooter; import com.team2813.subsystems.ShooterPivot; @@ -56,6 +58,8 @@ public class RobotContainer { private final Magazine mag = new Magazine(); private final IntakePivot intakePivot = new IntakePivot(); private final ShooterPivot shooterPivot = new ShooterPivot(); + private final LEDs leds = new LEDs(mag); + private final Climber climber = new Climber(); private final XboxController driverController = new XboxController(driverControllerPort); private final XboxController operatorController = new XboxController(operatorControllerPort); @@ -89,78 +93,79 @@ private void configureBindings(AutoCommands autoCommands) { slowmodeButton.whileTrue(new InstantCommand(() -> drive.enableSlowMode(true), drive)); slowmodeButton.onFalse(new InstantCommand(() -> drive.enableSlowMode(false), drive)); - //intake & outtake buttons + // intake & outtake buttons intakeButton.whileTrue(autoCommands.startIntake()); intakeButton.onFalse(autoCommands.stopIntake()); outtakeButton.whileTrue(new ParallelCommandGroup( - new InstantCommand(intake::outtakeNote, intake), - new InstantCommand(mag::reverseMag, mag) - )); + new InstantCommand(intake::outtakeNote, intake), + new InstantCommand(mag::reverseMag, mag))); altOuttakeButton.whileTrue(new SequentialCommandGroup( - new InstantCommand(() -> intakePivot.setSetpoint(Rotations.INTAKE_DOWN), intakePivot), - new WaitCommand(0.2), - new ParallelCommandGroup( - new InstantCommand(intake::outtakeNote, intake), - new InstantCommand(mag::reverseMag, mag) - ) - )); + new InstantCommand(() -> intakePivot.setSetpoint(Rotations.INTAKE_DOWN), intakePivot), + new WaitCommand(0.2), + new ParallelCommandGroup( + new InstantCommand(intake::outtakeNote, intake), + new InstantCommand(mag::reverseMag, mag)))); altOuttakeButton.onFalse(autoCommands.stopIntake()); - + outtakeButton.onFalse(autoCommands.stopIntake()); - - ampIntakeButton.onTrue(autoCommands.shootAmp()); - /*ampIntakeButton.onFalse( - new InstantCommand(amp::stop, amp) - //new InstantCommand(amp::ampStop, amp) - ); - ampOuttakeButton.onTrue( - new InstantCommand(amp::pushNoteOut, amp) - //new InstantCommand(amp::ampOuttake, amp) - ); - ampOuttakeButton.onFalse( - new InstantCommand(amp::stop, amp) - //new InstantCommand(amp::ampStop, amp) - );*/ + ampIntakeButton.onTrue(autoCommands.shootAmp()); + /* + * ampIntakeButton.onFalse( + * new InstantCommand(amp::stop, amp) + * //new InstantCommand(amp::ampStop, amp) + * ); + * ampOuttakeButton.onTrue( + * new InstantCommand(amp::pushNoteOut, amp) + * //new InstantCommand(amp::ampOuttake, amp) + * ); + * ampOuttakeButton.onFalse( + * new InstantCommand(amp::stop, amp) + * //new InstantCommand(amp::ampStop, amp) + * ); + */ + + climbUpButton.onTrue( + new InstantCommand(climber::extend, climber)); + + climbUpButton.onFalse( + new InstantCommand(climber::stop, climber)); + + climbDownButton.onTrue( + new InstantCommand(climber::retract, climber)); + + climbDownButton.onFalse( + new InstantCommand(climber::stop, climber)); ampInButton.onTrue( - new InstantCommand(amp::ampIntake, amp) - ); + new InstantCommand(amp::ampIntake, amp)); ampInButton.onFalse( - new InstantCommand(amp::ampStop, amp) - ); + new InstantCommand(amp::ampStop, amp)); ampOutButton.onTrue( - new InstantCommand(amp::ampOuttake, amp) - ); + new InstantCommand(amp::ampOuttake, amp)); ampOutButton.onFalse( - new InstantCommand(amp::ampStop, amp) - ); + new InstantCommand(amp::ampStop, amp)); orientButton.onTrue( - new InstantCommand(drive::orientForward, drive) - ); - - + new InstantCommand(drive::orientForward, drive)); shootButton.onTrue(new SequentialCommandGroup( - new InstantCommand(mag::runMagKicker, mag), - new WaitCommand(1), - new ParallelCommandGroup( - new InstantCommand(shooter::stop, shooter), - new InstantCommand(mag::stop, mag) - ) - )); + new InstantCommand(mag::runMagKicker, mag), + new WaitCommand(1), + new ParallelCommandGroup( + new InstantCommand(shooter::stop, shooter), + new InstantCommand(mag::stop, mag)))); spoolAutoAimButton.onTrue( - new AutoAimCommand(shooter, shooterPivot, mag, drive) - // new LockFunctionCommand(shooterPivot::atPosition, () -> shooterPivot.setSetpoint(Position.TEST), shooterPivot, drive) + new AutoAimCommand(shooter, shooterPivot, mag, drive) + // new LockFunctionCommand(shooterPivot::atPosition, () -> + // shooterPivot.setSetpoint(Position.TEST), shooterPivot, drive) ); spoolPodiumButton.onTrue( - new SpoolCommand(shooter) - ); + new SpoolCommand(shooter)); } public Command getAutonomousCommand() { diff --git a/Robot2024/src/main/java/com/team2813/commands/LockFunctionCommand.java b/Robot2024/src/main/java/com/team2813/commands/LockFunctionCommand.java index b9c7c81..bcc1259 100644 --- a/Robot2024/src/main/java/com/team2813/commands/LockFunctionCommand.java +++ b/Robot2024/src/main/java/com/team2813/commands/LockFunctionCommand.java @@ -1,10 +1,10 @@ package com.team2813.commands; +import java.util.function.BooleanSupplier; + import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; -import java.util.function.BooleanSupplier; - public class LockFunctionCommand extends WaitUntilCommand { private final Runnable function; @@ -20,7 +20,7 @@ public LockFunctionCommand(BooleanSupplier condition, Runnable function, Subsyst addRequirements(requirements); } - @Override + @Override public void initialize() { function.run(); } diff --git a/Robot2024/src/main/java/com/team2813/commands/ShootFromPosCommand.java b/Robot2024/src/main/java/com/team2813/commands/ShootFromPosCommand.java index b23fd94..dc81738 100644 --- a/Robot2024/src/main/java/com/team2813/commands/ShootFromPosCommand.java +++ b/Robot2024/src/main/java/com/team2813/commands/ShootFromPosCommand.java @@ -10,17 +10,18 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; public class ShootFromPosCommand extends SequentialCommandGroup { - public ShootFromPosCommand(Magazine magazine, Shooter shooter, ShooterPivot pivot, ShooterPivot.Position pos, double shooterSpeed) { + public ShootFromPosCommand(Magazine magazine, Shooter shooter, ShooterPivot pivot, ShooterPivot.Position pos, + double shooterSpeed) { super( - // new LockFunctionCommand(shooter::atSetpoint, () -> shooter.setSetpoint(pos), shooter), - new InstantCommand(() -> shooter.run(shooterSpeed), shooter), - new WaitCommand(0.5), - new InstantCommand(magazine::runMagKicker, magazine), - new WaitCommand(0.5), - new ParallelCommandGroup( - new InstantCommand(magazine::stop, magazine), - new InstantCommand(shooter::stop, shooter) - ) + new InstantCommand(() -> pivot.setSetpoint(pos), pivot), + new InstantCommand(() -> shooter.run(shooterSpeed), shooter), + new WaitCommand(0.5), + new InstantCommand(magazine::runMagKicker, magazine), + new WaitCommand(0.5), + new ParallelCommandGroup( + new InstantCommand(magazine::stop, magazine), + new InstantCommand(shooter::stop, shooter) + ) ); } } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Climber.java b/Robot2024/src/main/java/com/team2813/subsystems/Climber.java index 79b158b..b15d0c8 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Climber.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Climber.java @@ -1,34 +1,52 @@ package com.team2813.subsystems; import static com.team2813.Constants.CLIMBER; +import static java.util.stream.Collectors.toCollection; +import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs; +import com.ctre.phoenix6.configs.TalonFXConfigurator; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.InvertType; import com.team2813.lib2813.control.PIDMotor; import com.team2813.lib2813.control.motors.TalonFXWrapper; +import com.team2813.lib2813.util.ConfigUtils; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { - private final PIDMotor climberMotor; - - public Climber() { - climberMotor = new TalonFXWrapper(CLIMBER, InvertType.CLOCKWISE); + private final PIDMotor climberMotor; + + public Climber() { + TalonFXWrapper climberMotor = new TalonFXWrapper(CLIMBER, InvertType.CLOCKWISE); + climberMotor.motor().setNeutralMode(NeutralModeValue.Brake); + TalonFXConfigurator cnf = climberMotor.motor().getConfigurator(); + ConfigUtils.phoenix6Config( + () -> cnf.apply( + new SoftwareLimitSwitchConfigs() + .withForwardSoftLimitEnable(true) + .withForwardSoftLimitThreshold(52.632324) + ) + ); + this.climberMotor = climberMotor; } - public void extend() { - climberMotor.set(ControlMode.DUTY_CYCLE, 0.7); //TODO: Find out proper demand value for extending - } + public void extend() { + climberMotor.set(ControlMode.DUTY_CYCLE, 0.7); // TODO: Find out proper demand value for extending + } - public void retract() { - climberMotor.set(ControlMode.DUTY_CYCLE, -0.3); //TODO: Find out proper demand value for retracting - } + public void retract() { + climberMotor.set(ControlMode.DUTY_CYCLE, -0.3); // TODO: Find out proper demand value for retracting + } - // @Override - public void outputTelemetry() { - SmartDashboard.putNumber("Climber Encoder", climberMotor.position()); - SmartDashboard.putNumber("Climber Velocity", climberMotor.getVelocity()); - } -} + public void stop() { + climberMotor.set(ControlMode.DUTY_CYCLE, 0); + } + // @Override + public void outputTelemetry() { + SmartDashboard.putNumber("Climber Encoder", climberMotor.position()); + SmartDashboard.putNumber("Climber Velocity", climberMotor.getVelocity()); + } +} diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java index 45f92c1..f206182 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java @@ -144,8 +144,8 @@ public Drive() { drivetrain::getChassisSpeeds, this::drive, new HolonomicPathFollowerConfig( - new PIDConstants(0.4, 0, 0), // Translation PID - new PIDConstants(0, 0, 0), // Rotation PID + new PIDConstants(0.9, 0, 0), // Translation PID + new PIDConstants(0.2, 0, 0), // Rotation PID MAX_VELOCITY, 0.410178, new ReplanningConfig() @@ -232,6 +232,7 @@ public void drive(ChassisSpeeds demand) { } public void resetOdometry(Pose2d currentPose) { + useLimelightOffset = false; drivetrain.seedFieldRelative(currentPose); } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/LEDs.java b/Robot2024/src/main/java/com/team2813/subsystems/LEDs.java index f232a9c..98f9e98 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/LEDs.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/LEDs.java @@ -1,6 +1,11 @@ package com.team2813.subsystems; -import java.util.function.BooleanSupplier; +import static com.team2813.Constants.CANIFIER; +import static com.team2813.Constants.OperatorConstants.intakeButton; + +import java.util.function.Function; +import java.util.HashSet; +import java.util.Set; import com.ctre.phoenix.CANifier; import com.team2813.lib2813.subsystems.lightshow.QueueLightshow; @@ -8,38 +13,58 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; -import static com.team2813.Constants.*; public class LEDs extends QueueLightshow { - final CANifier canifier; - public LEDs() { - super(States.class); + private final CANifier canifier; + private final Magazine magazine; + public LEDs(Magazine magazine) { + super(new HashSet<>()); canifier = new CANifier(CANIFIER); - setDefaultState(States.OFF); + this.magazine = magazine; + Set states = new HashSet<>(); + for (NewStates s : NewStates.values()) { + states.add(s.createState(this)); + } + addStates(states); } protected void useColor(Color c) { - canifier.setLEDOutput(c.red, CANifier.LEDChannel.LEDChannelA); - canifier.setLEDOutput(c.green, CANifier.LEDChannel.LEDChannelB); + canifier.setLEDOutput(c.red, CANifier.LEDChannel.LEDChannelB); + canifier.setLEDOutput(c.green, CANifier.LEDChannel.LEDChannelA); canifier.setLEDOutput(c.blue, CANifier.LEDChannel.LEDChannelC); } - public enum States implements State { - OFF(new Color(0, 0, 0), () -> true), - BLUE(new Color(0, 0, 255), DriverStation::isEnabled); + private enum NewStates { + Disabled(new Color(255, 0, 0), (j) -> true), + Green( + new Color(0, 255, 0), + (j) -> DriverStation.isEnabled() + ), + Yellow( + new Color(255, 255, 0), + (j) -> j.magazine.magMotor.getVelocity() < 0.01 && intakeButton.getAsBoolean() + ), + Blue(new Color(0, 0, 255), (j) -> false), + Orange(new Color(255, 165, 0), (j) -> false), + White(new Color(255, 255, 255), (j) -> false); private final Color c; - private final BooleanSupplier sup; - States(Color c, BooleanSupplier sup) { + private final Function sup; + NewStates(Color c, Function sup) { this.c = c; this.sup = sup; } - @Override - public Color color() { - return c; - } - @Override - public boolean apply() { - return sup.getAsBoolean(); + private State createState(LEDs leds) { + return new State() { + @Override + public Color color() { + return c; + } + + @Override + public boolean apply() { + return sup.apply(leds); + } + }; } } } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java b/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java index 32c052a..a36b860 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java @@ -4,13 +4,14 @@ import com.team2813.lib2813.control.ControlMode; import com.team2813.lib2813.control.InvertType; import com.team2813.lib2813.control.Motor; +import com.team2813.lib2813.control.PIDMotor; import edu.wpi.first.wpilibj2.command.SubsystemBase; import static com.team2813.Constants.*; public class Magazine extends SubsystemBase { Motor kickerMotor; - Motor magMotor; + PIDMotor magMotor; public Magazine() { kickerMotor = new TalonFXWrapper(KICKER, InvertType.COUNTER_CLOCKWISE); diff --git a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java index 7dc66d2..3ef941a 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java @@ -20,7 +20,7 @@ public class ShooterPivot extends MotorSubsystem { public ShooterPivot() { super(new MotorSubsystemConfiguration( - pivotMotor()).acceptableError(0.003) + pivotMotor()).acceptableError(0.004) .PID(3.8, 0, 0)); SmartDashboard.putData("Shooter Pivot PID", m_controller); } @@ -54,8 +54,8 @@ protected void useOutput(double output, double setpoint) { public static enum Position implements MotorSubsystem.Position { TOP_HARD_STOP(0), - SUBWOOFER_FRONT(0), - SUBWOOFER_SIDE(0), + SUBWOOFER_FRONT(0.007080), + SUBWOOFER_SIDE(0.007080), PODIUM(0), TEST(0.067871), BOTTOM_HARD_STOP(0.111816);