From 4fb67cfc9c3e852dd01aaa6b69f4a5d68fa1eef9 Mon Sep 17 00:00:00 2001 From: Abhishrek05 Date: Sat, 18 Feb 2023 14:12:55 -0800 Subject: [PATCH] Robot Units done --- src/main/java/frc4388/robot/Constants.java | 14 ------- src/main/java/frc4388/robot/Robot.java | 2 +- .../frc4388/robot/subsystems/SwerveDrive.java | 12 +++--- .../robot/subsystems/SwerveModule.java | 10 ++--- src/main/java/frc4388/utility/RobotUnits.java | 38 ++++++++++++++----- 5 files changed, 41 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6aad04d..ac33b46 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -66,20 +66,6 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11.0; // TODO: find the actual value public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 1.0; // TODO: find the actual value - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; - public static final double MOTOR_REV_PER_STEER_REV = 12.8; - - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double WHEEL_DIAMETER_INCHES = 3.9; - public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; - - public static final double WHEEL_REV_PER_MOTOR_REV = 1 / MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; - public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; - public static final double INCHES_PER_TICK = 1 / TICKS_PER_INCH; - - public static final double TICK_TIME_TO_SECONDS = 10; - public static final double SECONDS_TO_TICK_TIME = 1 / TICK_TIME_TO_SECONDS; } public static final class Configurations { diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 32b75bf..43e9fd4 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -24,7 +24,7 @@ public class Robot extends TimedRobot { Command m_autonomousCommand; private RobotTime m_robotTime = RobotTime.getInstance(); - private RobotUnits m_robotUnits = RobotUnits.getInstance(); + private RobotUnits m_robotUnits = RobotUnits.getInstance(3.9, new double[] {5.12, 12.8}); private RobotContainer m_robotContainer; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0b13b6c..1fefa67 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -14,11 +14,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; +import frc4388.utility.RobotUnits; public class SwerveDrive extends SubsystemBase { @@ -29,10 +29,10 @@ public class SwerveDrive extends SubsystemBase { private SwerveModule[] modules; - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftFrontLocation = new Translation2d(RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(-RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -RobotUnits.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); @@ -108,7 +108,7 @@ public class SwerveDrive extends SubsystemBase { * @param desiredStates Array of module states to set. */ public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, RobotUnits.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); for (int i = 0; i < desiredStates.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = desiredStates[i]; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index a88fca7..c315d65 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -14,10 +14,10 @@ import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; +import frc4388.utility.RobotUnits; public class SwerveModule extends SubsystemBase { private WPI_TalonFX driveMotor; @@ -87,7 +87,7 @@ public class SwerveModule extends SubsystemBase { } public double getDrivePos() { - return this.driveMotor.getSelectedSensorPosition() / SwerveDriveConstants.Conversions.TICKS_PER_MOTOR_REV; + return this.driveMotor.getSelectedSensorPosition(); } public double getDriveVel() { @@ -109,7 +109,7 @@ public class SwerveModule extends SubsystemBase { */ public SwerveModuleState getState() { return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + RobotUnits.tickTimeToSeconds(RobotUnits.inchesToMeters(RobotUnits.ticksToInches(driveMotor.getSelectedSensorVelocity()))), getAngle() ); } @@ -119,7 +119,7 @@ public class SwerveModule extends SubsystemBase { * @return The current position of the SwerveModule in meters traveled by the driveMotor and the angle of the angleMotor. */ public SwerveModulePosition getPosition() { - return new SwerveModulePosition(Units.inchesToMeters(driveMotor.getSelectedSensorPosition() * SwerveDriveConstants.Conversions.INCHES_PER_TICK), getAngle()); + return new SwerveModulePosition(RobotUnits.inchesToMeters(RobotUnits.ticksToInches(driveMotor.getSelectedSensorPosition())), getAngle()); } /** @@ -144,7 +144,7 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); } - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); + double feetPerSecond = RobotUnits.metersToFeet(state.speedMetersPerSecond); // double inchesPerSecond = Units.metersToFeet(state.speedMetersPerSecond) * 12; // driveMotor.set(TalonFXControlMode.Velocity, inchesPerSecond * SwerveDriveConstants.Conversions.TICKS_PER_INCH * SwerveDriveConstants.Conversions.SECONDS_TO_TICK_TIME); diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java index e11393f..3cf3745 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/RobotUnits.java @@ -12,19 +12,25 @@ public final class RobotUnits { // variables private static boolean isSwerve; - // constants + // measurement 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; + // robot constants + private static final int 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 static double INCHES_PER_WHEEL_REV; + private static double TICKS_PER_WHEEL_REV; + + // final conversions + private static final double TICK_TIME_TO_SECONDS = 0.1; + private static double TICKS_PER_INCH; + - - // private constructor private RobotUnits(final double WHEEL_DIAMETER_INCHES, final double[] gearRatios) throws IllegalArgumentException { if (gearRatios.length == 1) { @@ -43,17 +49,25 @@ public final class RobotUnits { } RobotUnits.WHEEL_DIAMETER_INCHES = WHEEL_DIAMETER_INCHES; + RobotUnits.initVar(); } private static RobotUnits instance = null; - public static RobotUnits getInstance() { + public static RobotUnits getInstance(final double WHEEL_DIAMETER_INCHES, final double[] gearRatios) { if (instance == null) { - instance = new RobotUnits(0, new double[] {0.0, 0.0}); + instance = new RobotUnits(WHEEL_DIAMETER_INCHES, gearRatios); } return instance; } + private static void initVar() { + INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; + TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV; + + TICKS_PER_INCH = TICKS_PER_WHEEL_REV / INCHES_PER_WHEEL_REV; + } + // angle conversions public static double degreesToRadians(final double degrees) {return Math.toRadians(degrees);} @@ -78,11 +92,17 @@ public final class RobotUnits { public static double secondsToMilliseconds(final double seconds) {return seconds * MILLISECONDS_PER_SECONDS;} // falcon conversions - public static double falconTicksToMotorRotations(final double ticks) {return ticks / FALCON_TICKS_PER_MOTOR_REV;} + public static double falconTicksToMotorRotations(final double ticks) {return ticks / TICKS_PER_MOTOR_REV;} - public static double falconMotorRotationsToTicks(final double rotations) {return rotations * FALCON_TICKS_PER_MOTOR_REV;} + public static double falconMotorRotationsToTicks(final double rotations) {return rotations * TICKS_PER_MOTOR_REV;} + + public static double inchesToTicks(final double inches) {return inches * TICKS_PER_INCH;} + + public static double ticksToInches(final double ticks) {return ticks / TICKS_PER_INCH;} - // TODO: Implement later + public static double secondsToTickTime(final double seconds) {return seconds * TICK_TIME_TO_SECONDS;} + + public static double tickTimeToSeconds(final double tickTime) {return tickTime / TICK_TIME_TO_SECONDS;} // distance conversions public static double metersToInches(final double meters) {return meters / METERS_PER_INCH;}