rebase from 2024

src/main/java/frc4388/utility/DataUtils.java

@@ -0,0 +1,35 @@
+package frc4388.utility;
+
+import java.nio.ByteBuffer;
+
+public class DataUtils {
+    public static byte[] doubleToByteArray(double value) {
+        byte[] bytes = new byte[8];
+        ByteBuffer.wrap(bytes).putDouble(value);
+        return bytes;
+    }
+    
+    public static double byteArrayToDouble(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getDouble();
+    }
+
+    public static byte[] intToByteArray(int value) {
+        byte[] bytes = new byte[4];
+        ByteBuffer.wrap(bytes).putInt(value);
+        return bytes;
+    }
+    
+    public static int byteArrayToInt(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getInt();
+    }
+
+    public static byte[] shortToByteArray(short value) {
+        byte[] bytes = new byte[2];
+        ByteBuffer.wrap(bytes).putShort(value);
+        return bytes;
+    }
+    
+    public static short byteArrayToShort(byte[] bytes) {
+        return ByteBuffer.wrap(bytes).getShort();
+    }
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -7,20 +7,22 @@
 
 package frc4388.utility;
 
-import com.ctre.phoenix.sensors.WPI_Pigeon2;
+// import com.ctre.phoenix.sensors.WPI_Pigeon2;
+import com.ctre.phoenix6.hardware.Pigeon2;
 import com.kauailabs.navx.frc.AHRS;
 
 // import edu.wpi.first.wpilibj.GyroBase;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
+// import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 /**
  * Gyro class that allows for interchangeable use between a pigeon and a navX
  */
-public class RobotGyro implements Gyro {
+public class RobotGyro {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private WPI_Pigeon2 m_pigeon = null;
+    private Pigeon2 m_pigeon = null;
     private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
@@ -34,7 +36,7 @@ public class RobotGyro implements Gyro {
      * Creates a Gyro based on a pigeon
      * @param gyro the gyroscope to use for Gyro
      */
-    public RobotGyro(WPI_Pigeon2 gyro) {
+    public RobotGyro(Pigeon2 gyro) {
         m_pigeon = gyro;
         m_isGyroAPigeon = true;
     }
@@ -54,8 +56,8 @@ public class RobotGyro implements Gyro {
     public void resetZeroValues() {
         if (!m_isGyroAPigeon) return;
 
-        pitchZero = m_pigeon.getPitch();
-        rollZero =  m_pigeon.getRoll();
+        // pitchZero = m_pigeon.getPitch();
+        // rollZero =  m_pigeon.getRoll();
     }
 
     /**
@@ -84,16 +86,15 @@ public class RobotGyro implements Gyro {
      * is typically done when the robot is first turned on while it's sitting at rest before the
      * competition starts.
      */
-    @Override
     public void calibrate() {
-        if (m_isGyroAPigeon) {
-            m_pigeon.calibrate();
-        } else {
-            m_navX.calibrate();
-        }
+        return;
+        // if (m_isGyroAPigeon) {
+        //     m_pigeon.calibrate();
+        // } else {
+        //     m_navX.calibrate();
+        // }
     }
 
-    @Override
     public void reset() {
         resetZeroValues();
 
@@ -102,6 +103,73 @@ public class RobotGyro implements Gyro {
         } else {
             m_navX.reset();
         }
+
+    }
+
+    public void reset(double val) {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(val);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetFlip() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(180);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetNinety() {
+        resetZeroValues();
+        
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(90);
+        } else {
+            m_navX.reset();
+        }
+        
+    }
+
+    public void resetTwoSeventy() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(270);
+        } else {
+            m_navX.reset();
+        }
+
+    }
+
+    public void resetRightSideBlue() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(60);
+        } else {
+            m_navX.reset();
+        }   
+
+    }
+
+    public void resetAmpSide() {
+        resetZeroValues();
+
+        if (m_isGyroAPigeon) {
+            m_pigeon.setYaw(-60);
+        } else {
+            m_navX.reset();
+        }   
+
     }
 
     /**
@@ -113,16 +181,19 @@ public class RobotGyro implements Gyro {
 	 *					Roll is within [-90,+90] degrees.
      */
     private double[] getPigeonAngles() {
-        double[] ypr = new double[3];
-        m_pigeon.getYawPitchRoll(ypr);
+        m_pigeon.getAngle();
+        var rotation = m_pigeon.getRotation3d();
 
-        return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)};
+        return new double[] {RobotUnits.radiansToDegrees(rotation.getX() - rollZero), RobotUnits.radiansToDegrees(rotation.getY() - pitchZero), RobotUnits.radiansToDegrees(rotation.getZ())};
+    }
+
+    public Rotation2d getRotation2d() {
+        return m_pigeon.getRotation2d();
     }
 
