diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc2d2873..37b4ad0e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,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.CrossLine; +import frc.robot.autochooser.ShootCrossRight; import frc.robot.autochooser.chooser.AutoChooser; import frc.robot.autochooser.chooser.AutoChooser2024; import frc.robot.commands.MoveToGamepiece; @@ -93,7 +95,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(); @@ -155,6 +157,10 @@ private void setupDriveTrain() { } public void putShuffleboardCommands() { + if (Constants.AUTO_DEBUG) { + 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) { // SmartShuffleboard.putCommand("Amp", "Deploy AMP", CommandUtil.logged(new DeployAmpSequence(ramp, amp))); @@ -187,11 +193,12 @@ public void putShuffleboardCommands() { SmartShuffleboard.putCommand("Intake", "Start Intake", CommandUtil.logged(new StartIntake(intake, 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))); - 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/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java index 12aa3c70..e3b8e9d8 100644 --- a/src/main/java/frc/robot/autochooser/AutoAction.java +++ b/src/main/java/frc/robot/autochooser/AutoAction.java @@ -2,10 +2,10 @@ public enum AutoAction { DoNothing("Do Nothing"), - ShootAndCross("Shoot & Cross"), ShootFour("Shoot Four"), ShootTwo("Shoot Two"), ShootTwoDip("Shoot Two & Dip"), + ShootCross("Shoot & Cross"), Fork("Fork"); private final String name; diff --git a/src/main/java/frc/robot/autochooser/CrossLine.java b/src/main/java/frc/robot/autochooser/CrossLine.java new file mode 100644 index 00000000..ef8dcf6f --- /dev/null +++ b/src/main/java/frc/robot/autochooser/CrossLine.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 CrossLine extends ParallelCommandGroup { + double direction; + public CrossLine(SwerveDrivetrain drivetrain, Ramp ramp) { + if (RobotContainer.isRedAlliance() == true) { + direction = -1.715; + } + else if (RobotContainer.isRedAlliance() == false) { + direction = 1.715; + } + addCommands( + new MoveDistance(drivetrain, direction, 0.0, 0.3, false), + new ResetRamp(ramp)); + } + } 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..3d8d2a7a --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCrossLeft.java @@ -0,0 +1,35 @@ +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{ + public ShootCrossLeft(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; + if (RobotContainer.isRedAlliance() == true) { + 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, .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 new file mode 100644 index 00000000..4b444bb7 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCrossMid.java @@ -0,0 +1,33 @@ +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{ + public ShootCrossMid(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; + if (RobotContainer.isRedAlliance() == true) { + 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.5, true) + ); + } +} diff --git a/src/main/java/frc/robot/autochooser/ShootCrossRight.java b/src/main/java/frc/robot/autochooser/ShootCrossRight.java new file mode 100644 index 00000000..e5f004c3 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/ShootCrossRight.java @@ -0,0 +1,35 @@ +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 ShootCrossRight extends SequentialCommandGroup{ + public ShootCrossRight(SwerveDrivetrain drivetrain, Shooter shooter, Ramp ramp, IntakeSubsystem intake, Feeder feeder) { + double direction = 1.0; + if (RobotContainer.isRedAlliance() == true) { + 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, -2.74, 0.5, true), + new WaitCommand(0.5), + 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 88743a7a..cd01efc9 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.ShootAndDrop; import frc.robot.subsystems.*; @@ -15,7 +18,7 @@ public class AutoChooser2024 extends Nt4AutoValidationChooser { private final Map commandMap; - public AutoChooser2024(IntakeSubsystem intake, 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()), @@ -23,9 +26,9 @@ public AutoChooser2024(IntakeSubsystem intake, Shooter shooter, Feeder feeder, D 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,ramp))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakerRight), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossRight")).beforeStarting(new ShootAndDrop(shooter,feeder,deployer,ramp))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakerLeft), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossLeft")).beforeStarting(new ShootAndDrop(shooter,feeder,deployer,ramp))), - Map.entry(new AutoEvent(AutoAction.ShootAndCross, FieldLocation.SpeakFront), AutoBuilder.followPath(PathPlannerPath.fromPathFile("ShootAndCrossMid")).beforeStarting(new ShootAndDrop(shooter,feeder,deployer,ramp))), + 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)), Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakerLeft), AutoBuilder.buildAuto("FourPieceLeft")), Map.entry(new AutoEvent(AutoAction.ShootFour, FieldLocation.SpeakFront), AutoBuilder.buildAuto("FourPieceCenter")), Map.entry(new AutoEvent(AutoAction.Fork, FieldLocation.SpeakerRight), AutoBuilder.buildAuto("ForkAuto")), 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 ad9b07e8..d47a4985 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -5,4 +5,5 @@ package frc.robot.constants; public final class Constants extends Constants2024 { + } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/Constants2023.java b/src/main/java/frc/robot/constants/Constants2023.java index 565d2100..2310b925 100644 --- a/src/main/java/frc/robot/constants/Constants2023.java +++ b/src/main/java/frc/robot/constants/Constants2023.java @@ -11,6 +11,9 @@ public class Constants2023 extends GameConstants { //RAMP public static final double RAMP_ERROR_RANGE = 0.00; 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 //Servo public static final int RIGHT_SERVO_ENGAGED = 0; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 004577d3..cf836142 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -17,6 +17,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 = false; public static final boolean VISION_DEBUG = false; public static final boolean PATHPLANNER_DEBUG = false; public static final boolean ENABLE_LOGGING = true;