diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b8e87f0..e889538 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,16 +2,24 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.pathplanner.lib.util.PIDConstants; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Current; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.TimedRobot; public final class Constants { public static final class RobotConstants { - public static final double kNominalVoltage = 12.0; + public static final Measure kNominalVoltage = Volts.of(12); public static final double kPeriod = TimedRobot.kDefaultPeriod; } @@ -41,7 +49,6 @@ public static final class AbsoluteEncoders { public static final int kRearLeftTurningEncoderPort = 45; public static final String kAbsEncoderMagnetOffsetKey = "AbsEncoderMagnetOffsetKey"; - public static final double kDefaultAbsEncoderMagnetOffset = 0.0; } /** @@ -56,42 +63,40 @@ public static final class AbsoluteEncoders { * kRearRightTurningEncoderReversed = true; */ - // Units are meters. // Distance between centers of right and left wheels on robot - public static final double kTrackWidth = 0.6096; // 2024 Competion Robot 24 inches + public static final Measure kTrackWidth = Inches.of(24); // 2024 Competion Robot // Distance between front and rear wheels on robot - public static final double kWheelBase = 0.5715; // 2024 Competion Robot 22.5 inches - - // public static final double kTrackWidth = 0.572; // 2023 Competion Robot - // public static final double kWheelBase = 0.622; // 2023 Competion Robot + public static final Measure kWheelBase = Inches.of(22.5); // 2024 Competion Robot // Robot Radius - public static final double kRadius = 0.423; + public static final Measure kRadius = Inches.of(16.449); // Units are meters per second - public static final double kMaxTranslationalVelocity = 4.0; // 2023 Competion Robot // max 4.5 + public static final Measure> kMaxTranslationalVelocity = + MetersPerSecond.of(4.0); // 2023 Competion Robot // max 4.5 // Units are radians per second - public static final double kMaxRotationalVelocity = 5.0; // 2023 Competion Robot // max 5.0 + public static final Measure> kMaxRotationalVelocity = + RadiansPerSecond.of(5.0); // 2023 Competion Robot // max 5.0 // The locations for the modules must be relative to the center of the robot. // Positive x values represent moving toward the front of the robot // Positive y values represent moving toward the left of the robot. public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics( - new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase / 2.0, -kTrackWidth / 2.0), // front right - new Translation2d(-kWheelBase / 2.0, kTrackWidth / 2.0), // rear left - new Translation2d(-kWheelBase / 2.0, -kTrackWidth / 2.0) // rear right + new Translation2d(kWheelBase.times(0.5), kTrackWidth.times(0.5)), // front left + new Translation2d(kWheelBase.times(0.5), kTrackWidth.times(-0.5)), // front right + new Translation2d(kWheelBase.times(-0.5), kTrackWidth.times(0.5)), // rear left + new Translation2d(kWheelBase.times(-0.5), kTrackWidth.times(-0.5)) // rear right ); public static final SwerveDriveKinematics kDriveKinematicsDriveFromArm = new SwerveDriveKinematics( - new Translation2d(kWheelBase, kTrackWidth / 2.0), // front left - new Translation2d(kWheelBase, -kTrackWidth / 2.0), // front right - new Translation2d(0.0, kTrackWidth / 2.0), // rear left - new Translation2d(0.0, -kTrackWidth / 2.0) // rear right + new Translation2d(kWheelBase, kTrackWidth.times(0.5)), // front left + new Translation2d(kWheelBase, kTrackWidth.times(-0.5)), // front right + new Translation2d(Inches.zero(), kTrackWidth.times(0.5)), // rear left + new Translation2d(Inches.zero(), kTrackWidth.times(-0.5)) // rear right ); // public static final boolean kGyroReversed = false; @@ -102,8 +107,8 @@ public static final class AbsoluteEncoders { public static final class ModuleConstants { - public static final int kDriveMotorCurrentLimit = 80; - public static final int kTurningMotorCurrentLimit = 80; + public static final Measure kDriveMotorCurrentLimit = Amps.of(80); + public static final Measure kTurningMotorCurrentLimit = Amps.of(80); public static final double kDriveP = 0.1; // 2023 Competition Robot public static final double kDriveI = 0.0; // 2023 Competition Robot @@ -122,18 +127,19 @@ public static final class ModuleConstants { // public static final SimpleMotorFeedforward driveFF = new SimpleMotorFeedforward(0.254, // 0.137); - public static final double kWheelDiameterMeters = 0.10; // 3.7 in; 2023 Competion Robot + public static final Measure kWheelDiameter = + Inches.of(3.937); // 3.7 in; 2023 Competion Robot // By default, the drive encoder in position mode measures rotations at the drive motor // Convert to meters at the wheel public static final double kDriveGearRatio = 6.75; // 2023 Competion Robot - public static final double kDrivePositionConversionFactor = - (kWheelDiameterMeters * Math.PI) / kDriveGearRatio; + public static final Measure kDrivePositionConversionFactor = + kWheelDiameter.times(Math.PI).divide(kDriveGearRatio); // By default, the drive encoder in velocity mode measures RPM at the drive motor // Convert to meters per second at the wheel - public static final double kDriveVelocityConversionFactor = - kDrivePositionConversionFactor / 60.0; + public static final Measure> kDriveVelocityConversionFactor = + kDrivePositionConversionFactor.per(Minute); // By default, the turn encoder in position mode measures rotations at the turning motor // Convert to rotations at the module azimuth @@ -188,7 +194,7 @@ public static enum IntakeState { public static final int kMotorID = 16; - public static final int kCurrentLimit = 15; + public static final Measure kCurrentLimit = Amps.of(15); public static final double kVelocityP = 0.1; public static final double kVelocityI = 0.0; @@ -198,18 +204,22 @@ public static enum IntakeState { public static final double kPositionI = 0.0; public static final double kPositionD = 0.0; - public static final double kFirstRepositionDistance = 0.15; - public static final double kSecondRepositionDistance = 0.23; - public static final double kPositionTolerance = 0.01; + public static final Measure kFirstRepositionDistance = Meters.of(0.15); + public static final Measure kSecondRepositionDistance = Meters.of(0.23); + public static final Measure kPositionTolerance = Meters.of(0.01); - public static final double kRollerDiameter = 0.0508; // 2 inches + public static final Measure kRollerDiameter = Inches.of(2); public static final double kGearRatio = 10.0; - public static final double kPositionConversionFactor = (kRollerDiameter * Math.PI) / kGearRatio; - public static final double kVelocityConversionFactor = kPositionConversionFactor / 60.0; + public static final Measure kPositionConversionFactor = + kRollerDiameter.times(Math.PI).divide(kGearRatio); + public static final Measure> kVelocityConversionFactor = + kPositionConversionFactor.per(Minute); - public static final double kMaxVelocity = (0.5 * 11710.0) * kVelocityConversionFactor; - public static final double kMaxAcceleration = kMaxVelocity / 0.1; + public static final Measure> kMaxVelocity = + kVelocityConversionFactor.times(0.5).times(11710); + public static final Measure>> kMaxAcceleration = + kMaxVelocity.per(Seconds.of(0.1)); public static final TrapezoidProfile.Constraints kConstraints = new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration); @@ -217,8 +227,9 @@ public static enum IntakeState { public static final int kRetroReflectiveSensorDIOPort = 1; public static final int kBeamBreakSensorDIOPort = 0; - public static final double kRepositionSpeedArmDown = 1.0; - public static final double kRepositionSpeedArmUp = 1.0; + public static final Measure> kRepositionSpeedArmDown = + MetersPerSecond.of(1.0); + public static final Measure> kRepositionSpeedArmUp = MetersPerSecond.of(1.0); } public static final class ArmConstants { @@ -242,8 +253,8 @@ public static final class ClimberConstants { public static final int kLeftMotorPort = 24; public static final int kRightMotorPort = 23; - public static final int kMotorCurrentLimit = 60; - public static final double kMotorCurrentHardStop = 15.0; + public static final Measure kMotorCurrentLimit = Amps.of(60); + public static final Measure kMotorCurrentHardStop = Amps.of(15); public static final double kP = 3.0; public static final double kI = 0.0; @@ -267,14 +278,14 @@ public static final class ClimberConstants { public static final TrapezoidProfile.Constraints slowConstraints = new TrapezoidProfile.Constraints(kSlowMaxVelocity, kSlowMaxAcceleration); - public static final float kUpperLimit = -0.25f; - public static final float kLowerLimit = -16.0f; + public static final Measure kUpperLimit = Inches.of(-0.25); + public static final Measure kLowerLimit = Inches.of(-16.0); - public static final double kSeekPosition = 25.0; - public static final double kHomePosition = -3.2; - public static final double kDeployPosition = -0.25; + public static final Measure kSeekPosition = Inches.of(25); + public static final Measure kHomePosition = Inches.of(-3.2); + public static final Measure kDeployPosition = Inches.of(-0.25); - public static final double kAllowablePositionError = 0.01; + public static final Measure kAllowablePositionError = Inches.of(0.01); public static enum CalibrationState { UNCALIBRATED, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2a3afb..42931ca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,6 +2,8 @@ package frc.robot; +import static edu.wpi.first.units.Units.MetersPerSecond; + import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj.DigitalInput; @@ -248,7 +250,7 @@ private void createNamedCommands() { private void setDefaultCommands() { m_swerve.setDefaultCommand( new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver)); - m_intake.setDefaultCommand(m_intake.createSetVelocityCommand(0)); + m_intake.setDefaultCommand(m_intake.createSetVelocityCommand(MetersPerSecond.zero())); m_climber.setDefaultCommand(m_climber.createStopCommand()); } diff --git a/src/main/java/frc/robot/climber/Actuator.java b/src/main/java/frc/robot/climber/Actuator.java index 8727e8b..d6a93c2 100644 --- a/src/main/java/frc/robot/climber/Actuator.java +++ b/src/main/java/frc/robot/climber/Actuator.java @@ -1,5 +1,6 @@ package frc.robot.climber; +import static edu.wpi.first.units.Units.*; import static frc.robot.RobotContainer.getRobotContainer; import com.revrobotics.CANSparkBase; @@ -12,6 +13,8 @@ import edu.wpi.first.math.filter.Debouncer.DebounceType; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants.ClimberConstants; @@ -51,15 +54,15 @@ public Actuator(String climberName, int climberMotorChannel) { m_climberMover.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true); m_climberMover.setSoftLimit( - CANSparkMax.SoftLimitDirection.kForward, ClimberConstants.kUpperLimit); + CANSparkMax.SoftLimitDirection.kForward, (float) ClimberConstants.kUpperLimit.in(Inches)); m_climberMover.setSoftLimit( - CANSparkMax.SoftLimitDirection.kReverse, ClimberConstants.kLowerLimit); + CANSparkMax.SoftLimitDirection.kReverse, (float) ClimberConstants.kLowerLimit.in(Inches)); m_climberMover.setIdleMode(IdleMode.kBrake); - m_climberMover.setSmartCurrentLimit(ClimberConstants.kMotorCurrentLimit); + m_climberMover.setSmartCurrentLimit((int) ClimberConstants.kMotorCurrentLimit.in(Amps)); m_climberMover.setInverted(false); - m_climberPIDController.setTolerance(ClimberConstants.kAllowablePositionError); + m_climberPIDController.setTolerance(ClimberConstants.kAllowablePositionError.in(Inches)); m_climberRelativeEncoder = m_climberMover.getEncoder(); @@ -68,15 +71,15 @@ public Actuator(String climberName, int climberMotorChannel) { m_climberRelativeEncoder.setVelocityConversionFactor( ClimberConstants.kVelocityConversionFactor); - Preferences.initDouble(climberName + "position", ClimberConstants.kHomePosition); + Preferences.initDouble(climberName + "position", ClimberConstants.kHomePosition.in(Inches)); m_climberRelativeEncoder.setPosition( - Preferences.getDouble(climberName + "position", ClimberConstants.kHomePosition)); + Preferences.getDouble(climberName + "position", ClimberConstants.kHomePosition.in(Inches))); } public void configurePositionController( - TrapezoidProfile.Constraints constraints, double targetPosition) { + TrapezoidProfile.Constraints constraints, Measure targetPosition) { m_climberPIDController.setConstraints(constraints); - m_climberPIDController.setGoal(targetPosition); + m_climberPIDController.setGoal(targetPosition.in(Inches)); m_climberPIDController.reset(m_climberRelativeEncoder.getPosition()); } @@ -104,7 +107,8 @@ public void resetEncoder() { } public boolean getCurrentSenseState() { - return m_debouncer.calculate(getOutputCurrent() > ClimberConstants.kMotorCurrentHardStop); + return m_debouncer.calculate( + getOutputCurrent() > ClimberConstants.kMotorCurrentHardStop.in(Amps)); } private boolean getUpperSoftLimitSwtichState() { diff --git a/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java b/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java index d13e374..85cf26c 100644 --- a/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java +++ b/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java @@ -2,6 +2,8 @@ package frc.robot.climber.commands; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ClimberConstants; import frc.robot.climber.Actuator; @@ -11,9 +13,9 @@ public class DriveToPositionCommand extends Command { private final Climber m_climber; private final Actuator[] m_actuators; - private final double m_targetPosition; + private final Measure m_targetPosition; - public DriveToPositionCommand(Climber subsystem, double targetPosition) { + public DriveToPositionCommand(Climber subsystem, Measure targetPosition) { m_climber = subsystem; addRequirements(m_climber); m_actuators = m_climber.getClimberSides(); diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 97feb60..5cec421 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -2,6 +2,8 @@ package frc.robot.drivetrain; +import static edu.wpi.first.units.Units.*; + import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -17,7 +19,6 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; import frc.robot.Constants.AutoConstants; import frc.robot.Constants.DriveConstants; import frc.robot.Constants.RobotConstants; @@ -25,8 +26,6 @@ /** Constructs a swerve drive style drivetrain. */ public class Drivetrain extends SubsystemBase { - static double kMaxSpeed = Constants.DriveConstants.kMaxTranslationalVelocity; - static double kMaxAngularSpeed = Constants.DriveConstants.kMaxRotationalVelocity; private final SwerveDriveKinematics m_kinematics = DriveConstants.kDriveKinematics; @@ -122,7 +121,8 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod)); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConstants.kMaxTranslationalVelocity); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); @@ -134,7 +134,8 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds, SwerveDriveKinematics var swerveModuleStates = kinematicsType.toSwerveModuleStates( ChassisSpeeds.discretize(chassisSpeeds, RobotConstants.kPeriod)); - SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed); + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConstants.kMaxTranslationalVelocity); m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); @@ -240,8 +241,8 @@ public void configurePathPlanner() { new HolonomicPathFollowerConfig( AutoConstants.kTranslationControllerGains, // Translation PID constants AutoConstants.kRotationControllerGains, // Rotation PID constants - DriveConstants.kMaxTranslationalVelocity, // Max module speed, in m/s - DriveConstants.kRadius, // Drive base radius in meters + DriveConstants.kMaxTranslationalVelocity.in(MetersPerSecond), // Max module speed + DriveConstants.kRadius.in(Meters), // Drive base radius new ReplanningConfig() // Default path replanning config ), diff --git a/src/main/java/frc/robot/drivetrain/SwerveModule.java b/src/main/java/frc/robot/drivetrain/SwerveModule.java index 59d9531..eb2819b 100644 --- a/src/main/java/frc/robot/drivetrain/SwerveModule.java +++ b/src/main/java/frc/robot/drivetrain/SwerveModule.java @@ -2,6 +2,8 @@ package frc.robot.drivetrain; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.AbsoluteSensorRangeValue; @@ -59,10 +61,10 @@ public SwerveModule( m_driveMotor.setIdleMode(IdleMode.kCoast); m_turningMotor.setIdleMode(IdleMode.kBrake); - m_driveMotor.setSmartCurrentLimit(ModuleConstants.kDriveMotorCurrentLimit); - m_turningMotor.setSmartCurrentLimit(ModuleConstants.kTurningMotorCurrentLimit); - m_driveMotor.enableVoltageCompensation(RobotConstants.kNominalVoltage); - m_turningMotor.enableVoltageCompensation(RobotConstants.kNominalVoltage); + m_driveMotor.setSmartCurrentLimit((int) ModuleConstants.kDriveMotorCurrentLimit.in(Amps)); + m_turningMotor.setSmartCurrentLimit((int) ModuleConstants.kTurningMotorCurrentLimit.in(Amps)); + m_driveMotor.enableVoltageCompensation(RobotConstants.kNominalVoltage.in(Volts)); + m_turningMotor.enableVoltageCompensation(RobotConstants.kNominalVoltage.in(Volts)); m_driveMotor.setInverted(false); m_turningMotor.setInverted(false); @@ -104,8 +106,10 @@ public SwerveModule( m_driveEncoder = m_driveMotor.getEncoder(); m_turningRelativeEncoder = m_turningMotor.getEncoder(); - m_driveEncoder.setPositionConversionFactor(ModuleConstants.kDrivePositionConversionFactor); - m_driveEncoder.setVelocityConversionFactor(ModuleConstants.kDriveVelocityConversionFactor); + m_driveEncoder.setPositionConversionFactor( + ModuleConstants.kDrivePositionConversionFactor.in(Meters)); + m_driveEncoder.setVelocityConversionFactor( + ModuleConstants.kDriveVelocityConversionFactor.in(MetersPerSecond)); m_turningRelativeEncoder.setPositionConversionFactor( ModuleConstants.kTurnPositionConversionFactor); diff --git a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java index ba9dcb3..34e583e 100644 --- a/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java +++ b/src/main/java/frc/robot/drivetrain/commands/DriveCommand.java @@ -5,6 +5,10 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.DriveConstants; @@ -12,9 +16,9 @@ public abstract class DriveCommand extends Command { - private double xDot; - private double yDot; - private double thetaDot; + private Measure> xDot; + private Measure> yDot; + private Measure> thetaDot; // used to swap control locations SwerveDriveKinematics kinematicsType; @@ -34,9 +38,9 @@ public void initialize() {} @Override public void execute() { - xDot = getX() * DriveConstants.kMaxTranslationalVelocity; - yDot = getY() * DriveConstants.kMaxTranslationalVelocity; - thetaDot = getTheta() * DriveConstants.kMaxRotationalVelocity; + xDot = DriveConstants.kMaxTranslationalVelocity.times(getX()); + yDot = DriveConstants.kMaxTranslationalVelocity.times(getY()); + thetaDot = DriveConstants.kMaxRotationalVelocity.times(getTheta()); SmartDashboard.putBoolean("fieldRelative", fieldRelative()); SmartDashboard.putBoolean("rotateField", drivetrain.fieldRotatedSupplier().getAsBoolean()); diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index a4948ff..de47b3d 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -1,5 +1,7 @@ package frc.robot.intake; +import static edu.wpi.first.units.Units.*; + import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; @@ -7,6 +9,9 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.event.BooleanEvent; @@ -52,7 +57,7 @@ public Intake() { m_motor = new CANSparkMax(IntakeConstants.kMotorID, MotorType.kBrushless); m_motor.restoreFactoryDefaults(); m_motor.setIdleMode(IdleMode.kBrake); - m_motor.setSmartCurrentLimit(IntakeConstants.kCurrentLimit); + m_motor.setSmartCurrentLimit((int) IntakeConstants.kCurrentLimit.in(Amps)); m_motor.setInverted(false); m_velocityController = m_motor.getPIDController(); @@ -60,12 +65,14 @@ public Intake() { m_velocityController.setI(IntakeConstants.kVelocityI); m_velocityController.setD(IntakeConstants.kVelocityD); - m_positionController.setTolerance(IntakeConstants.kPositionTolerance); + m_positionController.setTolerance(IntakeConstants.kPositionTolerance.in(Meters)); m_relativeEncoder = m_motor.getEncoder(); m_relativeEncoder.setPosition(0.0); - m_relativeEncoder.setPositionConversionFactor(IntakeConstants.kPositionConversionFactor); - m_relativeEncoder.setVelocityConversionFactor(IntakeConstants.kVelocityConversionFactor); + m_relativeEncoder.setPositionConversionFactor( + IntakeConstants.kPositionConversionFactor.in(Meters)); + m_relativeEncoder.setVelocityConversionFactor( + IntakeConstants.kVelocityConversionFactor.in(MetersPerSecond)); } public BooleanSupplier stateChecker(IntakeState state) { @@ -87,18 +94,18 @@ public Command createStopIntakeCommand() { }); } - private void configurePositionController(double targetPosition) { + private void configurePositionController(Measure targetPosition) { m_positionController.reset(m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); - m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition.in(Meters)); } // spotless:off - private void advanceAfterIntaking(double targetPosition) { + private void advanceAfterIntaking(Measure targetPosition) { m_secondSensorTriggered.rising().ifHigh( () -> { m_positionController.reset( m_relativeEncoder.getPosition(), m_relativeEncoder.getVelocity()); - m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition); + m_positionController.setGoal(m_relativeEncoder.getPosition() + targetPosition.in(Meters)); }); setState(IntakeState.PROCESSING); m_motor.set(m_positionController.calculate(m_relativeEncoder.getPosition())); @@ -155,19 +162,20 @@ public BooleanSupplier atGoalSupplier() { return () -> m_positionController.atGoal(); } - private void setVelocity(double targetVelocity) { - m_velocityController.setReference(targetVelocity, ControlType.kVelocity); + private void setVelocity(Measure> targetVelocity) { + m_velocityController.setReference(targetVelocity.in(MetersPerSecond), ControlType.kVelocity); } - public Command createSetVelocityCommand(double targetVelocity) { + public Command createSetVelocityCommand(Measure> targetVelocity) { return this.startEnd(() -> this.setVelocity(targetVelocity), () -> {}); } - public Command createJoystickControlCommand(XboxController m_controller, double factor) { + public Command createJoystickControlCommand( + XboxController m_controller, Measure> factor) { return this.run( () -> { setState(IntakeState.MANUALLY_REPOSITIONING); - this.setVelocity(m_controller.getLeftY() * factor); + this.setVelocity(factor.times(m_controller.getLeftY())); }); }