Work on adding advantagekit

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

@@ -0,0 +1,29 @@
+package frc4388.robot.subsystems.differential;
+
+import static edu.wpi.first.units.Units.Inches;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.Distance;
+import frc4388.utility.status.CanDevice;
+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 TRACK_DISPLACEMENT = Inches.of(6.5); //meters
+    public static final Distance TRACK_WIDTH = TRACK_DISPLACEMENT.times(2);
+
+    public static final Gains ROT_GAINS = new Gains(20, 0, 0);
+
+    public static final class IDs {
+        public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 4);
+        public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 1);
+        public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 2);
+        public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 3);
+        
+        // public static final CanDevice FRONT_LEFT_MOTOR = new CanDevice("SRX_FRONT_LEFT", 3);
+        // public static final CanDevice FRONT_RIGHT_MOTOR = new CanDevice("SRX_FRONT_RIGHT", 2);
+        // public static final CanDevice REAR_LEFT_MOTOR = new CanDevice("SRX_REAR_LEFT", 1);
+        // public static final CanDevice REAR_RIGHT_MOTOR = new CanDevice("SRX_REAR_RIGHT", 4);
+    }
+}

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

@@ -0,0 +1,118 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+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.wpilibj2.command.SubsystemBase;
+import frc4388.utility.status.Queryable;
+import frc4388.utility.status.Status;
+import frc4388.utility.structs.Drivebase;
+
+public class DiffDrive extends SubsystemBase implements Drivebase, Queryable {
+    DiffIO io;
+    DiffStateAutoLogged state = new DiffStateAutoLogged();
+    GyroIO gyroIO;
+    GyroStateAutoLogged gyroState = new GyroStateAutoLogged();
+
+
+    DifferentialDriveKinematics kinematics = new DifferentialDriveKinematics(DiffConstants.TRACK_WIDTH);
+    DifferentialDrivePoseEstimator odometry = new DifferentialDrivePoseEstimator(kinematics, new Rotation2d(), 0, 0, new Pose2d());
+
+    public DiffDrive(DiffIO io, GyroIO gyroIO) {
+        this.io = io;
+        this.gyroIO = gyroIO;
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(state);
+        Logger.processInputs(getName(), state);
+        gyroIO.updateInputs(gyroState);
+        Logger.processInputs("gyro", gyroState);
+
+        var speeds = new DifferentialDriveWheelPositions(
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.leftOutputPosition),
+            DiffConstants.WHEEL_RADIUS_TO_ARC.times(state.rightOutputPosition)
+        );
+
+        odometry.update(gyroState.yaw, speeds);
+    }
+
+    @Override
+    public void arcadeDrive(Translation2d left, Translation2d right) {
+        io.driveWithInput(
+            left.getY() + right.getX(), 
+            left.getY() - right.getX()
+        );
+    }
+
+    private PID rotPid = new PID(DiffConstants.ROT_GAINS);
+
+    @Override
+    public void driveFieldRelative(Translation2d left, Translation2d right) {
+        // In case the driver's stick is inside the deadband
+        if(right.getNorm() == 0) {
+            io.driveWithInput(0, 0);
+            return;
+        };
+
+        double targetAngle = right.getAngle().getRotations();
+        double curAngle = gyroState.yaw.getRotations();
+
+        double error = targetAngle - curAngle - signedFloor(curAngle);
+        if(error > 0.5) {
+            error -= 1;
+        }else if(error < -0.5) {
+            error += 1;
+        }
+
+        double move = 0;
+        double rot = rotPid.update(targetAngle - curAngle);
+
+        io.driveWithInput(
+            move + rot, 
+            move - rot
+        );
+    }
+
+    private double signedFloor(double x){
+        if(x > 0) {
+            return x - Math.floor(x);
+        } else {
+            return Math.ceil(x) - x;
+        }
+    }
+
+    @Override
+    public void resetOdometry() {
+        gyroIO.reset();
+        odometry.resetPose(new Pose2d());
+    }
+
+
+
+    @AutoLogOutput
+    public Pose2d estimatedPose() {
+        return odometry.getEstimatedPosition();
+    }
+
+
+    @Override
+    public String getName() {
+        return "Diff drive";
+    }
+
+    @Override
+    public Status diagnosticStatus() {
+        return new Status();
+    }
+
+}

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

