Merge pull request #105 from strykeforce/iri-hotfix

Iri hotfix
strykeforce · Jul 16, 2024 · 3655202 · 3655202
2 parents 6011063 + a293736
commit 3655202
Show file tree

Hide file tree

Showing 19 changed files with 188 additions and 47 deletions.
diff --git a/src/main/deploy/paths/MiddleNote4_NonAmpShoot2_B.toml b/src/main/deploy/paths/MiddleNote4_NonAmpShoot2_B.toml
@@ -1,5 +1,5 @@
 max_velocity = 3.0 #m/s
-max_acceleration = 3.0 #m/s
+max_acceleration = 1.5 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false

diff --git a/src/main/deploy/paths/MiddleNote5_NonAmpShoot2.toml b/src/main/deploy/paths/MiddleNote5_NonAmpShoot2.toml
@@ -1,5 +1,5 @@
 max_velocity = 5.0 #m/s
-max_acceleration = 3.0 #m/s
+max_acceleration = 1.5 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -66,7 +66,6 @@
 import frc.robot.commands.robotState.ClimbCommand;
 import frc.robot.commands.robotState.ClimbTrapDecendCommand;
 import frc.robot.commands.robotState.DecendCommand;
-import frc.robot.commands.robotState.FeedCommand;
 import frc.robot.commands.robotState.FullTrapClimbCommand;
 import frc.robot.commands.robotState.IntakeCommand;
 import frc.robot.commands.robotState.MovingVisionShootCommand;
@@ -80,6 +79,7 @@
 import frc.robot.commands.robotState.SpeedUpPassCommand;
 import frc.robot.commands.robotState.StowCommand;
 import frc.robot.commands.robotState.SubWooferCommand;
+import frc.robot.commands.robotState.TogglePunchAirCommand;
 import frc.robot.commands.robotState.TunedShotCommand;
 import frc.robot.commands.robotState.TuningOffCommand;
 import frc.robot.commands.robotState.TuningShootCommand;
