diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2d6f348..32b75bf 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.utility.RobotTime; +import frc4388.utility.RobotUnits; /** * The VM is configured to automatically run this class, and to call the @@ -23,6 +24,8 @@ public class Robot extends TimedRobot { Command m_autonomousCommand; private RobotTime m_robotTime = RobotTime.getInstance(); + private RobotUnits m_robotUnits = RobotUnits.getInstance(); + private RobotContainer m_robotContainer; diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java index 34ac6b8..e11393f 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/RobotUnits.java @@ -9,12 +9,51 @@ package frc4388.utility; * @author Aarav Shah */ public final class RobotUnits { + // variables + private static boolean isSwerve; + // constants private static final double INCHES_PER_FOOT = 12.0; private static final double METERS_PER_INCH = 0.0254; private static final double SECONDS_PER_MINUTE = 60; private static final double MILLISECONDS_PER_SECONDS = 1000; + private static final int FALCON_TICKS_PER_MOTOR_REV = 2048; + private static double WHEEL_DIAMETER_INCHES; + private static double MOTOR_REV_PER_WHEEL_REV; + private static double MOTOR_REV_PER_STEER_REV; + + + + // private constructor + private RobotUnits(final double WHEEL_DIAMETER_INCHES, final double[] gearRatios) throws IllegalArgumentException { + if (gearRatios.length == 1) { + RobotUnits.isSwerve = false; + + RobotUnits.MOTOR_REV_PER_WHEEL_REV = gearRatios[0]; + + } else if (gearRatios.length == 2) { + RobotUnits.isSwerve = true; + + RobotUnits.MOTOR_REV_PER_WHEEL_REV = gearRatios[0]; + RobotUnits.MOTOR_REV_PER_STEER_REV = gearRatios[1]; + + } else { + throw new IllegalArgumentException(); + } + + RobotUnits.WHEEL_DIAMETER_INCHES = WHEEL_DIAMETER_INCHES; + } + + private static RobotUnits instance = null; + + public static RobotUnits getInstance() { + if (instance == null) { + instance = new RobotUnits(0, new double[] {0.0, 0.0}); + } + return instance; + } + // angle conversions public static double degreesToRadians(final double degrees) {return Math.toRadians(degrees);} @@ -39,9 +78,11 @@ public final class RobotUnits { public static double secondsToMilliseconds(final double seconds) {return seconds * MILLISECONDS_PER_SECONDS;} // falcon conversions - public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} + public static double falconTicksToMotorRotations(final double ticks) {return ticks / FALCON_TICKS_PER_MOTOR_REV;} - public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} + public static double falconMotorRotationsToTicks(final double rotations) {return rotations * FALCON_TICKS_PER_MOTOR_REV;} + + // TODO: Implement later // distance conversions public static double metersToInches(final double meters) {return meters / METERS_PER_INCH;}