From 807aed3a62b36dcf5490f10039bbe324e3feb7a3 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Mon, 26 Feb 2024 20:26:40 -0500 Subject: [PATCH 1/7] Cross the line command --- src/main/java/frc/robot/RobotContainer.java | 5 ++++ .../frc/robot/autochooser/CrossTheLine.java | 25 +++++++++++++++++++ .../java/frc/robot/constants/Constants.java | 2 +- .../frc/robot/constants/Constants2023.java | 1 + .../frc/robot/constants/GameConstants.java | 1 + 5 files changed, 33 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/autochooser/CrossTheLine.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b512a801..7cfe8506 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.autochooser.CrossTheLine; import frc.robot.autochooser.chooser.AutoChooser; import frc.robot.autochooser.chooser.AutoChooser2024; import frc.robot.commands.SetAlignable; @@ -142,6 +143,9 @@ private void setupDriveTrain() { } public void putShuffleboardCommands() { + if (Constants.AUTO_DEBUG) { + SmartShuffleboard.putCommand("Autonomous", "Cross the line", CommandUtil.logged(new CrossTheLine(drivetrain, ramp))); + } if (Constants.AMP_DEBUG) { SmartShuffleboard.putCommand("Amp", "Deploy AMP", CommandUtil.logged(new DeployAmpSequence(ramp, amp))); @@ -172,6 +176,7 @@ public void putShuffleboardCommands() { SmartShuffleboard.putCommand("Intake", "Start Intake", CommandUtil.logged(new StartIntake(intakeSubsystem,5))); } if (Constants.SWERVE_DEBUG) { + SmartShuffleboard.putCommand("Drivetrain", "Move Forward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0.3048, 0, 0.4))); SmartShuffleboard.putCommand("Drivetrain", "Move Backward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, -0.3048, 0, 0.4))); SmartShuffleboard.putCommand("Drivetrain", "Move Left 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0 , 0.3048, 0.4))); diff --git a/src/main/java/frc/robot/autochooser/CrossTheLine.java b/src/main/java/frc/robot/autochooser/CrossTheLine.java new file mode 100644 index 00000000..3b9822ca --- /dev/null +++ b/src/main/java/frc/robot/autochooser/CrossTheLine.java @@ -0,0 +1,25 @@ +package frc.robot.autochooser; + + + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.RobotContainer; +import frc.robot.commands.drivetrain.MoveDistance; +import frc.robot.commands.ramp.ResetRamp; +import frc.robot.subsystems.Ramp; +import frc.robot.subsystems.SwerveDrivetrain; + +public class CrossTheLine extends ParallelCommandGroup { + double direction; + public CrossTheLine(SwerveDrivetrain drivetrain, Ramp ramp) { + if (RobotContainer.isRedAlliance() == true) { + direction = 1.015; + } + else if (RobotContainer.isRedAlliance() == false) { + direction = -1.015; + } + addCommands( + new MoveDistance(drivetrain, direction, 0.0, 0.3), + new ResetRamp(ramp)); + } + } diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index cbb098df..eb4b806a 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -4,6 +4,6 @@ package frc.robot.constants; -public final class Constants extends Constants2024{ +public final class Constants extends Constants2023{ } diff --git a/src/main/java/frc/robot/constants/Constants2023.java b/src/main/java/frc/robot/constants/Constants2023.java index a3c239dc..77416d9d 100644 --- a/src/main/java/frc/robot/constants/Constants2023.java +++ b/src/main/java/frc/robot/constants/Constants2023.java @@ -12,6 +12,7 @@ public class Constants2023 extends GameConstants { public static final double RAMP_ERROR_RANGE = 0.00; public static final double RAMP_POS = 0.0; public static final int RAMP_ID = 45; + public static final double RAMP_ANGLE = 15.0; public static final double RAMP_MAX_RPM_ACCELERATION = 3000; public static final double RESET_RAMP_SPEED = 0.3; //assuming positive is forward, also needs to be refined do the real robot diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 82c37b56..f4bc341c 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -21,6 +21,7 @@ public class GameConstants { public static final boolean INTAKE_DEBUG = false; public static final boolean DEPLOYER_DEBUG = false; public static final boolean AMP_DEBUG = false; + public static final boolean AUTO_DEBUG = true; //DEPLOYER public static final int DEPLOYER_LOWER_TIMEOUT = 5; From a3afe61d7b16f7ec6676eabb7e2f93134ec2407a Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 1 Mar 2024 18:19:42 -0500 Subject: [PATCH 2/7] debugged auto commands --- src/main/java/frc/robot/RobotContainer.java | 2 ++ .../frc/robot/autochooser/CrossTheLine.java | 4 +-- .../frc/robot/autochooser/ShootCross.java | 36 +++++++++++++++++++ .../java/frc/robot/constants/Constants.java | 2 +- 4 files changed, 41 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/autochooser/ShootCross.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7cfe8506..256fb4bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.autochooser.CrossTheLine; +import frc.robot.autochooser.ShootCross; import frc.robot.autochooser.chooser.AutoChooser; import frc.robot.autochooser.chooser.AutoChooser2024; import frc.robot.commands.SetAlignable; @@ -145,6 +146,7 @@ private void setupDriveTrain() { public void putShuffleboardCommands() { if (Constants.AUTO_DEBUG) { SmartShuffleboard.putCommand("Autonomous", "Cross the line", CommandUtil.logged(new CrossTheLine(drivetrain, ramp))); + SmartShuffleboard.putCommand("Autonomous", "ShootCross", CommandUtil.logged(new ShootCross(drivetrain, shooter, ramp, intakeSubsystem, feeder))); } if (Constants.AMP_DEBUG) { diff --git a/src/main/java/frc/robot/autochooser/CrossTheLine.java b/src/main/java/frc/robot/autochooser/CrossTheLine.java index 3b9822ca..2bc019c9 100644 --- a/src/main/java/frc/robot/autochooser/CrossTheLine.java +++ b/src/main/java/frc/robot/autochooser/CrossTheLine.java @@ -13,10 +13,10 @@ public class CrossTheLine extends ParallelCommandGroup { double direction; public CrossTheLine(SwerveDrivetrain drivetrain, Ramp ramp) { if (RobotContainer.isRedAlliance() == true) { - direction = 1.015; + direction = -1.715; } else if (RobotContainer.isRedAlliance() == false) { - direction = -1.015; + direction = 1.715; } addCommands( new MoveDistance(drivetrain, direction, 0.0, 0.3), diff --git a/src/main/java/frc/robot/autochooser/ShootCross.java b/src/main/java/frc/robot/autochooser/ShootCross.java new file mode 100644 index 00000000..c96cd5fd --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCross.java @@ -0,0 +1,36 @@ +package frc.robot.autochooser; + + + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.RobotContainer; +import frc.robot.commands.drivetrain.MoveDistance; +import frc.robot.commands.ramp.RampMove; +import frc.robot.commands.ramp.ResetRamp; +import frc.robot.commands.sequences.SpoolExitAndShoot; +import frc.robot.commands.shooter.ShootSpeaker; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.Ramp; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.SwerveDrivetrain; + +public class ShootCross extends SequentialCommandGroup{ + double direction; + public ShootCross(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + if (RobotContainer.isRedAlliance() == true) { + direction = -1.9; + } + else if (RobotContainer.isRedAlliance() == false) { + direction = 1.9; + } + addCommands( + new ResetRamp(ramp), + new MoveDistance(drivetrain, direction, 0.0, 0.3), + new RampMove(ramp, ()->10), //change later + new WaitCommand(0.5), //change later + new SpoolExitAndShoot(shooter, feeder, drivetrain) + ); + } +} diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index eb4b806a..ce990894 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -4,6 +4,6 @@ package frc.robot.constants; -public final class Constants extends Constants2023{ +public final class Constants extends Constants2023 { } From beb536d22aeae9ee0c46c907a2646121ccf26073 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 1 Mar 2024 18:35:55 -0500 Subject: [PATCH 3/7] change --- src/main/java/frc/robot/autochooser/ShootCross.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/ShootCross.java b/src/main/java/frc/robot/autochooser/ShootCross.java index c96cd5fd..78a24820 100644 --- a/src/main/java/frc/robot/autochooser/ShootCross.java +++ b/src/main/java/frc/robot/autochooser/ShootCross.java @@ -27,10 +27,11 @@ else if (RobotContainer.isRedAlliance() == false) { } addCommands( new ResetRamp(ramp), - new MoveDistance(drivetrain, direction, 0.0, 0.3), new RampMove(ramp, ()->10), //change later new WaitCommand(0.5), //change later - new SpoolExitAndShoot(shooter, feeder, drivetrain) + new SpoolExitAndShoot(shooter, feeder, drivetrain), + new WaitCommand(0.5), + new MoveDistance(drivetrain, direction, 0.0, 0.3) ); } } From a5db1d1605f6dbd30ef1594b89495364ade6a648 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 1 Mar 2024 19:59:19 -0500 Subject: [PATCH 4/7] changes shootcross --- src/main/java/frc/robot/RobotContainer.java | 12 +++++----- .../frc/robot/autochooser/CrossTheLine.java | 2 +- .../frc/robot/autochooser/ShootCross.java | 11 ++++++---- .../commands/drivetrain/MoveDistance.java | 22 ++++++++++--------- .../java/frc/robot/constants/Constants.java | 2 +- 5 files changed, 27 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e50e7239..ce311362 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,7 +148,7 @@ private void setupDriveTrain() { public void putShuffleboardCommands() { if (Constants.AUTO_DEBUG) { SmartShuffleboard.putCommand("Autonomous", "Cross the line", CommandUtil.logged(new CrossTheLine(drivetrain, ramp))); - SmartShuffleboard.putCommand("Autonomous", "ShootCross", CommandUtil.logged(new ShootCross(drivetrain, shooter, ramp, intakeSubsystem, feeder))); + SmartShuffleboard.putCommand("Autonomous", "ShootCross", CommandUtil.logged(new ShootCross(drivetrain, shooter, ramp, intake, feeder))); } if (Constants.AMP_DEBUG) { @@ -182,11 +182,11 @@ public void putShuffleboardCommands() { } if (Constants.SWERVE_DEBUG) { - SmartShuffleboard.putCommand("Drivetrain", "Move Forward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0.3048, 0, 0.4))); - SmartShuffleboard.putCommand("Drivetrain", "Move Backward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, -0.3048, 0, 0.4))); - SmartShuffleboard.putCommand("Drivetrain", "Move Left 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0, 0.3048, 0.4))); - SmartShuffleboard.putCommand("Drivetrain", "Move Right 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0, -0.3048, 0.4))); - SmartShuffleboard.putCommand("Drivetrain", "Move Left + Forward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0.3048, 0.3048, 0.4))); + SmartShuffleboard.putCommand("Drivetrain", "Move Forward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0.3048, 0, 0.4, false))); + SmartShuffleboard.putCommand("Drivetrain", "Move Backward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, -0.3048, 0, 0.4, false))); + SmartShuffleboard.putCommand("Drivetrain", "Move Left 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0, 0.3048, 0.4, false))); + SmartShuffleboard.putCommand("Drivetrain", "Move Right 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0, -0.3048, 0.4, false))); + SmartShuffleboard.putCommand("Drivetrain", "Move Left + Forward 1ft", CommandUtil.logged(new MoveDistance(drivetrain, 0.3048, 0.3048, 0.4, false))); } } diff --git a/src/main/java/frc/robot/autochooser/CrossTheLine.java b/src/main/java/frc/robot/autochooser/CrossTheLine.java index 2bc019c9..981be778 100644 --- a/src/main/java/frc/robot/autochooser/CrossTheLine.java +++ b/src/main/java/frc/robot/autochooser/CrossTheLine.java @@ -19,7 +19,7 @@ else if (RobotContainer.isRedAlliance() == false) { direction = 1.715; } addCommands( - new MoveDistance(drivetrain, direction, 0.0, 0.3), + new MoveDistance(drivetrain, direction, 0.0, 0.3, false), new ResetRamp(ramp)); } } diff --git a/src/main/java/frc/robot/autochooser/ShootCross.java b/src/main/java/frc/robot/autochooser/ShootCross.java index 78a24820..215c8365 100644 --- a/src/main/java/frc/robot/autochooser/ShootCross.java +++ b/src/main/java/frc/robot/autochooser/ShootCross.java @@ -10,6 +10,7 @@ import frc.robot.commands.ramp.ResetRamp; import frc.robot.commands.sequences.SpoolExitAndShoot; import frc.robot.commands.shooter.ShootSpeaker; +import frc.robot.constants.GameConstants; import frc.robot.subsystems.Feeder; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Ramp; @@ -20,18 +21,20 @@ public class ShootCross extends SequentialCommandGroup{ double direction; public ShootCross(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { if (RobotContainer.isRedAlliance() == true) { - direction = -1.9; + direction = -1.0; } else if (RobotContainer.isRedAlliance() == false) { - direction = 1.9; + direction = 1.0; } addCommands( new ResetRamp(ramp), - new RampMove(ramp, ()->10), //change later + new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later new WaitCommand(0.5), //change later new SpoolExitAndShoot(shooter, feeder, drivetrain), new WaitCommand(0.5), - new MoveDistance(drivetrain, direction, 0.0, 0.3) + new MoveDistance(drivetrain, 0.0, direction * 2.74, 0.3, true), + new WaitCommand(0.5), + new MoveDistance(drivetrain, direction * 1.9, 0.0, 0.3, true) ); } } diff --git a/src/main/java/frc/robot/commands/drivetrain/MoveDistance.java b/src/main/java/frc/robot/commands/drivetrain/MoveDistance.java index a2dcc6d6..8242252c 100644 --- a/src/main/java/frc/robot/commands/drivetrain/MoveDistance.java +++ b/src/main/java/frc/robot/commands/drivetrain/MoveDistance.java @@ -18,15 +18,17 @@ public class MoveDistance extends Command { private double maxSpeed; private double desiredPoseX; private double desiredPoseY; + private boolean fieldCentric; private SwerveDrivetrain drivetrain; private TimeoutCounter timeoutCounter = new TimeoutCounter("Move Distance"); - public MoveDistance(SwerveDrivetrain drivetrain, double changeXMeters, double changeYMeters, double maxSpeed) { + public MoveDistance(SwerveDrivetrain drivetrain, double changeXMeters, double changeYMeters, double maxSpeed, boolean fieldCentric) { // Use addRequirements() here to declare subsystem dependencies. this.drivetrain = drivetrain; this.changeXMeters = changeXMeters; this.changeYMeters = changeYMeters; this.maxSpeed = Math.abs(maxSpeed); + this.fieldCentric = fieldCentric; addRequirements(drivetrain); } @@ -41,15 +43,15 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double speedY = 0; - double speedX = 0; - double neededChangeX = desiredPoseX - drivetrain.getPose().getX(); - double neededChangeY = desiredPoseY - drivetrain.getPose().getY(); - if ((neededChangeX != 0) || (neededChangeY != 0)) { - speedX = (neededChangeX * maxSpeed) / (Math.abs(neededChangeX) + Math.abs(neededChangeY)); - speedY = (neededChangeY * maxSpeed) / (Math.abs(neededChangeX) + Math.abs(neededChangeY)); - } - drivetrain.drive(drivetrain.createChassisSpeeds(speedX, speedY, 0.0, Constants.FIELD_RELATIVE)); + double speedY = 0; + double speedX = 0; + double neededChangeX = desiredPoseX - drivetrain.getPose().getX(); + double neededChangeY = desiredPoseY - drivetrain.getPose().getY(); + if ((neededChangeX != 0) || (neededChangeY != 0)) { + speedX = (neededChangeX * maxSpeed) / (Math.abs(neededChangeX) + Math.abs(neededChangeY)); + speedY = (neededChangeY * maxSpeed) / (Math.abs(neededChangeX) + Math.abs(neededChangeY)); + } + drivetrain.drive(drivetrain.createChassisSpeeds(speedX, speedY, 0.0, Constants.FIELD_RELATIVE)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index 5a9de856..d47a4985 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -4,6 +4,6 @@ package frc.robot.constants; -public final class Constants extends Constants2023 { +public final class Constants extends Constants2024 { } \ No newline at end of file From c3181943fea9a8765760eb4ff29e3250756a31e0 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sat, 2 Mar 2024 12:50:04 -0500 Subject: [PATCH 5/7] Added commands to autochooser --- src/main/java/frc/robot/RobotContainer.java | 10 ++--- .../frc/robot/autochooser/AutoAction.java | 3 +- .../{CrossTheLine.java => CrossLine.java} | 4 +- .../frc/robot/autochooser/ShootCrossLeft.java | 38 +++++++++++++++++++ .../frc/robot/autochooser/ShootCrossMid.java | 36 ++++++++++++++++++ .../{ShootCross.java => ShootCrossRight.java} | 6 +-- .../autochooser/chooser/AutoChooser2024.java | 12 ++++-- 7 files changed, 94 insertions(+), 15 deletions(-) rename src/main/java/frc/robot/autochooser/{CrossTheLine.java => CrossLine.java} (84%) create mode 100644 src/main/java/frc/robot/autochooser/ShootCrossLeft.java create mode 100644 src/main/java/frc/robot/autochooser/ShootCrossMid.java rename src/main/java/frc/robot/autochooser/{ShootCross.java => ShootCrossRight.java} (88%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ce311362..1f42b090 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,8 +17,8 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import frc.robot.autochooser.CrossTheLine; -import frc.robot.autochooser.ShootCross; +import frc.robot.autochooser.CrossLine; +import frc.robot.autochooser.ShootCrossRight; import frc.robot.autochooser.chooser.AutoChooser; import frc.robot.autochooser.chooser.AutoChooser2024; import frc.robot.commands.CancelAll; @@ -94,7 +94,7 @@ public RobotContainer() { setupDriveTrain(); registerPathPlanableCommands(); setupPathPlanning(); - autoChooser = new AutoChooser2024(intake, shooter, feeder, deployer, ramp); + autoChooser = new AutoChooser2024(drivetrain, intake, shooter, feeder, deployer, ramp); autoChooser.addOnValidationCommand(() -> CommandUtil.logged(new SetInitOdom(drivetrain, autoChooser))); autoChooser.forceRefresh(); configureBindings(); @@ -147,8 +147,8 @@ private void setupDriveTrain() { public void putShuffleboardCommands() { if (Constants.AUTO_DEBUG) { - SmartShuffleboard.putCommand("Autonomous", "Cross the line", CommandUtil.logged(new CrossTheLine(drivetrain, ramp))); - SmartShuffleboard.putCommand("Autonomous", "ShootCross", CommandUtil.logged(new ShootCross(drivetrain, shooter, ramp, intake, feeder))); + SmartShuffleboard.putCommand("Autonomous", "Cross the line", CommandUtil.logged(new CrossLine(drivetrain, ramp))); + SmartShuffleboard.putCommand("Autonomous", "ShootCross", CommandUtil.logged(new ShootCrossRight(drivetrain, shooter, ramp, intake, feeder))); } if (Constants.AMP_DEBUG) { diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index 5d507308..5a7ddc4d 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -5,7 +5,8 @@ public enum AutoAction { ShootAndCross("Shoot & Cross"), ShootFour("Shoot Four"), ShootTwo("Shoot Two"), - ShootTwoDip("Shoot Two & Dip"); + ShootTwoDip("Shoot Two & Dip"), + ShootCross("Shoot & Cross"); private final String name; AutoAction(String name) { diff --git a/src/main/java/frc/robot/autochooser/CrossTheLine.java b/src/main/java/frc/robot/autochooser/CrossLine.java similarity index 84% rename from src/main/java/frc/robot/autochooser/CrossTheLine.java rename to src/main/java/frc/robot/autochooser/CrossLine.java index 981be778..ef8dcf6f 100644 --- a/src/main/java/frc/robot/autochooser/CrossTheLine.java +++ b/src/main/java/frc/robot/autochooser/CrossLine.java @@ -9,9 +9,9 @@ import frc.robot.subsystems.Ramp; import frc.robot.subsystems.SwerveDrivetrain; -public class CrossTheLine extends ParallelCommandGroup { +public class CrossLine extends ParallelCommandGroup { double direction; - public CrossTheLine(SwerveDrivetrain drivetrain, Ramp ramp) { + public CrossLine(SwerveDrivetrain drivetrain, Ramp ramp) { if (RobotContainer.isRedAlliance() == true) { direction = -1.715; } diff --git a/src/main/java/frc/robot/autochooser/ShootCrossLeft.java b/src/main/java/frc/robot/autochooser/ShootCrossLeft.java new file mode 100644 index 00000000..00432d9c --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCrossLeft.java @@ -0,0 +1,38 @@ +package frc.robot.autochooser; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.RobotContainer; +import frc.robot.commands.drivetrain.MoveDistance; +import frc.robot.commands.ramp.RampMove; +import frc.robot.commands.ramp.ResetRamp; +import frc.robot.commands.sequences.SpoolExitAndShoot; +import frc.robot.commands.shooter.ShootSpeaker; +import frc.robot.constants.GameConstants; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.Ramp; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.SwerveDrivetrain; + +public class ShootCrossLeft extends SequentialCommandGroup{ + double direction; + public ShootCrossLeft(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + if (RobotContainer.isRedAlliance() == true) { + direction = -1.0; + } + else if (RobotContainer.isRedAlliance() == false) { + direction = 1.0; + } + addCommands( + new ResetRamp(ramp), + new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later + new WaitCommand(0.5), //change later + new SpoolExitAndShoot(shooter, feeder, drivetrain), + new WaitCommand(0.5), + new MoveDistance(drivetrain, 0.0, 1.2, 0.3, true), + new WaitCommand(0.5), + new MoveDistance(drivetrain, direction * 1.9, 0.0, 0.3, true) + ); + } +} diff --git a/src/main/java/frc/robot/autochooser/ShootCrossMid.java b/src/main/java/frc/robot/autochooser/ShootCrossMid.java new file mode 100644 index 00000000..c69e4b30 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCrossMid.java @@ -0,0 +1,36 @@ +package frc.robot.autochooser; + +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.RobotContainer; +import frc.robot.commands.drivetrain.MoveDistance; +import frc.robot.commands.ramp.RampMove; +import frc.robot.commands.ramp.ResetRamp; +import frc.robot.commands.sequences.SpoolExitAndShoot; +import frc.robot.commands.shooter.ShootSpeaker; +import frc.robot.constants.GameConstants; +import frc.robot.subsystems.Feeder; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.Ramp; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.SwerveDrivetrain; + +public class ShootCrossMid extends SequentialCommandGroup{ + double direction; + public ShootCrossMid(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + if (RobotContainer.isRedAlliance() == true) { + direction = -1.0; + } + else if (RobotContainer.isRedAlliance() == false) { + direction = 1.0; + } + addCommands( + new ResetRamp(ramp), + new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later + new WaitCommand(0.5), //change later + new SpoolExitAndShoot(shooter, feeder, drivetrain), + new WaitCommand(0.5), + new MoveDistance(drivetrain, direction*1.9, 0.0, 0.3, true) + ); + } +} diff --git a/src/main/java/frc/robot/autochooser/ShootCross.java b/src/main/java/frc/robot/autochooser/ShootCrossRight.java similarity index 88% rename from src/main/java/frc/robot/autochooser/ShootCross.java rename to src/main/java/frc/robot/autochooser/ShootCrossRight.java index 215c8365..ee7d63ae 100644 --- a/src/main/java/frc/robot/autochooser/ShootCross.java +++ b/src/main/java/frc/robot/autochooser/ShootCrossRight.java @@ -1,7 +1,5 @@ package frc.robot.autochooser; - - import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.RobotContainer; @@ -17,9 +15,9 @@ import frc.robot.subsystems.Shooter; import frc.robot.subsystems.SwerveDrivetrain; -public class ShootCross extends SequentialCommandGroup{ +public class ShootCrossRight extends SequentialCommandGroup{ double direction; - public ShootCross(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + public ShootCrossRight(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { if (RobotContainer.isRedAlliance() == true) { direction = -1.0; } diff --git a/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java index 6e5f4043..9284794d 100644 --- a/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java +++ b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java @@ -6,6 +6,9 @@ import frc.robot.autochooser.AutoAction; import frc.robot.autochooser.FieldLocation; import frc.robot.autochooser.PlaceHolderCommand; +import frc.robot.autochooser.ShootCrossLeft; +import frc.robot.autochooser.ShootCrossMid; +import frc.robot.autochooser.ShootCrossRight; import frc.robot.autochooser.event.AutoEvent; import frc.robot.commands.pathplanning.FourPieceLeft; import frc.robot.commands.pathplanning.ShootAndDrop; @@ -16,7 +19,7 @@ public class AutoChooser2024 extends Nt4AutoValidationChooser { private final Map commandMap; - public AutoChooser2024(IntakeSubsystem intakeSubsystem, Shooter shooter, Feeder feeder, Deployer deployer, Ramp ramp) { + public AutoChooser2024(SwerveDrivetrain drivetrain, IntakeSubsystem intakeSubsystem, Shooter shooter, Feeder feeder, Deployer deployer, Ramp ramp) { super(AutoAction.DoNothing, FieldLocation.SpeakFront); commandMap = Map.ofEntries( Map.entry(new AutoEvent(AutoAction.DoNothing, FieldLocation.SpeakFront), new PlaceHolderCommand()), @@ -29,8 +32,11 @@ public AutoChooser2024(IntakeSubsystem intakeSubsystem, Shooter shooter, Feeder Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakFront), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossMid"))), Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakerLeft), new FourPieceLeft(shooter,feeder,deployer)), Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakerRight), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot4Right"))), - Map.entry(new AutoEvent(AutoAction.ShootTwo, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot2Left"))) - ); + Map.entry(new AutoEvent(AutoAction.ShootTwo, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot2Left"))), + Map.entry(new AutoEvent(AutoAction.ShootCross, FieldLocation.SpeakFront), new ShootCrossMid(drivetrain, shooter, ramp, intakeSubsystem, feeder)), + Map.entry(new AutoEvent(AutoAction.ShootCross, FieldLocation.SpeakerRight), new ShootCrossRight(drivetrain, shooter, ramp, intakeSubsystem, feeder)), + Map.entry(new AutoEvent(AutoAction.ShootCross, FieldLocation.SpeakerLeft), new ShootCrossLeft(drivetrain, shooter, ramp, intakeSubsystem, feeder)) + ); } From fed2a5e870743e8eb96caec8a6aadc9d50f1bcc7 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sat, 2 Mar 2024 14:54:32 -0500 Subject: [PATCH 6/7] made changes to add commands to auto chooser --- src/main/java/frc/robot/autochooser/AutoAction.java | 1 - .../java/frc/robot/autochooser/ShootCrossLeft.java | 11 ++++------- .../java/frc/robot/autochooser/ShootCrossMid.java | 7 ++----- .../java/frc/robot/autochooser/ShootCrossRight.java | 9 +++------ .../robot/autochooser/chooser/AutoChooser2024.java | 4 +--- src/main/java/frc/robot/constants/Constants.java | 2 +- 6 files changed, 11 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index 5a7ddc4d..d0a0f261 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -2,7 +2,6 @@ public enum AutoAction { DoNothing("Do Nothing"), - ShootAndCross("Shoot & Cross"), ShootFour("Shoot Four"), ShootTwo("Shoot Two"), ShootTwoDip("Shoot Two & Dip"), diff --git a/src/main/java/frc/robot/autochooser/ShootCrossLeft.java b/src/main/java/frc/robot/autochooser/ShootCrossLeft.java index 00432d9c..3d8d2a7a 100644 --- a/src/main/java/frc/robot/autochooser/ShootCrossLeft.java +++ b/src/main/java/frc/robot/autochooser/ShootCrossLeft.java @@ -16,23 +16,20 @@ import frc.robot.subsystems.SwerveDrivetrain; public class ShootCrossLeft extends SequentialCommandGroup{ - double direction; public ShootCrossLeft(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; if (RobotContainer.isRedAlliance() == true) { direction = -1.0; } - else if (RobotContainer.isRedAlliance() == false) { - direction = 1.0; - } addCommands( new ResetRamp(ramp), new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later new WaitCommand(0.5), //change later new SpoolExitAndShoot(shooter, feeder, drivetrain), new WaitCommand(0.5), - new MoveDistance(drivetrain, 0.0, 1.2, 0.3, true), - new WaitCommand(0.5), - new MoveDistance(drivetrain, direction * 1.9, 0.0, 0.3, true) + new MoveDistance(drivetrain, 0.0, .4, 0.5, true), + new WaitCommand(5), + new MoveDistance(drivetrain, direction * 2.4, 0.0, 0.5, true) ); } } diff --git a/src/main/java/frc/robot/autochooser/ShootCrossMid.java b/src/main/java/frc/robot/autochooser/ShootCrossMid.java index c69e4b30..4b444bb7 100644 --- a/src/main/java/frc/robot/autochooser/ShootCrossMid.java +++ b/src/main/java/frc/robot/autochooser/ShootCrossMid.java @@ -16,21 +16,18 @@ import frc.robot.subsystems.SwerveDrivetrain; public class ShootCrossMid extends SequentialCommandGroup{ - double direction; public ShootCrossMid(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; if (RobotContainer.isRedAlliance() == true) { direction = -1.0; } - else if (RobotContainer.isRedAlliance() == false) { - direction = 1.0; - } addCommands( new ResetRamp(ramp), new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later new WaitCommand(0.5), //change later new SpoolExitAndShoot(shooter, feeder, drivetrain), new WaitCommand(0.5), - new MoveDistance(drivetrain, direction*1.9, 0.0, 0.3, true) + new MoveDistance(drivetrain, direction * 1.9, 0.0, 0.5, true) ); } } diff --git a/src/main/java/frc/robot/autochooser/ShootCrossRight.java b/src/main/java/frc/robot/autochooser/ShootCrossRight.java index ee7d63ae..e5f004c3 100644 --- a/src/main/java/frc/robot/autochooser/ShootCrossRight.java +++ b/src/main/java/frc/robot/autochooser/ShootCrossRight.java @@ -16,23 +16,20 @@ import frc.robot.subsystems.SwerveDrivetrain; public class ShootCrossRight extends SequentialCommandGroup{ - double direction; public ShootCrossRight(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; if (RobotContainer.isRedAlliance() == true) { direction = -1.0; } - else if (RobotContainer.isRedAlliance() == false) { - direction = 1.0; - } addCommands( new ResetRamp(ramp), new RampMove(ramp, ()->GameConstants.RAMP_POS_SHOOT_SPEAKER_CLOSE), //change later new WaitCommand(0.5), //change later new SpoolExitAndShoot(shooter, feeder, drivetrain), new WaitCommand(0.5), - new MoveDistance(drivetrain, 0.0, direction * 2.74, 0.3, true), + new MoveDistance(drivetrain, 0.0, -2.74, 0.5, true), new WaitCommand(0.5), - new MoveDistance(drivetrain, direction * 1.9, 0.0, 0.3, true) + new MoveDistance(drivetrain, direction * 2.2, 0.0, 0.5, true) ); } } diff --git a/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java index 9284794d..46cc5976 100644 --- a/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java +++ b/src/main/java/frc/robot/autochooser/chooser/AutoChooser2024.java @@ -27,9 +27,7 @@ public AutoChooser2024(SwerveDrivetrain drivetrain, IntakeSubsystem intakeSubsys Map.entry(new AutoEvent(AutoAction.DoNothing, FieldLocation.SpeakerLeft), new PlaceHolderCommand()), Map.entry(new AutoEvent(AutoAction.DoNothing, FieldLocation.ZERO), new PlaceHolderCommand()), Map.entry(new AutoEvent(AutoAction.ShootTwoDip, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot2AndDip")).beforeStarting(new ShootAndDrop(shooter,feeder,deployer))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakerRight), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossRight"))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossLeft"))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakFront), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossMid"))), + Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakerLeft), new FourPieceLeft(shooter,feeder,deployer)), Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakerRight), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot4Right"))), Map.entry(new AutoEvent(AutoAction.ShootTwo, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("Shoot2Left"))), diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index d47a4985..5a9de856 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -4,6 +4,6 @@ package frc.robot.constants; -public final class Constants extends Constants2024 { +public final class Constants extends Constants2023 { } \ No newline at end of file From a61ec1d34503289b4817d538cdcdb74f96788739 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Mon, 4 Mar 2024 19:52:54 -0500 Subject: [PATCH 7/7] added changes --- src/main/java/frc/robot/constants/Constants2023.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/constants/Constants2023.java b/src/main/java/frc/robot/constants/Constants2023.java index 36355332..2310b925 100644 --- a/src/main/java/frc/robot/constants/Constants2023.java +++ b/src/main/java/frc/robot/constants/Constants2023.java @@ -11,12 +11,9 @@ public class Constants2023 extends GameConstants { //RAMP public static final double RAMP_ERROR_RANGE = 0.00; public static final int RAMP_ID = 45; -<<<<<<< HEAD public static final double RAMP_ANGLE = 15.0; public static final double RAMP_MAX_RPM_ACCELERATION = 3000; public static final double RESET_RAMP_SPEED = 0.3; //assuming positive is forward, also needs to be refined do the real robot -======= ->>>>>>> main //Servo public static final int RIGHT_SERVO_ENGAGED = 0;