swerve straight start of auto

KHS-Robotics · Mar 5, 2024 · 72b4491 · 72b4491
1 parent 7655393
commit 72b4491
Show file tree

Hide file tree

Showing 7 changed files with 39 additions and 4 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto b/src/main/deploy/pathplanner/autos/Amp Side Shoot 2.auto
@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

diff --git a/src/main/deploy/pathplanner/autos/Center Shoot 2.auto b/src/main/deploy/pathplanner/autos/Center Shoot 2.auto
@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

diff --git a/src/main/deploy/pathplanner/autos/Center Shoot 3.auto b/src/main/deploy/pathplanner/autos/Center Shoot 3.auto
@@ -32,6 +32,12 @@
                 "data": {
                   "name": "RampShooterForManualShot"
                 }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "SwerveStraighten"
+                }
               }
             ]
           }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -15,6 +15,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.SerialPort.Port;
@@ -24,7 +25,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -384,6 +385,22 @@ private void registerNamedCommands() {
 
     // Swerves
     NamedCommands.registerCommand("StopSwerves", new InstantCommand(() -> swerveDrive.stop(), swerveDrive));
+    // SwerveStraighten - used when lowering intake and deploying demon horns to make initial odometry updates more accurate
+    NamedCommands.registerCommand("SwerveStraighten", 
+      new RepeatCommand(
+        new InstantCommand(() -> 
+          swerveDrive.setModuleStates(new SwerveModuleState[] {
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0)),
+            new SwerveModuleState(0, Rotation2d.fromDegrees(0))
+          }),
+          swerveDrive
+        )
+      )
+      .withTimeout(1)
+      .andThen(new InstantCommand(() -> swerveDrive.stop(), swerveDrive))
+    );
   }
 
   private void configurePathPlannerLogging() {

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
@@ -98,7 +98,7 @@ public enum ArmState {
   public void periodic() {
     // SmartDashboard.putNumber("armAngle", getPosition());
     // SmartDashboard.putNumber("armSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
+    // SmartDashboard.putNumber("armError", Math.abs(rotationSetpoint - getPosition()));
 
     // kG = SmartDashboard.getNumber("kG", kG);
     // kP = SmartDashboard.getNumber("kP", kP);

diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
@@ -127,7 +127,7 @@ public enum IntakeState {
   public void periodic() {
     // SmartDashboard.putNumber("IntakeAngle", getPosition());
     // SmartDashboard.putNumber("IntakeSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
+    // SmartDashboard.putNumber("IntakeError", Math.abs(getPosition() - rotationSetpoint));
     // SmartDashboard.putBoolean("IntakeHasNote", hasNoteInside());
 
     // kP = SmartDashboard.getNumber("kp", kP);

diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -398,7 +398,7 @@ public enum ShooterState {
   public void periodic() {
     // SmartDashboard.putNumber("shooterAngle", getPosition());
     // SmartDashboard.putNumber("shooterAngleSetpoint", rotationSetpoint);
-    SmartDashboard.putNumber("shooterAngleError", Math.abs(rotationSetpoint - getPosition()));
+    // SmartDashboard.putNumber("shooterAngleError", Math.abs(rotationSetpoint - getPosition()));
     // SmartDashboard.putNumber("shooterAngleAbsolute", getAbsoluteAngle());
     // SmartDashboard.putNumber("Shooter-Velocity", getVelocity());
     // SmartDashboard.putBoolean("Shooter-HasNote", hasNote());