5 meters forwards, turn 180 then continue 5 meters in that direction

SFUnity · Jan 8, 2024 · c6101c7 · c6101c7
1 parent 5f6b555
commit c6101c7
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 20 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -56,16 +56,16 @@ public RobotContainer() {
         configureBindings();
 
         // Add commands to the autonomous command chooser
-        m_chooser.setDefaultOption("Complex Auto", m_complexAuto);
+        m_chooser.addOption("Complex Auto", m_complexAuto);
 
         m_chooser.addOption("Straight Auto", m_straightAuto);
 
         m_chooser.addOption("Circle Auto", m_circleAuto);
 
-        m_chooser.addOption("Custom Auto", m_customAuto);
+        m_chooser.setDefaultOption("Custom Auto", m_customAuto);
 
         // Put the chooser on the dashboard
-        SmartDashboard.putData("Auto Options", m_chooser);
+        Shuffleboard.getTab("Auto Options").add(m_chooser);
     }
 
   private void configureBindings() {

diff --git a/src/main/java/frc/robot/commands/CustomTestCmd.java b/src/main/java/frc/robot/commands/CustomTestCmd.java
@@ -9,9 +9,9 @@ public class CustomTestCmd extends SequentialCommandGroup{
 
     public CustomTestCmd(SwerveSubsystem swerveSubsystem) {
         addCommands(
-            new DriveStraightCmd(0.0, 2.0, swerveSubsystem),
+            new DriveStraightCmd(5.0, 0.0, 5.0, swerveSubsystem),
             new RotateDegreesCmd(180.0, true, swerveSubsystem),
-            new DriveStraightCmd(0.0, 2.0, swerveSubsystem)
+            new DriveStraightCmd(-5.0, 0.0, 5.0, swerveSubsystem)
         );
     }
 

diff --git a/src/main/java/frc/robot/commands/DriveStraightCmd.java b/src/main/java/frc/robot/commands/DriveStraightCmd.java
@@ -7,26 +7,34 @@
 
 public class DriveStraightCmd extends CommandBase {
     private final SwerveSubsystem m_swerveSubsystem;
-    private final Double rotationDegrees, meters; 
+    private final Double rotationDegrees, meters, speed; 
     private Double initialPosX, initialPosY;
 
-
-    public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
+    public DriveStraightCmd(Double speed, Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.meters = meters;
+        this.speed = speed;
         addRequirements(swerveSubsystem);
     }
 
+    public DriveStraightCmd(Double rotationDegrees, Double meters, SwerveSubsystem swerveSubsystem) {
+        this(rotationDegrees, 1.0, meters, swerveSubsystem);
+    }
+
     @Override
-    public void execute() {
+    public void initialize() {
         initialPosX = m_swerveSubsystem.getPose().getX();
         initialPosY = m_swerveSubsystem.getPose().getY();
+    }
+
+    @Override
+    public void execute() {
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees)),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(rotationDegrees))
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees)),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(rotationDegrees))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }

diff --git a/src/main/java/frc/robot/commands/RotateDegreesCmd.java b/src/main/java/frc/robot/commands/RotateDegreesCmd.java
@@ -7,26 +7,35 @@
 
 public class RotateDegreesCmd extends CommandBase {
     private final SwerveSubsystem m_swerveSubsystem;
-    private final Double rotationDegrees;
+    private final Double rotationDegrees, speed;
     private final Boolean rotateRight;
     private Double initialRotation;
 
 
-    public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
+    public RotateDegreesCmd(Double speed, Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
         m_swerveSubsystem = swerveSubsystem;
         this.rotationDegrees = rotationDegrees;
         this.rotateRight = rotateRight;
+        this.speed = speed;
         addRequirements(swerveSubsystem);
     }
 
+    public RotateDegreesCmd(Double rotationDegrees, Boolean rotateRight, SwerveSubsystem swerveSubsystem) {
+        this(rotationDegrees, 1.0, rotateRight, swerveSubsystem);
+    }
+
     @Override
-    public void execute() {
+    public void initialize() {
         initialRotation = m_swerveSubsystem.getPose().getRotation().getDegrees();
+    }
+
+    @Override
+    public void execute() {
         SwerveModuleState[] module_states = {
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
-            new SwerveModuleState(1.0, Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1)))
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(135 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(45 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(-135 * (rotateRight ? 1 : -1))),
+            new SwerveModuleState(speed, Rotation2d.fromDegrees(-45 * (rotateRight ? 1 : -1)))
         };
         m_swerveSubsystem.setModuleStates(module_states);
     }