Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-08 16:28:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Robot Units done

Browse Source
This commit is contained in:
Abhishrek05
2023-02-18 14:12:55 -08:00
parent 7873d76366
commit 4fb67cfc9c
5 changed files with 41 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
-14
View File
@@ -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 {
src/main/java/frc4388/robot/Robot.java
+1 -1
View File
@@ -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;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -6
View File
@@ -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];
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+5 -5
View File
@@ -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);
src/main/java/frc4388/utility/RobotUnits.java
+29 -9
View File
@@ -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;}