From 746c1d380a8dfec3dfc057a08cb7b19bd9cb9d33 Mon Sep 17 00:00:00 2001 From: ArnoUUU Date: Tue, 19 Mar 2024 23:10:54 -0700 Subject: [PATCH] Merge branch 'main' into LED Should be ready for testing --- .../autos/0-Anywhere-Leave-StraightBack.auto | 25 ++++++++ ...s.auto => 1-AmpSide-Leave-Delayed-3s.auto} | 0 ...s.auto => 1-AmpSide-Leave-Delayed-8s.auto} | 0 ...ght-Leave-4s.auto => 1-AmpSide-Leave.auto} | 0 ...nter-Stays-8s.auto => 2-Center-Stays.auto} | 0 ...ght-Leave.auto => 2-SourceSide-Leave.auto} | 0 .../pathplanner/autos/3-Center-Stays.auto | 32 ++++------- .../pathplanner/autos/4-Center-Stays.auto | 57 +++++++++---------- .../pathplanner/paths/LeftNote-Center.path | 6 +- .../pathplanner/paths/Straight-Back.path | 52 +++++++++++++++++ src/main/java/frc/robot/Constants.java | 11 ++-- src/main/java/frc/robot/RobotContainer.java | 57 +++++++++++-------- .../frc/robot/subsystems/LEDSubsystem.java | 1 - .../robot/subsystems/ShooterSubsystem.java | 12 ++-- 14 files changed, 164 insertions(+), 89 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto rename src/main/deploy/pathplanner/autos/{1-Left-Leave-Delayed-6s.auto => 1-AmpSide-Leave-Delayed-3s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-Left-Leave-Delayed-12s.auto => 1-AmpSide-Leave-Delayed-8s.auto} (100%) rename src/main/deploy/pathplanner/autos/{1-Right-Leave-4s.auto => 1-AmpSide-Leave.auto} (100%) rename src/main/deploy/pathplanner/autos/{2-Center-Stays-8s.auto => 2-Center-Stays.auto} (100%) rename src/main/deploy/pathplanner/autos/{2-Right-Leave.auto => 2-SourceSide-Leave.auto} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Straight-Back.path diff --git a/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto new file mode 100644 index 0000000..398d77b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/0-Anywhere-Leave-StraightBack.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.55744644822623, + "y": 6.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Straight-Back" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-6s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-3s.auto diff --git a/src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Left-Leave-Delayed-12s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave-Delayed-8s.auto diff --git a/src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto b/src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/1-Right-Leave-4s.auto rename to src/main/deploy/pathplanner/autos/1-AmpSide-Leave.auto diff --git a/src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto b/src/main/deploy/pathplanner/autos/2-Center-Stays.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-Center-Stays-8s.auto rename to src/main/deploy/pathplanner/autos/2-Center-Stays.auto diff --git a/src/main/deploy/pathplanner/autos/2-Right-Leave.auto b/src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/2-Right-Leave.auto rename to src/main/deploy/pathplanner/autos/2-SourceSide-Leave.auto diff --git a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto index fe3a087..3c0c573 100644 --- a/src/main/deploy/pathplanner/autos/3-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/3-Center-Stays.auto @@ -22,16 +22,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterNote-Center" - } - } - ] + "pathName": "CenterNote-Center" } }, { @@ -39,6 +32,12 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "Spin up Shooter" + } } ] } @@ -52,7 +51,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -60,16 +59,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LeftNote-Center" - } - } - ] + "pathName": "LeftNote-Center" } }, { @@ -90,7 +82,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } } ] diff --git a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto index 8ac0f0a..56651e6 100644 --- a/src/main/deploy/pathplanner/autos/4-Center-Stays.auto +++ b/src/main/deploy/pathplanner/autos/4-Center-Stays.auto @@ -17,21 +17,26 @@ "name": "Shoot" } }, + { + "type": "named", + "data": { + "name": "Intake out" + } + }, + { + "type": "deadline", + "data": { + "commands": [] + } + }, { "type": "deadline", "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "CenterNote-Center" - } - } - ] + "pathName": "CenterNote-Center" } }, { @@ -39,6 +44,12 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "Spin up Shooter" + } } ] } @@ -52,7 +63,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -60,16 +71,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "LeftNote-Center" - } - } - ] + "pathName": "LeftNote-Center" } }, { @@ -90,7 +94,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } }, { @@ -98,16 +102,9 @@ "data": { "commands": [ { - "type": "sequential", + "type": "path", "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "RightNote-Center" - } - } - ] + "pathName": "RightNote-Center" } }, { @@ -128,7 +125,7 @@ { "type": "named", "data": { - "name": "Shoot" + "name": "Outtake" } } ] diff --git a/src/main/deploy/pathplanner/paths/LeftNote-Center.path b/src/main/deploy/pathplanner/paths/LeftNote-Center.path index d87c8f2..51473cb 100644 --- a/src/main/deploy/pathplanner/paths/LeftNote-Center.path +++ b/src/main/deploy/pathplanner/paths/LeftNote-Center.path @@ -16,15 +16,15 @@ }, { "anchor": { - "x": 2.9026396514331507, + "x": 2.75, "y": 6.5 }, "prevControl": { - "x": 2.913008302826397, + "x": 2.760368651393246, "y": 5.421660255102424 }, "nextControl": { - "x": 2.8927727086223416, + "x": 2.740133057189191, "y": 7.526162052324105 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Straight-Back.path b/src/main/deploy/pathplanner/paths/Straight-Back.path new file mode 100644 index 0000000..2eddfae --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Straight-Back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.55744644822623, + "y": 6.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.557446448226229, + "y": 6.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/Constants.java b/src/main/java/frc/robot/Constants.java index f39dc18..b573764 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,8 +106,7 @@ public static final class IntakeConstants { public static final double kIntakeAmpScoringAngle = -93; // 193 - 100 (previous angle) /** Encoder offset in rotations */ - //public static final double kArmEncoderOffset = 0.3415; - public static final double kArmEncoderOffset = 0.504; + public static final double kArmEncoderOffset = 0.715; public static final double kIntakeSpeed = 0.5; @@ -118,13 +117,13 @@ public static final class IntakeConstants { public static final class ShooterConstants { public static final int kTopShooterMotorPort = 20; public static final int kBottomShooterMotorPort = 35; - public static final double kTopShooterSpeed = 0.8; - public static final double kBottomShooterSpeed = 0.9; - public static final double kPreShooterSpeed = 0.4; + public static final double kShooterSpeedTop = 0.75; + public static final double kShooterSpeedBottom = 1; + public static final double kPrepShooterSpeed = 0.6; public static final double kShooterOff = 0; public static final double kShooterOffTime = 0.04; - public static final double kShooterOnTime = 1.9; + public static final double kShooterOnTime = 1; } public static class ClimberConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 24e24d1..056da24 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; @@ -20,7 +19,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.RepeatCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -54,7 +52,7 @@ public class RobotContainer { private final DriveSubsystem m_robotDrive = new DriveSubsystem(); private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - // private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); private final LEDSubsystem m_ledSubsystem = new LEDSubsystem(); @@ -70,7 +68,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new ParallelDeadlineGroup(new WaitCommand(0.8), new NoteOuttakeCommand(m_intakeSubsystem)), + new ParallelDeadlineGroup(new WaitCommand(0.25), new NoteOuttakeCommand(m_intakeSubsystem)), new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime))); NamedCommands.registerCommand("Intake", @@ -79,12 +77,24 @@ public RobotContainer() { new NoteIntakeCommand(m_intakeSubsystem), new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))); - NamedCommands.registerCommand("Pre-Speed - 30%", - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Halfway, 0.01)); + NamedCommands.registerCommand("Prep-Speed - 60%", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Prep, 0.01)); + + NamedCommands.registerCommand("Spin up Shooter", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 0.01)); + + NamedCommands.registerCommand("Spin down Shooter", + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01)); + + NamedCommands.registerCommand("Outtake", + new ParallelDeadlineGroup(new WaitCommand(0.25), new NoteOuttakeCommand(m_intakeSubsystem))); NamedCommands.registerCommand("Intake in", new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + NamedCommands.registerCommand("Intake out", + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended)); + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, m_robotDrive::getChassisSpeeds, m_robotDrive::autonDrive, @@ -172,8 +182,9 @@ private void configureBindings() { // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), - new NoteOuttakeCommand(m_intakeSubsystem))) + .onTrue(new SequentialCommandGroup( + new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), + new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); new JoystickButton(m_driverController, Button.kRightBumper.value) @@ -193,7 +204,7 @@ private void configureBindings() { // Outtake, Operator Controller Right Trigger new Trigger(() -> { return m_operatorController.getRightTriggerAxis() > 0.5; - }).whileTrue(new NoteOuttakeCommand(m_intakeSubsystem)); + }).whileTrue(new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem))); new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; @@ -201,16 +212,16 @@ private void configureBindings() { .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button - // new Trigger(() -> { - // return m_operatorController.getAButton() && m_operatorController.getRightBumper(); - // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); + new Trigger(() -> { + return m_operatorController.getAButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.forward())); // // Climber Down, Operator Controller Right Bumper + B Button - // new Trigger(() -> { - // return m_operatorController.getBButton() && m_operatorController.getRightBumper(); - // }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); + new Trigger(() -> { + return m_operatorController.getBButton() && m_operatorController.getRightBumper(); + }).whileTrue(new InstantCommand(() -> m_climberSubsystem.reverse())); - // Toggle Distance Sensor, Operatoe Controller Left Bumper + Start Button + // Toggle Distance Sensor, Operator Controller Left Bumper + Start Button new Trigger(() -> { return m_operatorController.getLeftBumper() && m_operatorController.getStartButton(); }).onTrue(new InstantCommand(() -> m_intakeSubsystem.toggleDistanceSensor())); @@ -247,13 +258,13 @@ public Command getAutonomousCommand() { return autoChooser.getSelected(); // return new SequentialCommandGroup( - // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 3), - // new ParallelDeadlineGroup(new WaitCommand(1.5), new NoteOuttakeCommand(m_intakeSubsystem)), - // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01) - // , new ParallelDeadlineGroup(new WaitCommand(20), new RepeatCommand(new InstantCommand(() -> m_robotDrive.autonDrive(new ChassisSpeeds(3, 0, 0))))) - // ); - - + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, 3), + // new ParallelDeadlineGroup(new WaitCommand(1.5), new + // NoteOuttakeCommand(m_intakeSubsystem)), + // new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, 0.01) + // , new ParallelDeadlineGroup(new WaitCommand(20), new RepeatCommand(new + // InstantCommand(() -> m_robotDrive.autonDrive(new ChassisSpeeds(3, 0, 0))))) + // ); } } diff --git a/src/main/java/frc/robot/subsystems/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/LEDSubsystem.java index 3fd93f1..2e235d0 100644 --- a/src/main/java/frc/robot/subsystems/LEDSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LEDSubsystem.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj.AddressableLEDBuffer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Constants.LEDConstants; public class LEDSubsystem extends SubsystemBase { diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 81e62ae..b8bf286 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -40,12 +40,12 @@ public void reset() { public void setShootingSpeed(ShootSpeed speed) { switch (speed) { case Shooting: - m_topSpeed = ShooterConstants.kTopShooterSpeed; - m_bottomSpeed = ShooterConstants.kBottomShooterSpeed; + m_topSpeed = ShooterConstants.kShooterSpeedTop; + m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; - case Halfway: - m_topSpeed = ShooterConstants.kPreShooterSpeed; - m_bottomSpeed = ShooterConstants.kPreShooterSpeed; + case Prep: + m_topSpeed = ShooterConstants.kPrepShooterSpeed; + m_bottomSpeed = ShooterConstants.kPrepShooterSpeed; break; case Off: m_topSpeed = 0.0; @@ -70,7 +70,7 @@ public void periodic() { public static enum ShootSpeed { Shooting, - Halfway, + Prep, Off } }