-    @Override
     public double getAngle() {
         if (m_isGyroAPigeon) {
-            return getPigeonAngles()[0];
+            return getPigeonAngles()[2];
         } else {
             return m_navX.getAngle();
         }
@@ -176,7 +247,6 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    @Override
     public double getRate() {
         if (m_isGyroAPigeon) {
             return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
@@ -185,7 +255,7 @@ public class RobotGyro implements Gyro {
         }
     }
 
-    public WPI_Pigeon2 getPigeon(){
+    public Pigeon2 getPigeon(){
         return m_pigeon;
     }
 
@@ -193,7 +263,6 @@ public class RobotGyro implements Gyro {
         return m_navX;
     }
 
-    @Override
     public void close() throws Exception {
 
     }

src/main/java/frc4388/utility/UtilityStructs.java

@@ -6,7 +6,21 @@ public class UtilityStructs {
         public double leftY  = 0.0;
         public double rightX = 0.0;
         public double rightY = 0.0;
+
+        public boolean OPLB;
+        public boolean OPRB;
+
     
         public long timedOffset = 0;
     }
+    public static class AutoRecordingControllerFrame {
+        public double[] axes = new double[6];
+        public short button = 0;
+        public short[] POV = new short[1];
+
+    }
+    public static class AutoRecordingFrame {
+        public AutoRecordingControllerFrame[] controllerFrames = new AutoRecordingControllerFrame[2];
+        public int timeStamp;
+    }
 }

src/main/java/frc4388/utility/configurable/ConfigurableDouble.java

@@ -0,0 +1,23 @@
+package frc4388.utility.configurable;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ConfigurableDouble {
+    private double defualtValue;
+    private String name;
+
+    /**
+     * Creates an new ConfigurableDouble through Smart Dashboard.
+     * @param name the name of the Smart Dashboard key.
+     * @param defualtValue the initilization value
+     */
+    public ConfigurableDouble(String name, double defualtValue) {
+        this.name = name;
+        this.defualtValue = defualtValue;
+        SmartDashboard.putNumber(name, defualtValue);
+    }
+
+    public double get() {
+        return SmartDashboard.getNumber(name, defualtValue);
+    }
+}

src/main/java/frc4388/utility/configurable/ConfigurableString.java

@@ -0,0 +1,23 @@
+package frc4388.utility.configurable;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ConfigurableString {
+    private String defualtValue;
+    private String name;
+
+    /**
+     * Creates an new ConfigurableString through Smart Dashboard.
+     * @param name the name of the Smart Dashboard key.
+     * @param defualtValue the initilization value
+     */
+    public ConfigurableString(String name, String defualtValue) {
+        this.name = name;
+        this.defualtValue = defualtValue;
+        SmartDashboard.putString(name, defualtValue);
+    }
+
+    public String get() {
+        return SmartDashboard.getString(name, defualtValue);
+    }
+}

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -0,0 +1,27 @@
+package frc4388.utility.controller;
+
+import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class DeadbandedXboxController extends XboxController {
+  public DeadbandedXboxController(int port) { super(port); }
+  
+  @Override public double getLeftX() { return getLeft().getX(); }
+  @Override public double getLeftY() { return getLeft().getY(); }
+  @Override public double getRightX() { return getRight().getX(); }
+  @Override public double getRightY() { return getRight().getY(); }
+
+  public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); }
+  public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); }
+
+  public static Translation2d skewToDeadzonedCircle(double x, double y) {
+    Translation2d translation2d = new Translation2d(x, y);
+    double magnitude = translation2d.getNorm();
+
+    if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
+
+    return translation2d;
+  }
+}

src/main/java/frc4388/utility/controller/VirtualController.java