@@ -956,10 +956,10 @@ private void configureOperatorBindings() {
         .onTrue(new SpeedUpPassCommand(robotStateSubsystem, superStructure));
 
     // Defense
-    // new JoystickButton(xboxController, XboxController.Button.kB.value)
-    //     .onTrue(new TogglePunchAirCommand(robotStateSubsystem));
+    new JoystickButton(xboxController, XboxController.Button.kB.value)
+        .onTrue(new TogglePunchAirCommand(robotStateSubsystem));
 
-    new JoystickButton(xboxController, XboxController.Button.kB.value).onTrue(testAuto);
+    // new JoystickButton(xboxController, XboxController.Button.kB.value).onTrue(testAuto);
 
     // new JoystickButton(xboxController, XboxController.Button.kB.value)
     //     .onTrue(new ToggleDefenseCommand(robotStateSubsystem, superStructure,
@@ -1088,18 +1088,18 @@ private void configureDriverBindings() {
     new JoystickButton(driveJoystick, Button.SWG_DWN.id)
         .onFalse(new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem));
 
-    new JoystickButton(driveJoystick, Button.SWF_UP.id)
-        .onFalse(
-            new FeedCommand(
-                robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
-    new JoystickButton(driveJoystick, Button.SWF_DWN.id)
-        .onTrue(
-            new FeedCommand(
-                robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
-    new JoystickButton(driveJoystick, Button.SWF_DWN.id)
-        .onFalse(
-            new FeedCommand(
-                robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
+    // new JoystickButton(driveJoystick, Button.SWF_UP.id)
+    //     .onFalse(
+    //         new FeedCommand(
+    //             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
+    // new JoystickButton(driveJoystick, Button.SWF_DWN.id)
+    //     .onTrue(
+    //         new FeedCommand(
+    //             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
+    // new JoystickButton(driveJoystick, Button.SWF_DWN.id)
+    //     .onFalse(
+    //         new FeedCommand(
+    //             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
   }
 
   public void configureClimbTestBindings() {

diff --git a/src/main/java/frc/robot/commands/auton/DeadeyeHuntCommand.java b/src/main/java/frc/robot/commands/auton/DeadeyeHuntCommand.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.constants.AutonConstants;
 import frc.robot.subsystems.drive.DriveSubsystem;
@@ -56,7 +57,13 @@ public void initialize() {
       driveSubsystem.move(AutonConstants.kXSpeed / (xSpeed * xSpeed + 1), ySpeed, 0.0, false);
       seenNote++;
     } else {
-      driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, false);
+      driveSubsystem.move(
+          0.0,
+          0.0,
+          robotStateSubsystem.getAllianceColor() == Alliance.Blue
+              ? AutonConstants.kDeadeyeHuntOmegaRadps
+              : -AutonConstants.kDeadeyeHuntOmegaRadps,
+          false);
     }
   }
 
@@ -70,7 +77,13 @@ public void execute() {
             huntState = HuntState.DRIVING;
           }
         } else {
-          driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, false);
+          driveSubsystem.move(
+              0.0,
+              0.0,
+              robotStateSubsystem.getAllianceColor() == Alliance.Blue
+                  ? AutonConstants.kDeadeyeHuntOmegaRadps
+                  : -AutonConstants.kDeadeyeHuntOmegaRadps,
+              false);
           seenNote = 0;
         }
         break;

diff --git a/src/main/java/frc/robot/commands/auton/DoNothingCommand.java b/src/main/java/frc/robot/commands/auton/DoNothingCommand.java
@@ -1,9 +1,12 @@
 package frc.robot.commands.auton;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.commands.drive.ResetGyroCommand;
+import frc.robot.commands.drive.ResetOdomCommand;
+import frc.robot.commands.drive.setAngleOffsetCommand;
 import frc.robot.commands.elbow.ZeroElbowCommand;
 import frc.robot.commands.robotState.SubWooferCommand;
 import frc.robot.subsystems.auto.AutoCommandInterface;
@@ -28,7 +31,8 @@ public DoNothingCommand(
       DriveSubsystem driveSubsystem,
       SuperStructure superStructure,
       MagazineSubsystem magazineSubsystem,
-      ElbowSubsystem elbowSubsystem) {
+      ElbowSubsystem elbowSubsystem,
+      Pose2d start) {
     this.robotStateSubsystem = robotStateSubsystem;
     this.driveSubsystem = driveSubsystem;
     this.superStructure = superStructure;
@@ -39,6 +43,8 @@ public DoNothingCommand(
         new ParallelCommandGroup(
             new ZeroElbowCommand(elbowSubsystem), // zero elbow
             new ResetGyroCommand(driveSubsystem)), // reset gyro
+        new setAngleOffsetCommand(driveSubsystem, start.getRotation().getDegrees()),
+        new ResetOdomCommand(driveSubsystem, start),
         new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem)); // shoot
   }
 

diff --git a/src/main/java/frc/robot/commands/auton/MiddleNote3AndWingNotesCommand.java b/src/main/java/frc/robot/commands/auton/MiddleNote3AndWingNotesCommand.java
@@ -1,19 +1,22 @@
 package frc.robot.commands.auton;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.drive.DriveAutonCommand;
 import frc.robot.commands.drive.ResetGyroCommand;
+import frc.robot.commands.drive.TurnUntilAngleCommand;
 import frc.robot.commands.drive.setAngleOffsetCommand;
 import frc.robot.commands.elbow.ZeroElbowCommand;
 import frc.robot.commands.robotState.IgnoreNotesCommand;
 import frc.robot.commands.robotState.IntakeCommand;
-import frc.robot.commands.robotState.PrepShooterCommand;
 import frc.robot.commands.robotState.SubWooferCommand;
 import frc.robot.commands.robotState.VisionShootCommand;
+import frc.robot.commands.superStructure.SpinUpWheelsCommand;
 import frc.robot.constants.AutonConstants;
+import frc.robot.constants.SuperStructureConstants;
 import frc.robot.subsystems.auto.AutoCommandInterface;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.elbow.ElbowSubsystem;
@@ -99,24 +102,36 @@ public MiddleNote3AndWingNotesCommand(
                     robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem))),
         new ParallelCommandGroup(
             middleNote3MiddleShoot3,
-            new SequentialCommandGroup(
-                new AutoWaitNoteStagedCommand(robotStateSubsystem),
-                new PrepShooterCommand(
-                    superStructure, robotStateSubsystem, AutonConstants.Setpoints.MS3))),
+            new AutoWaitNoteStagedCommand(robotStateSubsystem),
+            new SpinUpWheelsCommand(
+                superStructure,
+                SuperStructureConstants.kShooterSubwooferSetPoint,
+                SuperStructureConstants.kShooterSubwooferSetPoint)),
         new WaitCommand(0.02),
         new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
         middleShoot3WingNote3,
         new ParallelCommandGroup(
             wingNote3MidInit,
-            new SequentialCommandGroup(
-                new AutoWaitNoteStagedCommand(robotStateSubsystem),
-                new PrepShooterCommand(
-                    superStructure, robotStateSubsystem, AutonConstants.Setpoints.MI1))),
+            new AutoWaitNoteStagedCommand(robotStateSubsystem),
+            new SpinUpWheelsCommand(
+                superStructure,
+                SuperStructureConstants.kShooterSubwooferSetPoint,
+                SuperStructureConstants.kShooterSubwooferSetPoint)),
         new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
         midInitWingNote1,
         new AutoWaitNoteStagedCommand(robotStateSubsystem),
         new VisionShootCommand(
             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem),
