IDK

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.math.geometry.Translation2d;
 // Drive Systems
 import edu.wpi.first.wpilibj.DriverStation;
 
@@ -25,8 +26,10 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 // Autos
 import frc4388.utility.controller.VirtualController;
+import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.wait.waitSupplier;
 import frc4388.robot.constants.Constants.OIConstants;
 import frc4388.robot.subsystems.differential.DiffDrive;
@@ -89,8 +92,8 @@ public class RobotContainer {
         configureVirtualButtonBindings();
         
         DeferredBlock.addBlock(() -> { // Called on first robot enable
-            // m_robotSwerveDrive.resetGyro();
+            m_DiffDrive.resetOdometry();
-        }, false);
+        }, true);
         DeferredBlock.addBlock(() -> { // Called on every robot enable
             TimesNegativeOne.update();
         }, true);
@@ -214,7 +217,14 @@ public class RobotContainer {
     //     return autoCommand;
 	// } catch (Exception e) {
 	//     DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace());
-	    return autoCommand;
+	    return new SequentialCommandGroup(
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, -0.5), new Translation2d(), 2000),
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, 0.5), new Translation2d(), 1000),
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, -1), new Translation2d(), 500),
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(), new Translation2d(-1, 0), 250),
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(), new Translation2d(1, 0), 500),
+            new MoveForTimeCommand(m_DiffDrive, new Translation2d(0, 0.5), new Translation2d(-0.5, 0), 500)
+        );
 	// }
     // return new PathPlannerAuto("Line-up-no-arm");
 	// zach told me to do the below comment

src/main/java/frc4388/robot/RobotMap.java

@@ -12,9 +12,11 @@ import com.ctre.phoenix6.hardware.Pigeon2;
 
 import frc4388.robot.subsystems.differential.DiffConstants;
 import frc4388.robot.subsystems.differential.DiffIO;
-import frc4388.robot.subsystems.differential.DiffTalonSRX;
+import frc4388.robot.subsystems.differential.DiffReal;
+import frc4388.robot.subsystems.differential.DiffSim;
 import frc4388.robot.subsystems.differential.GyroIO;
 import frc4388.robot.subsystems.differential.GyroPigeon2;
