Merge branch 'auton' of github.com:strykeforce/reefscape into auton

Author: mwitcpalek

.vscode/settings.json

@@ -57,5 +57,5 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
 }

src/main/java/frc/robot/commands/drive/DriveAutonCommand.java

@@ -53,32 +53,35 @@ public void reassignAlliance() {
 
   @Override
   public void initialize() {
-    Pose2d initialPose = new Pose2d();
-    if (trajectory.getInitialPose(mirrorTrajectory) != null) {
+    if (isTherePath) {
+      driveSubsystem.setAutoDebugMsg("Initialize " + trajectoryName);
+      Pose2d initialPose = new Pose2d();
       initialPose = trajectory.getInitialPose(mirrorTrajectory).get();
+      if (resetOdometry) {
+        driveSubsystem.resetOdometry(initialPose);
+      }
+      driveSubsystem.setEnableHolo(true);
+      // driveSubsystem.recordAutoTrajectory(trajectory);
+      driveSubsystem.resetHolonomicController();
+      driveSubsystem.grapherTrajectoryActive(true);
+      timer.reset();
+      logger.info("Begin Trajectory: {}", trajectoryName);
+      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      driveSubsystem.calculateController(desiredState);
     }
-    if (resetOdometry && trajectory.getInitialPose(mirrorTrajectory) != null) {
-      driveSubsystem.resetOdometry(initialPose);
-    }
-    driveSubsystem.setEnableHolo(true);
-    // driveSubsystem.recordAutoTrajectory(trajectory);
-    driveSubsystem.resetHolonomicController();
-    driveSubsystem.grapherTrajectoryActive(true);
-    timer.reset();
-    logger.info("Begin Trajectory: {}", trajectoryName);
-    SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
-    driveSubsystem.calculateController(desiredState);
   }
 
   @Override
   public void execute() {
-    SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
-    driveSubsystem.calculateController(desiredState);
+    if (isTherePath) {
+      SwerveSample desiredState = trajectory.sampleAt(timer.get(), mirrorTrajectory).get();
+      driveSubsystem.calculateController(desiredState);
+    }
   }
 
   @Override
   public boolean isFinished() {
-    if (isTherePath) {
+    if (!isTherePath) {
       return false;
     }
     return timer.hasElapsed(trajectory.getTotalTime());
@@ -98,5 +101,6 @@ public void end(boolean interrupted) {
 
     driveSubsystem.grapherTrajectoryActive(false);
     logger.info("End Trajectory {}: {}", trajectoryName, timer.get());
+    driveSubsystem.setAutoDebugMsg("End " + trajectoryName);
   }
 }

src/main/java/frc/robot/constants/DriveConstants.java

@@ -18,6 +18,8 @@ public class DriveConstants {
   public static final double kMaxSpeedMetersPerSecond = 12.0;
   public static final double kSpeedStillThreshold = 0.1; // meters per second
   public static final double kGyroRateStillThreshold = 10.0; // 25  5 degrees per second
+  public static final double kGyroDifferentThreshold = 5.0; // 5 degrees
+  public static final int kGyroDifferentCount = 3;
 
   public static final double kRobotLength = 22.0;
   public static final double kRobotWidth = 22.0;

src/main/java/frc/robot/constants/RobotConstants.java

@@ -2,5 +2,4 @@
 
 public class RobotConstants {
   public static final int kTalonConfigTimeout = 10; // ms
-  public static final int kBreakerTempChannel = 0;
 }

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -1,3 +1,5 @@
 package frc.robot.constants;
 
-public class RobotStateConstants {}
+public class RobotStateConstants {
+    public static final double[] kNodeAngles = {0.0, 60.0, 120.0, 180.0, -120.0, -60.0};
+}

src/main/java/frc/robot/constants/TagServoingConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class TagServoingConstants {
+    public static final double kAngleCloseEnough = 3.0;
+}

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -10,29 +10,20 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.util.CircularBuffer;
-import edu.wpi.first.wpilibj.AnalogInput;
 import frc.robot.constants.DriveConstants;
-import frc.robot.constants.RobotConstants;
-import frc.robot.subsystems.robotState.RobotStateSubsystem;
 import java.util.Set;
 import java.util.function.BooleanSupplier;
 import net.jafama.FastMath;
-import org.slf4j.Logger;
+import org.littletonrobotics.junction.Logger;
 import org.slf4j.LoggerFactory;
-import org.strykeforce.swerve.SwerveModule;
 import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
 import org.strykeforce.telemetry.measurable.Measure;
 
 public class DriveSubsystem extends MeasurableSubsystem {
-  private static final Logger logger = LoggerFactory.getLogger(DriveSubsystem.class);
+  private static final org.slf4j.Logger logger = LoggerFactory.getLogger(DriveSubsystem.class);
   private final SwerveIO io;
   private SwerveIOInputsAutoLogged inputs = new SwerveIOInputsAutoLogged();
   private final HolonomicDriveController holonomicController;
-  private RobotStateSubsystem robotStateSubsystem;
-  private double avgTemp = 0.0;
-  private CircularBuffer<Integer> temps = new CircularBuffer<Integer>(DriveConstants.kTempAvgCount);
-  private AnalogInput breakerTemp;
 
   private final ProfiledPIDController omegaController;
   private final PIDController xController;
@@ -46,13 +37,13 @@ public class DriveSubsystem extends MeasurableSubsystem {
   private Trajectory<SwerveSample> autoTrajectory;
   private double trajectoryActive = 0.0;
 
+  private int gyroDifferentCount = 0;
+
   public DriveSubsystem(SwerveIO io) {
     org.littletonrobotics.junction.Logger.recordOutput("Swerve/YVelSpeed", 0.0);
     org.littletonrobotics.junction.Logger.recordOutput("Swerve/UsingDeadEye", false);
     org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", "Nothing");
 
-    this.breakerTemp = new AnalogInput(RobotConstants.kBreakerTempChannel);
-
     this.io = io;
     // Setup Holonomic Controller
     omegaController =
@@ -124,6 +115,28 @@ public void calculateController(SwerveSample desiredState) {
         false);
   }
 
+  // Choreo Holonomic Controller
+  public void calculateControllerServo(SwerveSample desiredState, double desiredRobotRelvY) {
+    holoContInput = desiredState;
+    double xFF = desiredState.vx;
+    double rotationFF = desiredState.heading;
+
+    Pose2d pose = inputs.poseMeters;
+    double xFeedback = xController.calculate(pose.getX(), desiredState.x);
+    double rotationFeedback =
+        omegaController.calculate(pose.getRotation().getRadians(), desiredState.heading);
+
+    holoContOutput.vxMetersPerSecond = xFF + xFeedback;
+    holoContOutput.vyMetersPerSecond = desiredRobotRelvY;
+    holoContOutput.omegaRadiansPerSecond = rotationFF + rotationFeedback;
+
+    io.move(
+        holoContOutput.vxMetersPerSecond,
+        holoContOutput.vyMetersPerSecond,
+        holoContOutput.omegaRadiansPerSecond,
+        false);
+  }
+
   public void resetOdometry(Pose2d pose) {
     io.resetOdometry(pose);
     logger.info("reset odometry with: {}", pose);
@@ -133,6 +146,18 @@ public void recordAutoTrajectory(Trajectory<SwerveSample> traj) {
     autoTrajectory = traj;
   }
 
+  public void setAutoDebugMsg(String msg) {
+    org.littletonrobotics.junction.Logger.recordOutput("Swerve/Auto Drive Info", msg);
+  }
+
+  public Trajectory<SwerveSample> getAutoTrajectory() {
+    if (autoTrajectory != null) {
+      return autoTrajectory;
+    } else {
+      return null;
+    }
+  }
+
   public void resetHolonomicController(double yaw) {
     xController.reset();
     yController.reset();
@@ -163,11 +188,7 @@ public Rotation2d getGyroRotation2d() {
   }
 
   public ChassisSpeeds getFieldRelSpeed() {
-    return io.getFieldRelSpeed();
-  }
-
-  public ChassisSpeeds getRobotRelSpeed() {
-    return io.getRobotRelSpeed();
+    return inputs.fieldRelSpeed;
   }
 
   public void setDriveState(DriveStates state) {
@@ -182,7 +203,7 @@ public boolean isDriveStill() {
     // logger.info(
     //     "Timestamp Before FieldRel: {}",
     //     org.littletonrobotics.junction.Logger.getRealTimestamp() / 1000);
-    ChassisSpeeds cs = getFieldRelSpeed();
+    ChassisSpeeds cs = inputs.fieldRelSpeed;
     double vX = cs.vxMetersPerSecond;
     double vY = cs.vyMetersPerSecond;
     // logger.info(
@@ -192,32 +213,30 @@ public boolean isDriveStill() {
     // Take fieldRel Speed and get the magnitude of the vector
     double wheelSpeed = FastMath.hypot(vX, vY);
 
-    double gyroRate = inputs.gyroRate;
-
     boolean velStill = Math.abs(wheelSpeed) <= DriveConstants.kSpeedStillThreshold;
-    boolean gyroStill = Math.abs(gyroRate) <= DriveConstants.kGyroRateStillThreshold;
+    boolean gyroStill = isGyroStill();
 
     return velStill && gyroStill;
   }
 
+  public boolean isGyroStill() {
+    return Math.abs(inputs.gyroRate) <= DriveConstants.kGyroRateStillThreshold;
+  }
+
   public void setGyroOffset(Rotation2d rotation) {
-    io.setGyroOffset(apply(rotation));
+    io.setBothGyroOffset(apply(rotation));
   }
 
   public void setEnableHolo(boolean enabled) {
     holonomicController.setEnabled(enabled);
     logger.info("Holonomic Controller Enabled: {}", enabled);
   }
 
-  public void setRobotStateSubsystem(RobotStateSubsystem robotStateSubsystem) {
-    this.robotStateSubsystem = robotStateSubsystem;
-  }
-
   public void teleResetGyro() {
     logger.info("Driver Joystick: Reset Gyro");
     // double gyroResetDegs = robotStateSubsystem.getAllianceColor() == Alliance.Blue ? 0.0 : 180.0;
     double gyroResetDegs = 0.0; // TODO change this to the above once we have RobotStateSubsystem
-    io.setGyroOffset(Rotation2d.fromDegrees(gyroResetDegs));
+    io.setBothGyroOffset(Rotation2d.fromDegrees(gyroResetDegs));
     io.resetGyro();
     io.resetOdometry(
         new Pose2d(inputs.poseMeters.getTranslation(), Rotation2d.fromDegrees(gyroResetDegs)));
@@ -271,10 +290,8 @@ public double apply(double x) {
 
   // Control Methods
   public void lockZero() {
-    SwerveModule[] swerveModules = io.getSwerveModules();
-    for (int i = 0; i < 4; i++) {
-      swerveModules[i].setAzimuthRotation2d(Rotation2d.fromDegrees(0.0));
-    }
+    Rotation2d rot = Rotation2d.fromDegrees(0.0);
+    io.setSwerveModuleAngles(rot, rot, rot, rot);
   }
 
   public void toSafeHold() {
@@ -287,10 +304,6 @@ public void toIdle() {
     io.configDriveCurrents(DriveConstants.getNormDriveLimits());
   }
 
-  public double getTemp() {
-    return avgTemp;
-  }
-
   public PIDController getxController() {
     return xController;
   }
@@ -310,28 +323,22 @@ public PIDController getomegaControllerNonProfiled() {
 
   public void periodic() {
     io.updateInputs(inputs);
-    int temp = breakerTemp.getValue();
-    temps.addFirst(temp);
-    double avg = 0;
-    if (temps.size() == DriveConstants.kTempAvgCount)
-      for (int i = 0; i < DriveConstants.kTempAvgCount; ++i) avg += temps.get(i);
-
-    avg /= DriveConstants.kTempAvgCount;
-
-    avgTemp = avg;
+    Logger.processInputs(getName(), inputs);
+    if (Math.abs(inputs.gyroRotation2d.getDegrees() - inputs.navxRotation2d.getDegrees())
+        > DriveConstants.kGyroDifferentThreshold) {
+      gyroDifferentCount++;
+    } else {
+      gyroDifferentCount = 0;
+    }
+    if (gyroDifferentCount > DriveConstants.kGyroDifferentCount && isDriveStill()) {
+      io.setPigeonGyroOffset(inputs.navxRotation2d);
+      gyroDifferentCount = 0;
+    }
 
     switch (currDriveState) {
       case IDLE:
-        if (avg > DriveConstants.kTripTemp) {
-          io.configDriveCurrents(DriveConstants.getSafeDriveLimits());
-          setDriveState(DriveStates.SAFE);
-        }
         break;
       case SAFE:
-        if (avg < DriveConstants.kRecoverTemp) {
-          io.configDriveCurrents(DriveConstants.getNormDriveLimits());
-          setDriveState(DriveStates.IDLE);
-        }
         break;
       case SAFE_HOLD:
         break;
@@ -372,14 +379,6 @@ public Set<Measure> getMeasures() {
         new Measure(
             "Holo Controller Omega Setpoint",
             () -> holonomicController.getThetaController().getSetpoint().position),
-        new Measure("Trajectory Active", () -> trajectoryActive),
-        new Measure("Wheel 0 Angle", () -> io.getSwerveModuleStates()[0].angle.getDegrees()),
-        new Measure("Wheel 0 Speed", () -> io.getSwerveModuleStates()[0].speedMetersPerSecond),
-        new Measure("Wheel 1 Angle", () -> io.getSwerveModuleStates()[1].angle.getDegrees()),
-        new Measure("Wheel 1 Speed", () -> io.getSwerveModuleStates()[1].speedMetersPerSecond),
-        new Measure("Wheel 2 Angle", () -> io.getSwerveModuleStates()[2].angle.getDegrees()),
-        new Measure("Wheel 2 Speed", () -> io.getSwerveModuleStates()[2].speedMetersPerSecond),
-        new Measure("Wheel 3 Angle", () -> io.getSwerveModuleStates()[3].angle.getDegrees()),
-        new Measure("Wheel 3 Speed", () -> io.getSwerveModuleStates()[3].speedMetersPerSecond));
+        new Measure("Trajectory Active", () -> trajectoryActive));
   }
 }