Sp kb basic auto

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)));
         }
     }
 

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;
 

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));
+  }
+    }

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)
+        );
+    }
+}

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)
+        );
+    }
+}

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)
+        );
+    }
+}

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,17 +18,17 @@
 public class AutoChooser2024 extends Nt4AutoValidationChooser {
     private final Map<AutoEvent, Command> 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()),
                 Map.entry(new AutoEvent(AutoAction.DoNothing, FieldLocation.SpeakerRight), new PlaceHolderCommand()),
                 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")),

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.

src/main/java/frc/robot/constants/Constants.java

@@ -5,4 +5,5 @@
 package frc.robot.constants;
 
 public final class Constants extends Constants2024 {
+
 }

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;

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;