@@ -0,0 +1,145 @@
+package frc4388.utility.controller;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * A virtual controller that can be bound like an standard controller.
+ * @author Zachary Wilke
+ */
+public class VirtualController extends GenericHID {
+    private short m_buttonStates = 0; 
+    private short m_buttonStatesLastFrame = 0; 
+    private double[] m_axes = new double[6];
+    private short[] m_pov = new short[1];
+
+    /**
+     * Create an virtual controller
+     * @param port virtual port (merely a formality).
+     */
+    public VirtualController(int port) {
+        super(port);
+    }
+
+    /**
+     * Set the curent inputs to the new frames.
+     * @param axes joystick axes, (i.e. joysticks and triggers).
+     * @param buttonFlags the bit packed button states.
+     * @param pov the array of dpads.
+     */
+    public void setFrame(double[] axes, short buttonFlags, short[] pov) {
+        m_axes = axes;
+        setOutputs(buttonFlags);
+        m_pov = pov;
+    }
+
+    /**
+     * Zero outs the controls.
+     */
+    public void zeroControls() {
+        m_axes = new double[6];
+        m_buttonStates = 0;
+        m_buttonStatesLastFrame = 0;
+        m_pov = new short[] {-1};
+    }
+
+    /**
+     * Gets the value of a bitflag from an int
+     * @param value int to search
+     * @param index index of bit
+     * @return if the bit is set
+     */
+    public static boolean getFlag(int value, int index) {
+        return ((value & 1 << index) != 0);
+    }
+    
+    @Override
+    public boolean getRawButton(int button) { // man why are buttons indexed at 1.
+        return getFlag(m_buttonStates, button - 1);
+    }
+
+    @Override
+    public boolean getRawButtonPressed(int button) {  
+        return (!getFlag(m_buttonStatesLastFrame, button - 1) && getRawButton(button));
+    }
+    
+    @Override
+    public boolean getRawButtonReleased(int button) {
+        return (getFlag(m_buttonStatesLastFrame, button - 1) && !getRawButton(button));
+    }
+
+    @Override
+    public double getRawAxis(int axis) {
+        return m_axes[axis];
+    }
+    
+    @Override
+    public int getPOV(int pov) {
+        return m_pov[pov];
+    }
+
+    @Override
+    public int getAxisCount() {
+        return m_axes.length;
+    }
+
+    @Override
+    public int getPOVCount() {
+        return m_pov.length;
+    }
+
+    @Override
+    public int getButtonCount() {
+        return 10;
+    }
+
+    @Override
+    public boolean isConnected() {
+        return true;
+    }
+    
+    @Override
+    public HIDType getType() {
+        return HIDType.kXInputGamepad;
+    }
+
+    @Override
+    public String getName() {
+        return "Virtual Controller";
+    }
+
+    @Override
+    public int getAxisType(int axis) {
+        return 1; /* ! Warning, does not return accurate data.
+                  Hopefully this isn't a problem            */
+    }
+
+    /** 
+     * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
+     * this is an no-op overide.
+     */
+    @Override
+    public void setOutput(int outputNumber, boolean value) {
+        // do not use
+        //m_buttonStatesLastFrame[outputNumber - 1] = m_buttonStates[outputNumber - 1];
+        //m_buttonStates[outputNumber - 1] = value;
+    }
+
+    /**
+     * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} 
+     */    
+    @Override
+    public void setOutputs(int value) {
+        m_buttonStatesLastFrame = m_buttonStates;
+        m_buttonStates = (short) value;
+    }
+
+    /**
+     * Why are you Setting rumble on an virtual controller?
+     * @param type the rumble type (even though it won't do anything)
+     * @param value the rumble strength (always multiplyed by 0.0)
+     */
+    @Override
+    public void setRumble(RumbleType type, double value) {
+        System.out.println("Why are you Setting rumble on an virtual controller?");
+    }
+}

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -1,13 +1,13 @@
 package frc4388.utility.controller;
 
-import edu.wpi.first.wpilibj2.command.button.Button;
+//import edu.wpi.first.wpilibj2.command.button.Trigger;
 
 /**
  * Mapping for the Xbox controller triggers to allow triggers to be defined as
  * buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger 
  * exceeds a tolerance defined in {@link XboxController}.
  */
-public class XboxTriggerButton extends Button {
+public class XboxTriggerButton {//extends Trigger {
 	public static final int RIGHT_TRIGGER = 0;
 	public static final int LEFT_TRIGGER = 1;
 	public static final int RIGHT_AXIS_UP_TRIGGER = 2;