+        new TurnUntilAngleCommand(
+            driveSubsystem,
+            robotStateSubsystem,
+            Rotation2d.fromDegrees(
+                robotStateSubsystem.getAllianceColor() == Alliance.Blue
+                    ? AutonConstants.kDeadeyeHuntStartYawDegs
+                    : 180 - AutonConstants.kDeadeyeHuntStartYawDegs),
+            robotStateSubsystem.getAllianceColor() == Alliance.Blue
+                ? AutonConstants.kDeadeyeHuntOmegaRadps
+                : -AutonConstants.kDeadeyeHuntOmegaRadps),
         new DeadeyeHuntCommand(deadeye, driveSubsystem, robotStateSubsystem, ledSubsystem),
         new AutoWaitNoteStagedCommand(robotStateSubsystem),
         new VisionShootCommand(

diff --git a/src/main/java/frc/robot/commands/drive/ResetOdomCommand.java b/src/main/java/frc/robot/commands/drive/ResetOdomCommand.java
@@ -0,0 +1,21 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class ResetOdomCommand extends InstantCommand {
+  private DriveSubsystem driveSubsystem;
+  private Pose2d pose;
+
+  public ResetOdomCommand(DriveSubsystem driveSubsystem, Pose2d pose) {
+    this.driveSubsystem = driveSubsystem;
+    this.pose = pose;
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.resetOdometry(pose);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/drive/TurnUntilAngleCommand.java b/src/main/java/frc/robot/commands/drive/TurnUntilAngleCommand.java
@@ -0,0 +1,54 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import net.jafama.FastMath;
+
+public class TurnUntilAngleCommand extends Command {
+  private DriveSubsystem driveSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+  private double target;
+  private double vOmega;
+
+  public TurnUntilAngleCommand(
+      DriveSubsystem driveSubsystem,
+      RobotStateSubsystem robotStateSubsystem,
+      Rotation2d target,
+      double vOmega) {
+    this.driveSubsystem = driveSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.target = target.getDegrees();
+    this.vOmega = vOmega;
+
+    if (robotStateSubsystem.getAllianceColor() == Alliance.Red) {
+      this.target = Units.radiansToDegrees(FastMath.normalizeZeroTwoPi(target.getRadians()));
+    }
+    addRequirements(driveSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.move(0, 0, vOmega, false);
+  }
+
+  @Override
+  public void execute() {
+    driveSubsystem.move(0, 0, vOmega, false);
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (robotStateSubsystem.getAllianceColor() == Alliance.Blue) {
+      return driveSubsystem.getGyroRotation2d().getDegrees() <= target;
+    } else {
+      return driveSubsystem.getGyroRotation2d().getDegrees() >= target;
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {}
+}
diff --git a/src/main/java/frc/robot/constants/AutonConstants.java b/src/main/java/frc/robot/constants/AutonConstants.java
@@ -16,12 +16,13 @@ public final class AutonConstants {
   public static final double kMaxAutonVelDeadeyeDrive = 2.0;
   public static final double kMaxAccelDeadeyeDrive = 3.5;
   public static final double kXSpeed = 2.0;
-  public static final double kMaxXOff = 1.0;
+  public static final double kMaxXOff = 1.93; // 1.0
   public static final double kSwitchXLine = 6.5;
   public static final double kWingSwitchXLine = 1.4;
   public static final double kPercentLeft = 0.65;
   public static final double kWingPercentLeft = 0.5;
   public static final double kDeadeyeHuntOmegaRadps = -1.5;
+  public static final double kDeadeyeHuntStartYawDegs = -10;
   public static final double kFoundNoteLoopCounts = 3;
 
   public static final int kSwitchStableCounts = 100;

diff --git a/src/main/java/frc/robot/constants/DriveConstants.java b/src/main/java/frc/robot/constants/DriveConstants.java
@@ -169,11 +169,14 @@ public static CurrentLimitsConfigs getSafeDriveLimits() {
 
   public static CurrentLimitsConfigs getNormDriveLimits() {
     CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
-    currentConfig.SupplyCurrentLimit = 45; // 40
-    currentConfig.SupplyCurrentThreshold = 50; // 45
+    currentConfig.SupplyCurrentLimit = 30; // 45
+    currentConfig.SupplyCurrentThreshold = 35; // 50
     currentConfig.SupplyTimeThreshold = 0.0;
+
+    currentConfig.StatorCurrentLimit = 140;
+
     currentConfig.SupplyCurrentLimitEnable = true;
-    currentConfig.StatorCurrentLimitEnable = false;
+    currentConfig.StatorCurrentLimitEnable = true;
     return currentConfig;
   }
 

diff --git a/src/main/java/frc/robot/constants/ElbowConstants.java b/src/main/java/frc/robot/constants/ElbowConstants.java
@@ -88,7 +88,7 @@ public static TalonFXConfiguration getFxConfiguration() {
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     Slot0Configs slot0 = new Slot0Configs();
-    slot0.kP = 150.0; // 1.2
+    slot0.kP = 150.0; // 150
     slot0.kI = 0.0;
     slot0.kD = 0.0;
     slot0.kS = 0.0;
@@ -99,7 +99,7 @@ public static TalonFXConfiguration getFxConfiguration() {
     config.Slot0 = slot0;
 
     Slot1Configs slot1 = new Slot1Configs();
-    slot1.kP = 150.0; // 1.2
+    slot1.kP = 150.0; // 150
     slot1.kI = 175.0;
     slot1.kD = 0.0;
     slot1.kS = 0.0;

diff --git a/src/main/java/frc/robot/constants/RobotStateConstants.java b/src/main/java/frc/robot/constants/RobotStateConstants.java
@@ -8,7 +8,7 @@ public final class RobotStateConstants {
   public static final double kLookupMaxDistance = 5.4; // 540cm
   public static final double kDistanceIncrement = 0.01; // 1cm
   public static final double kDistanceOffset = 0.00;
-  public static final double kElbowShootOffset = 0.0000; // .5 .7
+  public static final double kElbowShootOffset = -0.0027; // .5 .7
   public static final String kElbowPreferencesKey = "Elbow/Offset";
   public static final double kLeftFeedLinearCoeff = 2.5;
   public static final double kRightFeedLinearCoeff = 0;

diff --git a/src/main/java/frc/robot/constants/SuperStructureConstants.java b/src/main/java/frc/robot/constants/SuperStructureConstants.java
@@ -56,12 +56,12 @@ public final class SuperStructureConstants {
   // SUBWOOFER
   public static final double kWristSubwooferSetPoint = kWristIntakeSetPoint;
   public static final double kElbowSubwooferSetPoint = kElbowIntakeSetPoint;
-  public static final double kShooterSubwooferSetPoint = 50; // 60
+  public static final double kShooterSubwooferSetPoint = 55; // 60
 
   // FEEDING
   public static final double kWristFeedingSetPoint = kWristIntakeSetPoint;
-  public static final double kElbowFeedingSetPoint = kElbowIntakeSetPoint;
-  public static final double kLeftShooterFeedingSetPoint = 60;
+  public static final double kElbowFeedingSetPoint = kElbowSubwooferSetPoint;
+  public static final double kLeftShooterFeedingSetPoint = 50; // 60
   public static final double kRightShooterFeedingSetPoint = 40;
 
   // EJECTING

diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java
@@ -20,6 +20,10 @@ public final class VisionConstants {
   public static final int kMaxTimesOffWheels = 5;
   public static final double kBumperPixelLine = 87; // 100
 
+  //   public static final double kThetaStdDevUsed = Units.degreesToRadians(0.02);
+  //   public static final double kThetaStdDevRejected = Units.degreesToRadians(360);
+  //   public static final double kThetaStdThres = 0.2;
+
   // Velocity Filter
   public static final double kLinearCoeffOnVelFilter = 0.1;
   public static final double kOffsetOnVelFilter = 0.10;