+import frc4388.robot.subsystems.differential.GyroSim;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -35,7 +37,7 @@ public class RobotMap {
         TalonSRX m_leftRear = new TalonSRX(DiffConstants.IDs.REAR_LEFT_MOTOR.id);
         TalonSRX m_rightRear = new TalonSRX(DiffConstants.IDs.REAR_RIGHT_MOTOR.id);
 
-        map.m_DiffDrive = new DiffTalonSRX(
+        map.m_DiffDrive = new DiffReal(
             m_leftFront, m_rightFront,
             m_leftRear, m_rightRear
         );
@@ -45,5 +47,15 @@ public class RobotMap {
 
         return map;
     }
-   
+
+    public static RobotMap configureSim() {
+        RobotMap map = new RobotMap();
+
+        DiffSim sim = new DiffSim();
+        map.m_DiffDrive = sim;
+        map.m_gyro = new GyroSim(sim.getSimAngle());
+
+        return map;
+    }
+    
 }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -1,49 +1,47 @@
-// package frc4388.robot.commands;
+package frc4388.robot.commands;
 
-// import java.time.Instant;
+import java.time.Instant;
 
-// import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation2d;
-// import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Command;
-// import frc4388.robot.subsystems.SwerveDrive;
+import frc4388.robot.subsystems.differential.DiffDrive;
+import frc4388.utility.structs.Drivebase;
 
-// // Command to repeat a joystick movement for a specific time.
+// Command to repeat a joystick movement for a specific time.
-// public class MoveForTimeCommand extends Command {
+public class MoveForTimeCommand extends Command {
-//     private final SwerveDrive swerveDrive;
+    private final DiffDrive drivebase;
-//     private final Translation2d leftStick;
+    private final Translation2d leftStick;
-//     private final Translation2d rightStick;
+    private final Translation2d rightStick;
-//     private final long duration;
+    private final long duration;
-//     private final boolean robotRelative;
 
-//     private Instant startTime;
+    private Instant startTime;
 
-//     public MoveForTimeCommand(
+    public MoveForTimeCommand(
-//         SwerveDrive swerveDrive, 
+        DiffDrive drivebase, 
-//         Translation2d leftStick, 
+        Translation2d leftStick, 
-//         Translation2d rightStick, 
+        Translation2d rightStick, 
-//         long millis,
+        long millis) {
-//         boolean robotRelative) {
         
-//         addRequirements(swerveDrive);
+        addRequirements(drivebase);
 
-//         this.swerveDrive = swerveDrive;
+        this.drivebase = drivebase;
-//         this.leftStick = leftStick;
+        this.leftStick = leftStick;
-//         this.rightStick = rightStick;
+        this.rightStick = rightStick;
-//         this.duration = millis;
+        this.duration = millis;
-//         this.robotRelative = robotRelative;
+    }
-//     }
 
-//     @Override
+    @Override
-//     public void initialize() {
+    public void initialize() {
-//         startTime = Instant.now();
+        startTime = Instant.now();
-//     }
+    }
 
-//     @Override
+    @Override
-//     public void execute() {
+    public void execute() {
-//         swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+        drivebase.arcadeDrive(leftStick, rightStick);
-//     }
+    }
 
-//     @Override
+    @Override
-//     public boolean isFinished() {
+    public boolean isFinished() {
-//         return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
+        return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
-//     }
+    }
-// }
+}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "Vision-Minibot";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 5;
+  public static final int GIT_REVISION = 6;
-  public static final String GIT_SHA = "a9e45fcf9ba356f63af7c567d51115d255bb269b";
+  public static final String GIT_SHA = "0bb2a774d711afb09921bee415ca946821a45335";
-  public static final String GIT_DATE = "2024-12-16 00:39:59 MST";
+  public static final String GIT_DATE = "2025-07-16 15:09:08 MDT";
   public static final String GIT_BRANCH = "master";
-  public static final String BUILD_DATE = "2025-07-16 14:12:56 MDT";
+  public static final String BUILD_DATE = "2025-07-18 12:15:43 MDT";
-  public static final long BUILD_UNIX_TIME = 1752696776235L;
+  public static final long BUILD_UNIX_TIME = 1752862543497L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/subsystems/differential/DiffConstants.java

@@ -9,10 +9,13 @@ import frc4388.utility.structs.Gains;
 
 public class DiffConstants {
     
-    public static final Distance WHEEL_RADIUS_TO_ARC = Inches.of(2.5).times(Math.PI * 2); //meters
+    public static final Distance WHEEL_RADIUS = Inches.of(2.5);
+    public static final Distance WHEEL_RADIUS_TO_ARC = WHEEL_RADIUS.times(Math.PI * 2); //meters
     public static final Distance TRACK_DISPLACEMENT = Inches.of(6.5); //meters
     public static final Distance TRACK_WIDTH = TRACK_DISPLACEMENT.times(2);
 
+    public static final int ENCODER_TICKS_PER_ROTATION = 1100;
+
     public static final Gains ROT_GAINS = new Gains(20, 0, 0);
 
     public static final class IDs {

src/main/java/frc4388/robot/subsystems/differential/DiffDrive.java

@@ -11,10 +11,12 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.utility.status.Queryable;
 import frc4388.utility.status.Status;
 import frc4388.utility.structs.Drivebase;
+import pabeles.concurrency.IntOperatorTask.Max;
 
 public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
     DiffIO io;
@@ -39,8 +41,8 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
         Logger.processInputs("gyro", gyroState);
 
         var speeds = new DifferentialDriveWheelPositions(
-            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.leftOutputPosition),
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.leftOutputPosition),
-            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.rightOutputPosition)
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(-state.rightOutputPosition)
         );
 
         odometry.update(gyroState.yaw, speeds);
@@ -54,28 +56,30 @@ public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
         );
     }
 
