2025 Robot Essentials

Copied 2025 repository and removed reefscape or not reusable code.

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

@@ -0,0 +1,35 @@
+package frc4388.utility.compute;
+
+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/compute/ReefPositionHelper.java

@@ -0,0 +1,106 @@
+package frc4388.utility.compute;
+
+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.util.Units;
+import frc4388.robot.constants.Constants.AutoConstants;
+import frc4388.robot.constants.Constants.FieldConstants;
+
+public class ReefPositionHelper {
+    public enum Side {
+        LEFT,
+        RIGHT,
+        CENTER,
+        FAR_LEFT
+    }
+
+    public static final Pose2d[] RED_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
+    };
+
+    public static final Pose2d[] BLUE_TAGS = {
+        FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
+        FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
+    };
+
+    public static double distanceTo(Pose2d first, Pose2d second){
+        return Math.sqrt(Math.pow(first.getX()  -  second.getX(),2) + Math.pow(first.getY()  -  second.getY(),2));
+    }
+
+
+    /*
+    * Function to loop through a list of tag locations to figure out closest one
+    */
+    public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
+        if(locations.length <= 0) return new Pose2d();
+
+        Pose2d minPos = locations[0];
+        double minDistance = distanceTo(position,minPos);
+
+        for(int i = 1; i < locations.length; i++){
+            double dist = distanceTo(locations[i],position);
+            if(dist < minDistance){
+                minPos = locations[i];
+                minDistance = dist;
+            }
+        }
+        
+        System.out.println(minPos.getRotation().getDegrees());
+
+        return minPos;
+    }
+
+    /*
+     * Function to find closest tag location based on side
+     */
+    public static Pose2d getNearestTag(Pose2d position) {
+
+        if(TimesNegativeOne.isRed)
+            return getNearestTag(RED_TAGS, position);
+        else
+            return getNearestTag(BLUE_TAGS, position);
+    }
+
+    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
+        return offset(getNearestTag(position), 
+        getSide(side) + xtrim,
+        ydistance); 
+    }
+
+    public static double getSide(Side side){
+        switch(side) {
+            case LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET);
+            case FAR_LEFT:
+                return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
+            case RIGHT:
+                return (AutoConstants.X_SCORING_POSITION_OFFSET);
+            case CENTER:
+                return 0;
+        }
+        assert false;
+        return 0;
+    }
+
+
+    public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
+        Translation2d oldTranslation = oldPose.getTranslation();
+        
+        double rot = oldPose.getRotation().getRadians();
+
+        return new Pose2d(new Translation2d(
+            oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
+            oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
+        ), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
+    }
+}

src/main/java/frc4388/utility/compute/RobotTime.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.utility.compute;
+
+/**
+ * <p>Keeps track of Robot times like time passed, delta time, etc
+ * <p>All times are in milliseconds
+ */
+public class RobotTime {
+    private long m_currTime = System.currentTimeMillis();
+    public long m_deltaTime = 0;
+
+    private long m_startRobotTime = m_currTime;
+    public long m_robotTime = 0;
+    public long m_lastRobotTime = 0;
+
+    private long m_startMatchTime = 0;
+    public  long m_matchTime = 0;
+    public long m_lastMatchTime = 0;
+
+    public long m_frameNumber = 0;
+
+    /**
+     * Private constructor prevents other classes from instantiating
+     */
+    private RobotTime(){}
+
+    private static RobotTime instance = null;
+
+    /**
+     * Gets the instance of Robot Time. If there is no instance running one will be created.
+     * @return instance of Robot Time
+     */
+    public static RobotTime getInstance() {
+        if (instance == null) {
+            instance = new RobotTime();
+        }
+        return instance;
+    }
+
+    /**
+     * Call this once per periodic loop.
+     */
+    public void updateTimes() {
+        m_lastRobotTime = m_robotTime;
+        m_lastMatchTime = m_matchTime;
+
+        m_currTime = System.currentTimeMillis();
+        m_robotTime = m_currTime - m_startRobotTime;
+        m_deltaTime = m_robotTime - m_lastRobotTime;
+        m_frameNumber++;
+
+        if (m_startMatchTime != 0) {
+            m_matchTime = m_currTime - m_startMatchTime;
+        }
+    }
+
+    /**
+     * Call this in both the auto and periodic inits
+     */
+    public void startMatchTime() {
+        if (m_startMatchTime == 0) {
+            m_startMatchTime = m_currTime;
+        }
+    }
+
+    /**
+     * Call this in disabled init
+     */
+    public void endMatchTime() {
+        m_startMatchTime = 0;
+        m_matchTime = 0;
+    }
+}

src/main/java/frc4388/utility/compute/RobotUnits.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility.compute;
+
+/** Aarav's good units class (better than WPILib)
+ * @author Aarav Shah */
+
+public class RobotUnits {
+  // constants
+
+  // angle conversions
+  public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;}
+
+  public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;}
+
+  // falcon conversions
+  public static double falconTicksToRotations(final double ticks) {return ticks / 2048;}
+
+  public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;}
+
+  // distance conversions
+  public static double metersToFeet(final double meters) {return meters * 3.28084;}
+
+  public static double feetToMeters(final double feet) {return feet / 3.28084;}
+}

src/main/java/frc4388/utility/compute/TimesNegativeOne.java

