Pre-playoffs-2

Author: dabeycorn

src/main/java/wmironpatriots/Robot.java

@@ -7,7 +7,6 @@
 package wmironpatriots;
 
 import com.ctre.phoenix6.SignalLogger;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.Alert;
@@ -28,6 +27,7 @@
 import java.util.Optional;
 import monologue.Logged;
 import wmironpatriots.commands.Autonomous;
+import wmironpatriots.commands.DriveToPose;
 import wmironpatriots.subsystems.climb.Climb;
 import wmironpatriots.subsystems.climb.ClimbIOComp;
 import wmironpatriots.subsystems.superstructure.Superstructure;
@@ -61,6 +61,8 @@ public class Robot extends TimedRobot implements Logged {
 
   Command auton;
 
+  private Command goTo;
+
   public Robot() {
     // * SYSTEMS INIT
     // Shuts up driverstation
@@ -116,6 +118,8 @@ public Robot() {
         new Superstructure(
             swerve, new ElevatorIOComp(), new TailIOComp(), new RollerIOComp(), new ChuteIOComp());
 
+    goTo = new DriveToPose(swerve, swerve::getAlignPose);
+
     climb.setDefaultCommand(climb.runClimb(0));
 
     autons = Autonomous.configureAutons(swerve, superstructure);
@@ -126,7 +130,8 @@ public Robot() {
             () -> -JoystickUtil.applyTeleopModifier(driver::getLeftY),
             () -> -JoystickUtil.applyTeleopModifier(driver::getLeftX),
             () -> -JoystickUtil.applyTeleopModifier(driver::getRightX),
-            () -> MathUtil.clamp(1.5 - driver.getRightTriggerAxis(), 0.0, 1.0)));
+            () -> 1.0));
+    // () -> MathUtil.clamp(1.5 - driver.getRightTriggerAxis(), 0.0, 1.0)));
     driver
         .a()
         .whileTrue(
@@ -135,6 +140,9 @@ public Robot() {
                     swerve.resetOdo(
                         new Pose2d(swerve.getPose().getTranslation(), new Rotation2d()))));
     driver.rightBumper().whileTrue(superstructure.score());
+    driver.x().whileTrue(goTo);
+    driver.rightTrigger(0.3).onTrue(setbah(0.0));
+    driver.leftTrigger(0.3).onTrue(setbah(1.0));
 
     // driver.y().whileTrue(swerve.driveToPoseCmmd(() -> Swerve.AlignTargets.A));
     operator.a().whileTrue(superstructure.scoreCoralCmmd(ReefLevel.L1));

src/main/java/wmironpatriots/commands/DriveToPose.java

@@ -45,9 +45,9 @@ public class DriveToPose extends Command {
   // }
 
   public static final double LINEAR_P = 0.8;
-  public static final double LINEAR_D = 0.8;
-  public static final double ANGULAR_P = 0.8;
-  public static final double ANGULAR_D = 0.8;
+  public static final double LINEAR_D = 0.0;
+  public static final double ANGULAR_P = 0.3;
+  public static final double ANGULAR_D = 0.0;
 
   public static final double MAX_VEL = 3.8;
   public static final double MAX_ACCEL = 3.0;

src/main/java/wmironpatriots/subsystems/superstructure/tail/Tail.java

@@ -23,7 +23,7 @@ public abstract class Tail implements Logged, Subsystem {
   public static final double POSE_MAX = 10;
   public static final double POSE_MIN = 8.2;
   public static final double POSE_SAFTEY =
-      6.8; // The position where the tail is safe from the top of 1st stage
+      7.2; // The position where the tail is safe from the top of 1st stage
 
   public static final double POSE_L1 = 10;
   public static final double POSE_L2 = (20 * 5 / 36); // 5;

src/main/java/wmironpatriots/subsystems/superstructure/tail/roller/Roller.java

@@ -13,8 +13,8 @@
 
 public abstract class Roller implements Logged, Subsystem {
   // * CONSTANTS
-  public static final double SPEED_INTAKING = 2;
-  public static final double SPEED_OUTAKING = -2;
+  public static final double SPEED_INTAKING = 1.25;
+  public static final double SPEED_OUTAKING = -1.25;
   public static final double SPEED_SCORING = 3.5;
 
   // * LOGGED VALUES

src/main/java/wmironpatriots/subsystems/swerve/Swerve.java

@@ -231,6 +231,10 @@ public void periodic() {
     }
   }
 
+  public Pose2d getAlignPose() {
+    return alignTarget.pose;
+  }
+
   @Override
   public void simulationPeriodic() {
     simulation.get().periodic();
@@ -426,22 +430,21 @@ public static Pose2d calculateReefPose(int a, int b) {
     if (b != -1 && b != 1) {
       b = 1;
     }
-    double angle = 0.137 - 1 * 0.0175;
+    double angle;
+
+    if (b == 1) {
+      angle = 0.137 + 2 * 0.0175;
+    } else {
+      angle = 0.137 - 1 * 0.0175;
+    }
+
     // - 4 * 0.0175;
     double x = ((50.49 * Math.cos(((a * Math.PI) / 3) + (angle * b))) + 177.25) / 39.37;
     double y = ((50.49 * Math.sin(((a * Math.PI) / 3) + (angle * b))) + 158.50) / 39.37;
     double theta = (a * Math.PI) / 3;
 
     x = (Units.inchesToMeters(689.751) - x);
     y = (Units.inchesToMeters(317.5) - y);
-    // x =
-    //     true
-    //         ? (Units.inchesToMeters(689.751) - x)
-    //         : x;
-    // y =
-    //     true
-    //         ? (Units.inchesToMeters(317.5) - y)
-    //         : y;
     theta =
         DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red ? theta + Math.PI : theta;
 

src/main/java/wmironpatriots/subsystems/swerve/SwerveConstants.java

@@ -52,7 +52,7 @@ public class SwerveConstants {
       Rotation2d.fromRotations(
           DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue ? 0 : 0.5);
 
-  public static final double MAX_LINEAR_SPEED = Units.feetToMeters(16.5);
+  public static final double MAX_LINEAR_SPEED = Units.feetToMeters(17.25);
   public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / RADIUS_METERS;
 
   public static final double LINEAR_P = 3.0; // 4.5;