yayy finished kousha happy yayy

diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index 28775d4ac7c411cad7d1d2cc479ad04827bb93b3..90f0c5ea9da1da7b3eca3b4327c3503e663ee59f 100644 (file)
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -2,6 +2,7 @@ package frc.robot.commands.drive_comm;
 
 import org.littletonrobotics.junction.Logger;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -26,6 +27,7 @@ import lib.controllers.PS5Controller.PS5Axis;
 public class DefaultDriveCommand extends Command {
     protected final Drivetrain swerve;
     protected final BaseDriverConfig driver;
+    private PIDController trenchAssistPid = new PIDController(9, 0.0, 3);
 
     public DefaultDriveCommand(
             Drivetrain swerve,
@@ -39,6 +41,10 @@ public class DefaultDriveCommand extends Command {
     @Override
     public void initialize() {
         swerve.setStateDeadband(true);
+
+        trenchAssistPid.setIZone(2);
+        trenchAssistPid.setIntegratorRange(-1, 1);
+
     }
 
     private boolean trenchAlign = false;
@@ -90,13 +96,11 @@ public class DefaultDriveCommand extends Command {
 
         Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
         if (swerve.getTrenchAssist()) {
-            drive(TrenchAssist2.calculate(swerve, corrected));
+            drive(TrenchAssist2.calculate(swerve, corrected, trenchAssistPid));
         } else {
+            trenchAssistPid.reset();
             drive(corrected);
         }
-
-        Logger.recordOutput("isAlign", swerve.getIsAlign());
-        Logger.recordOutput("alignAngle", swerve.getAlignAngle());
     }
 
     /**

diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index 0e035cc3b8e222bd06dbffed6c99d11914bbb571..97e9b950ac0711568638374e81b93a8b51629fd8 100644 (file)
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -4,6 +4,7 @@ import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.PS5Controller.Button;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.FunctionalCommand;
@@ -14,6 +15,7 @@ import frc.robot.subsystems.drivetrain.Drivetrain;
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.turret.Turret;
 import lib.controllers.PS5Controller;
+import lib.controllers.PS5Controller.DPad;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
 
@@ -60,8 +62,11 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         controller.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAlign(true)))
             .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAlign(false)));
 
-        controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> getDrivetrain().setTrenchAssist(true)))
-            .onFalse(new InstantCommand(() -> getDrivetrain().setTrenchAssist(false)));
+        controller.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);}))
+            .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);}));
+
+        controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(true); getDrivetrain().setTrenchAlign(true);}))
+            .onFalse(new InstantCommand(() -> {getDrivetrain().setTrenchAssist(false); getDrivetrain().setTrenchAlign(false);}));
 
 
 

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
index 0a2f1425deff376d7e1e90688cc8af7b4b97f26a..18972a5c43298d83e5dfb81d9edc7c0c1592de77 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssist2.java
@@ -1,70 +1,68 @@
 package frc.robot.util.TrenchAssist;
 
-import static edu.wpi.first.units.Units.Rotation;
-
-import java.util.Optional;
-
 import org.littletonrobotics.junction.Logger;
-import org.opencv.core.Point;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rectangle2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Robot;
-import frc.robot.constants.swerve.DriveConstants;
-import frc.robot.controls.BaseDriverConfig;
 import frc.robot.subsystems.drivetrain.Drivetrain;
-import frc.robot.util.Vision.DriverAssist;
-import frc.robot.util.TrenchAssist.TrenchAssistConstants;
 
 public class TrenchAssist2 {
 
-    private static final double SCALE = 1.0;
+    // private static final double SCALE = 1.0;
 
-    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds) {
-        //ChassisSpeeds speedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds, drive.getYaw().unaryMinus()); //this singular unary minus is best we can get
+    public static ChassisSpeeds calculate(Drivetrain drive, ChassisSpeeds chassisSpeeds, PIDController pid) {
+        // ChassisSpeeds speedsFieldRelative =
+        // ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds,
+        // drive.getYaw().unaryMinus());
 
         double distanceFromSlideLatitude;
 
-        if (drive.getPose().getY() > (8.07 / 2.0)){
+        if (drive.getPose().getY() > (8.07 / 2.0)) {
             distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[0]);
         } else {
             distanceFromSlideLatitude = (drive.getPose().getY() - TrenchAssistConstants.SLIDE_LATITUDES[1]);
         }
 
-        var impulse = -distanceFromSlideLatitude * SCALE;
+        Logger.recordOutput("slides",
+                new Pose2d[] { new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[0], Rotation2d.kZero),
+                        new Pose2d(2.0, TrenchAssistConstants.SLIDE_LATITUDES[1], Rotation2d.kZero) });
+
+        double impulse = pid.calculate(distanceFromSlideLatitude, 0);
+
+        if (distanceFromSlideLatitude < Units.inchesToMeters(3)){
+            impulse = 0.0;
+        }
+
 
-        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse, chassisSpeeds.omegaRadiansPerSecond);
+        ChassisSpeeds horizontalSpeeds = new ChassisSpeeds(chassisSpeeds.vxMetersPerSecond, impulse,
+                chassisSpeeds.omegaRadiansPerSecond);
 
-        var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond, chassisSpeeds.omegaRadiansPerSecond);
+        var x = new ChassisSpeeds(horizontalSpeeds.vxMetersPerSecond, horizontalSpeeds.vyMetersPerSecond,
+                chassisSpeeds.omegaRadiansPerSecond);
         var y = new Translation2d(x.vxMetersPerSecond, x.vyMetersPerSecond).rotateBy(drive.getYaw());
 
-        var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond), drive.getYaw());
+        var z = ChassisSpeeds.fromFieldRelativeSpeeds(new ChassisSpeeds(y.getX(), y.getY(), x.omegaRadiansPerSecond),
+                drive.getYaw());
 
         return z;
 
     }
 
-    public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive){
+    public static ChassisSpeeds convertToChassisSpeedsRobotRelative(Translation2d translation, Drivetrain drive) {
         Rotation2d yaw = drive.getYaw();
         Translation2d robotRelative = translation.rotateBy(yaw);
         return new ChassisSpeeds(robotRelative.getX(), robotRelative.getY(), 0.0);
 
     }
 
-    public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive){
+    public static Translation2d convertToTranslation2dFieldRelative(ChassisSpeeds speeds, Drivetrain drive) {
         Rotation2d yaw = drive.getYaw();
         Translation2d robotTranslation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
-        return robotTranslation.rotateBy(yaw.times(-1));        
+        return robotTranslation.rotateBy(yaw.times(-1));
 
     }
 }
\ No newline at end of file

diff --git a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
index 1c9c5cacec5f7be4965c586d287e877545e6446b..48b926510937aca07c06fe22b7cbedef919142d0 100644 (file)
--- a/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
+++ b/src/main/java/frc/robot/util/TrenchAssist/TrenchAssistConstants.java
@@ -21,10 +21,10 @@ public class TrenchAssistConstants {
     };
 
     public static final double[] SLIDE_LATITUDES = new double[]{
-        // 8.07 - Units.inchesToMeters(30.0),
-        // Units.inchesToMeters(30.0), should be accurate, i think our field is slightly too small
-        6.550,
-        0.668,
+        8.07 - Units.inchesToMeters(30.0),
+        Units.inchesToMeters(30.0), // should be accurate, i think our field is slightly too small
+        // 6.550,
+        // 0.668,
 
     };