@@ -0,0 +1,20 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface DiffIO {
+    @AutoLog
+    public class DiffState {
+        public double leftOutputPosition = 0;
+        public double leftOutputVelocity = 0;
+        public double[] leftCurrent = new double[] {};
+
+        public double rightOutputPosition = 0;
+        public double rightOutputVelocity = 0;
+        public double[] rightCurrent = new double[] {};
+    }
+
+    public default void driveWithInput(double leftInput, double rightInput) {}
+
+    public default void updateInputs(DiffState state) {}
+}

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

@@ -0,0 +1,52 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+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 {
+    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) {
+        this.m_leftFront = m_leftFront;
+        this.m_rightFront = m_rightFront;
+        this.m_leftRear = m_leftRear;
+        this.m_rightRear = m_rightRear;
+
+
+        m_leftRear.follow(m_leftFront);
+        m_rightRear.follow(m_rightFront);
+
+        
+        this.m_rightFront.setInverted(true);
+        this.m_rightRear.setInverted(true);
+    }
+
+    @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.leftOutputVelocity = (m_leftFront.getSelectedSensorVelocity() + m_leftRear.getSelectedSensorVelocity()) / 2 / 1100;
+
+        state.rightCurrent = new double[] {m_rightFront.getStatorCurrent(), m_rightRear.getStatorCurrent()};
+        state.rightOutputPosition = (m_rightFront.getSelectedSensorPosition() + m_rightFront.getSelectedSensorPosition()) / 2 / 1100;
+        state.rightOutputVelocity = (m_rightFront.getSelectedSensorVelocity() + m_rightFront.getSelectedSensorVelocity()) / 2 / 1100;
+    }
+    
+    @Override
+    public void driveWithInput(double leftInput, double rightInput) {
+        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/GyroIO.java

@@ -0,0 +1,19 @@
+package frc4388.robot.subsystems.differential;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.Voltage;
+
+public interface GyroIO {
+    @AutoLog
+    public class GyroState {
+        public boolean connected = false;
+        public Voltage voltage;
+        public Rotation2d yaw;
+    }
+    
+    public default void updateInputs(GyroState inputs) {}
+
+    public default void reset() {}
+}

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

@@ -0,0 +1,36 @@
+package frc4388.robot.subsystems.differential;
+
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import frc4388.utility.status.Alerts;
+
+public class GyroPigeon2 implements GyroIO {
+    Pigeon2 m_gyro;
+
+    StatusSignal<Voltage> voltage;
+
+    public GyroPigeon2(Pigeon2 m_gyro){
+        this.m_gyro = m_gyro;
+
+        voltage = m_gyro.getSupplyVoltage();
+    }
+
+    @Override
+    public void updateInputs(GyroState state) {
+        voltage.refresh();
+        
+        state.connected = m_gyro.isConnected();
+        state.voltage = voltage.getValue();
+        state.yaw = m_gyro.getRotation2d();
+
+        Alerts.validate(!state.connected, "Gyro is not connected!", AlertType.kError);
+    }
+
+    @Override
+    public void reset() {
+        m_gyro.reset();
+    }
+}

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

@@ -0,0 +1,29 @@
+package frc4388.robot.subsystems.differential;
+
+import frc4388.utility.structs.Gains;
+
+public class PID {
+    protected Gains  gains;
+    private   double output    = 0;
+
+
+    /** Creates a new PelvicInflammatoryDisease. */
+    public PID(Gains gains) {
+        this.gains = gains;
+    }
+
+    private double prevError, cumError = 0;
+    
+    // Called every time the scheduler runs while the command is scheduled.
+    public double update(double error) {
+        cumError += error * .02; // 20 ms
+        double delta = error - prevError;
+
+        output = error * gains.kP;
+        output += cumError * gains.kI;
+        output += delta * gains.kD;
+        output += gains.kF;
+
+        return output;
+    }
+}