-    private PID rotPid = new PID(DiffConstants.ROT_GAINS);
+    private PID rotPid = new PID(DiffConstants.ROT_GAINS); 
 
     @Override
     public void driveFieldRelative(Translation2d left, Translation2d right) {
+        double magnatude = right.getNorm();
         // In case the driver's stick is inside the deadband
-        if(right.getNorm() == 0) {
+        if(magnatude == 0) {
             io.driveWithInput(0, 0);
             return;
         };
 
-        double targetAngle = right.getAngle().getRotations();
+        double targetAngle = right.getAngle().getRotations() + 0.25;
         double curAngle = gyroState.yaw.getRotations();
 
-        double error = targetAngle - curAngle - signedFloor(curAngle);
+        double error = targetAngle - signedFloor(curAngle);
         if(error > 0.5) {
             error -= 1;
         }else if(error < -0.5) {
             error += 1;
         }
+        SmartDashboard.putNumber("error", error);
 
-        double move = 0;
+        double move = (0.5 - Math.abs(error))*2 * magnatude;
-        double rot = rotPid.update(targetAngle - curAngle);
+        double rot = Math.max(rotPid.update(error) * magnatude, magnatude);
 
         io.driveWithInput(
             move + rot, 

src/main/java/frc4388/robot/subsystems/differential/DiffReal.java

@@ -7,13 +7,13 @@ import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
 import com.ctre.phoenix.motorcontrol.can.TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
 
-public class DiffTalonSRX implements DiffIO {
+public class DiffReal implements DiffIO {
     TalonSRX m_leftFront;
     TalonSRX m_rightFront;
     TalonSRX m_leftRear;
     TalonSRX m_rightRear;
 
-    public DiffTalonSRX(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) {
+    public DiffReal(TalonSRX m_leftFront, TalonSRX m_rightFront, TalonSRX m_leftRear, TalonSRX m_rightRear) {
         this.m_leftFront = m_leftFront;
         this.m_rightFront = m_rightFront;
         this.m_leftRear = m_leftRear;
@@ -31,12 +31,12 @@ public class DiffTalonSRX implements DiffIO {
     @Override
     public void updateInputs(DiffState state) {
         state.leftCurrent = new double[] {m_leftFront.getStatorCurrent(), m_leftRear.getStatorCurrent()};
-        state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / 1100;
+        state.leftOutputPosition = (m_leftFront.getSelectedSensorPosition() + m_leftRear.getSelectedSensorPosition()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
-        state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / 1100;
+        state.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
 
         state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()};
-        state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / 1100;
+        state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
-        state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / 1100;
+        state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / DiffConstants.ENCODER_TICKS_PER_ROTATION;
     }
     
     @Override
@@ -44,9 +44,4 @@ public class DiffTalonSRX implements DiffIO {
         m_leftFront.set(TalonSRXControlMode.PercentOutput, leftInput);
         m_rightFront.set(TalonSRXControlMode.PercentOutput, rightInput);
     }
-
-    @AutoLogOutput
-    public double test() {
-        return m_leftFront.getSelectedSensorPosition();
-    }
 }

src/main/java/frc4388/robot/subsystems/differential/DiffSim.java

@@ -0,0 +1,79 @@
+package frc4388.robot.subsystems.differential;
+
+import static edu.wpi.first.units.Units.Meters;
+
+import java.time.Duration;
+import java.time.Instant;
+import java.util.function.Supplier;
+
+import com.ctre.phoenix.motorcontrol.TalonSRXControlMode;
+import com.ctre.phoenix6.StatusSignal;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+
+public class DiffSim implements DiffIO {
+    private static final DCMotor MOTOR_SIM = new DCMotor(
+        12.3, 
+        3, 
+        -1.325, 
+        -0.5, 
+        -5.5, 
+        2);
+
+    public DifferentialDrivetrainSim sim = new DifferentialDrivetrainSim(MOTOR_SIM, 
+        1 / (double) DiffConstants.ENCODER_TICKS_PER_ROTATION, 
+        1, 
+        1, 
+        DiffConstants.WHEEL_RADIUS.in(Meters),
+        DiffConstants.TRACK_WIDTH.in(Meters), 
+        null
+    );
+
+    private Supplier<Rotation2d> currentRot = new Supplier<Rotation2d>() {
+        @Override
+        public Rotation2d get() {
+            return sim.getHeading();
+        }
+    };
+
+    private double clamp(double x, double min, double max) {
+        return Math.max(Math.min(x, max), min);
+    }
+    
+    @Override
+    public void driveWithInput(double leftInput, double rightInput) {
+        sim.setInputs(
+            clamp(leftInput, -1, 1) * 12, 
+            clamp(rightInput, -1, 1) * 12
+        );
+    }
+
+    Instant lastUpdate = Instant.now();
+
+    private double getTiemDelta() {
+        Instant now = Instant.now();
+        double elapsedSeconds = ((double) Duration.between(lastUpdate, now).toMillis()) / 1000;
+        // System.out.println(elapsedSeconds);%
+        lastUpdate = now;
+        return elapsedSeconds;
+    }
+
+    @Override
+    public void updateInputs(DiffState state) {
+        sim.update(getTiemDelta());
+
+        state.leftOutputPosition =  sim.getLeftPositionMeters() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters);
+        state.leftOutputVelocity = sim.getLeftVelocityMetersPerSecond() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters);
+        state.leftCurrent = new double[]{sim.getLeftCurrentDrawAmps()};
+
+        state.rightOutputPosition =  sim.getRightPositionMeters() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters);
+        state.rightOutputVelocity = sim.getRightVelocityMetersPerSecond() / DiffConstants.WHEEL_RADIUS_TO_ARC.in(Meters);
+        state.rightCurrent = new double[]{sim.getRightCurrentDrawAmps()};
+    }
+
+    public Supplier<Rotation2d> getSimAngle() {
+        return currentRot;
+    }
+}

src/main/java/frc4388/robot/subsystems/differential/GyroSim.java

@@ -0,0 +1,30 @@
+package frc4388.robot.subsystems.differential;
+
+import static edu.wpi.first.units.Units.Volts;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Voltage;
+
+public class GyroSim implements GyroIO {
+    private Supplier<Rotation2d> diffRot;
+
+    public GyroSim(Supplier<Rotation2d> diffRot){
+        this.diffRot = diffRot;
+    }
+    
+    private Rotation2d offsetRot = new Rotation2d();
+
+    @Override
+    public void updateInputs(GyroState inputs) {
+        inputs.connected = true;
+        inputs.voltage = Volts.of(12);
+        inputs.yaw = diffRot.get().rotateBy(offsetRot);
+    }
+
+    @Override
+    public void reset() {
+        offsetRot = diffRot.get().unaryMinus();
+    }
+}

src/main/java/frc4388/robot/subsystems/lidar/LidarReal.java

@@ -4,12 +4,12 @@ import edu.wpi.first.wpilibj.Counter;
 import frc4388.robot.constants.Constants.LiDARConstants;
 
 // https://girlsofsteeldocs.readthedocs.io/en/latest/technical-resources/sensors/LIDAR-Lite-Distance-Sensor.html#minimal-roborio-interface
-public class LidarLiteV2 implements LidarIO {
+public class LidarReal implements LidarIO {
 
     
     private Counter LidarPWM;
 
-    public LidarLiteV2(int port) {
+    public LidarReal(int port) {
         LidarPWM = new Counter(port);
         LidarPWM.setMaxPeriod(1.00); //set the max period that can be measured
         LidarPWM.setSemiPeriodMode(true); //Set the counter to period measurement

src/main/java/frc4388/robot/subsystems/vision/VisionReal.java

@@ -11,7 +11,7 @@ import org.photonvision.targeting.PhotonPipelineResult;
 import frc4388.robot.constants.Constants.FieldConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
 
-public class VisionPhotonvision implements VisionIO {
+public class VisionReal implements VisionIO {
     // private PhotonCamera[] cameras;
     // private PhotonPoseEstimator[] estimators;
     
@@ -21,7 +21,7 @@ public class VisionPhotonvision implements VisionIO {
     // public List<EstimatedRobotPose> poses = new ArrayList<>();
 
     
-    public VisionPhotonvision(PhotonCamera camera, Transform3d position){
+    public VisionReal(PhotonCamera camera, Transform3d position){
         this.camera = camera;
         estimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, position);
         estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
@@ -34,7 +34,7 @@ public class VisionPhotonvision implements VisionIO {
         state.isTagProcessed = false;
         state.isTagDetected = false;
 
-        state.lastEstimatedPose = null;
+        // state.lastEstimatedPose = null;
 
         var results = camera.getAllUnreadResults();