@@ -0,0 +1,51 @@
+package frc4388.utility.compute;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
+
+// Class that holds weather the drivers sticks should be inverted
+public class TimesNegativeOne {
+
+    public static boolean XAxis = SwerveDriveConstants.INVERT_X;
+    public static boolean YAxis = SwerveDriveConstants.INVERT_Y;
+    public static boolean RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+    public static boolean isRed = false;
+    public static Rotation2d ForwardOffset = Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET);
+
+    private static boolean isRed() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        if(alliance.isEmpty()) return false;
+        return (alliance.get() == Alliance.Red);
+    }
+
+    public static void update(){
+        isRed = isRed();
+        XAxis = SwerveDriveConstants.INVERT_X ^ isRed;
+        YAxis = SwerveDriveConstants.INVERT_Y ^ isRed;
+        RotAxis = SwerveDriveConstants.INVERT_ROTATION;
+        ForwardOffset = Rotation2d.fromDegrees((SwerveDriveConstants.FORWARD_OFFSET + (isRed ? 0 : 0)));
+        SmartDashboard.putBoolean("Is red alliance", isRed);
+    }
+
+    public static double invert(double num, boolean invert){
+        return invert ? -num : num;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertXY){
+        if(invertXY) return stick.times(-1);
+        else return stick;
+    }
+
+    public static Translation2d invert(Translation2d stick, boolean invertX, boolean invertY){
+        return new Translation2d(
+            invert(stick.getX(), invertX), 
+            invert(stick.getY(), invertY)
+        );
+    }
+}

src/main/java/frc4388/utility/compute/Trim.java

@@ -0,0 +1,145 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.utility.compute;
+
+import java.io.FileInputStream;
+import java.io.FileOutputStream;
+import java.util.ArrayList;
+
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+
+/**
+ * Reboot persistant Trims.
+ * @author Zachary Wilke
+ */
+public class Trim {
+    private static ArrayList<Trim> trims = new ArrayList<Trim>();
+    private static ShuffleboardTab trimTab = Shuffleboard.getTab("Trims");
+
+    private String trimName; 
+    private double upperBound;
+    private double lowerBound;
+    private double step;
+
+    private boolean modified = false;
+    private double currentValue;
+    private boolean persistant = false;
+
+    private GenericEntry trimElement = null;
+
+    /**
+     * Creates a variably Trim with a given name, upper and lower bounds, step size and intial value
+     * @param trimName please keep the trim name without special symbols
+     * @param upperBound the upper limit inclusive
+     * @param lowerBound the lower limit inclusive
+     * @param step the step size
+     * @param inital the inital value, will get overridden if the persistant trim exists on disk.
+     * @param persistnat Weather the trim is persistant or not
+     */
+    public Trim(String trimName, double upperBound, double lowerBound, double step, double inital, boolean persistant) {
+        this.trimName = trimName;
+        this.upperBound = upperBound;
+        this.lowerBound = lowerBound;
+        this.step = step;
+        this.persistant = persistant;
+        currentValue = inital;
+        load();
+        trimElement = trimTab.add(trimName, currentValue).getEntry();
+
+        trims.add(this);
+    }
+
+    /**
+     * Creates a non-Trim with a given name, upper and lower bounds, step size and intial value
+     * @param trimName please keep the trim name without special symbols
+     * @param upperBound the upper limit inclusive
+     * @param lowerBound the lower limit inclusive
+     * @param step the step size
+     * @param inital the inital value, will get overridden if the persistant trim exists on disk.
+     */
+    public Trim(String trimName, double upperBound, double lowerBound, double step, double inital) {
+        this.trimName = trimName;
+        this.upperBound = upperBound;
+        this.lowerBound = lowerBound;
+        this.step = step;
+        currentValue = inital;
+        load();
+        trimElement = trimTab.add(trimName, currentValue).getEntry();
+
+        trims.add(this);
+    }
+
+    private void clampModify() {
+        currentValue = Math.min(upperBound, Math.max(currentValue, lowerBound));
+        if (trimElement != null) 
+            trimElement.setValue(currentValue);
+        modified = true;
+    }
+
+    public void stepUp() {
+        this.currentValue += step;
+        clampModify();
+    }
+
+    public void stepDown() {
+        this.currentValue -= step;
+        clampModify();
+    }
+    
+    public void set(double value) {
+        this.currentValue = value;
+        clampModify();
+    }
+
+    public double get() {
+        return this.currentValue;
+    }
+
+    public boolean isModified() {
+        return modified;
+    }
+
+    public boolean load() {
+        if(!persistant)
+            return false;
+
+        try (FileInputStream stream = new FileInputStream("/home/lvuser/trims/" + trimName)) {
+            double fileValue = DataUtils.byteArrayToDouble(stream.readNBytes(8));
+            currentValue = fileValue;
+            clampModify();
+            modified = false;
+            if (fileValue != currentValue) {
+                System.out.println("TRIMS: Loaded trim `" + trimName + "` has a value that is higher than or less than the bounds set for the trim, clamping...");
+                modified = true;
+            }
+            return true;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to read trim file `" + trimName + "`, using current value...");
+            return false;
+        }
+        
+    }
+
+    public void dump() {
+        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/trims/" + trimName)) {
+            stream.write(DataUtils.doubleToByteArray(currentValue));
+            modified = false;
+        } catch (Exception e) {
+            // e.printStackTrace();
+            System.out.println("TRIMS: Unable to write to trim file `" + trimName + "`!?!");
+        }
+    }
+
+    public static void dumpAll() {
+        for (int i = 0; i < trims.size(); i++) {
+            Trim trim = trims.get(i);
+            if (trim.isModified()) 
+                trim.dump();
+        }
+    }
+}