diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java deleted file mode 100644 index ff2e71c..0000000 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "Alt-Swerve"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 1; - public static final String GIT_SHA = "b0e179382a7485d1a3c6823dccf4c424067fe519"; - public static final String GIT_DATE = "2024-12-13 12:31 IST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2024-12-13 13:01 IST"; - public static final long BUILD_UNIX_TIME = 1734087705771L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 636bf13..a1b2bee 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -1,17 +1,18 @@ package frc.robot import com.pathplanner.lib.auto.NamedCommands -import edu.wpi.first.math.MathUtil import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.units.Units import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController import frc.robot.ControllerInputs.driverController import frc.robot.subsystems.drive.Drive -import frc.robot.subsystems.drive.DriveCommands import frc.robot.subsystems.drive.getGyroIO import frc.robot.subsystems.drive.getSwerveModuleIOs +import frc.robot.subsystems.elevator.Elevator +import frc.robot.subsystems.elevator.ElevatorIOReal /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -21,9 +22,12 @@ import frc.robot.subsystems.drive.getSwerveModuleIOs */ object RobotContainer { private val swerveDrive: Drive + private val elevator: Elevator private val testController = CommandXboxController(2) init { + Elevator.initialize(ElevatorIOReal()) + elevator = Elevator.getInstance() swerveDrive = Drive(getGyroIO(), getSwerveModuleIOs()) registerAutoCommands() @@ -32,12 +36,13 @@ object RobotContainer { } private fun configureDefaultCommands() { - swerveDrive.defaultCommand = DriveCommands.joystickDrive( - swerveDrive, - { MathUtil.applyDeadband(driverController().leftY, 0.15) }, - { MathUtil.applyDeadband(driverController().leftX, 0.15) }, - { 0.7 * MathUtil.applyDeadband(-driverController().rightX, 0.15) } - ) +// swerveDrive.defaultCommand = DriveCommands.joystickDrive( +// swerveDrive, +// { MathUtil.applyDeadband(driverController().leftY, 0.15) }, +// { MathUtil.applyDeadband(driverController().leftX, 0.15) }, +// { 0.7 * MathUtil.applyDeadband(-driverController().rightX, 0.15) } +// ) + elevator.defaultCommand = elevator.setPower { driverController().rightTriggerAxis - driverController().leftTriggerAxis } } private fun configureButtonBindings() { @@ -47,6 +52,9 @@ object RobotContainer { }, swerveDrive) .ignoringDisable(true) ) + driverController().a().onTrue(elevator.reset()) + driverController().x().onTrue(elevator.setPosition(Units.Centimeter.of(50.0))) + driverController().b().whileTrue(elevator.setPosition(Units.Centimeter.of(100.0))) } fun getAutonomousCommand(): Command = Commands.none() diff --git a/src/main/kotlin/frc/robot/generated/TunerConstants.java b/src/main/kotlin/frc/robot/generated/TunerConstants.java index 0dcc03e..ff209f7 100644 --- a/src/main/kotlin/frc/robot/generated/TunerConstants.java +++ b/src/main/kotlin/frc/robot/generated/TunerConstants.java @@ -1,7 +1,6 @@ package frc.robot.generated; import static edu.wpi.first.units.Units.*; -import static frc.robot.ConstantsKt.ALT_ROBORIO_SERIAL; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.*; @@ -12,27 +11,40 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.*; -import frc.robot.ConstantsKt; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { // Both sets of gains need to be tuned to your individual robot. - private static double[] offsets; + private static final double[] offsets = + new double[] { + 43.129403832192686, 49.36043379258416, 34.134140492031285, 36.502606828526716 + }; // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static Slot0Configs steerGains; + private static Slot0Configs steerGains = + new Slot0Configs() + .withKP(330) + .withKI(1) + .withKD(20) + .withKS(0) + .withKV(0) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static Slot0Configs driveGains; + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(28).withKI(0).withKD(0).withKS(2.66447).withKV(1.18028); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static ClosedLoopOutputType kSteerClosedLoopOutput; + private static final ClosedLoopOutputType kSteerClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static ClosedLoopOutputType kDriveClosedLoopOutput; + private static final ClosedLoopOutputType kDriveClosedLoopOutput = + ClosedLoopOutputType.TorqueCurrentFOC; // The remote sensor feedback type to use for the steer motors; // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder @@ -40,252 +52,53 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static Current kSlipCurrent; + private static final Current kSlipCurrent = Amps.of(80.0); // Initial configs for the drive and steer motors and the CANcoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static TalonFXConfiguration driveInitialConfigs; - private static TalonFXConfiguration steerInitialConfigs; - + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can + // set a relatively + // low + // stator current limit to help avoid brownouts without + // impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration cancoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs private static final Pigeon2Configuration pigeonConfigs = null; // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static CANBus kCANBus; + public static final CANBus kCANBus = new CANBus("rio", "./logs/example.hoot"); // Theoretical free speed (m/s) at 12 V applied output; // This needs to be tuned to your individual robot - public static LinearVelocity kSpeedAt12Volts; + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.5); // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; // This may need to be tuned to your individual robot - private static double kCoupleRatio; + private static final double kCoupleRatio = 3.5; - private static double kDriveGearRatio; - private static double kSteerGearRatio; - private static Distance kWheelRadius; + private static final double kDriveGearRatio = 4.41; + private static final double kSteerGearRatio = 11.3142; + private static final Distance kWheelRadius = Centimeter.of(5.1); - private static boolean kInvertLeftSide; - private static boolean kInvertRightSide; + private static final boolean kInvertLeftSide = true; + private static final boolean kInvertRightSide = true; - private static int kPigeonId; + private static final int kPigeonId = 1; // These are only used for simulation - private static double kSteerInertia; - private static double kDriveInertia; + private static final double kSteerInertia = 0.004; + private static final double kDriveInertia = 0.025; // Simulated voltage necessary to overcome friction - private static Voltage kSteerFrictionVoltage; - private static Voltage kDriveFrictionVoltage; - - public static void init() { - if (ConstantsKt.getROBORIO_SERIAL_NUMBER().equals(ALT_ROBORIO_SERIAL)) { - offsets = - new double[] { - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 - }; - - steerGains = - new Slot0Configs() - .withKP(330) - .withKI(1) - .withKD(20) - .withKS(0) - .withKV(0) - .withKA(0) - .withStaticFeedforwardSign( - StaticFeedforwardSignValue.UseClosedLoopSign); - driveGains = - new Slot0Configs() - .withKP(30) - .withKI(0) - .withKD(0) - .withKS(2.66447) - .withKV(1.18028); - - kSteerClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; - kDriveClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; - - kSlipCurrent = Amps.of(80.0); - - driveInitialConfigs = new TalonFXConfiguration(); - steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(60) - .withStatorCurrentLimitEnable(true)); - - kCANBus = new CANBus("rio", "./logs/example.hoot"); - - kSpeedAt12Volts = MetersPerSecond.of(4.5); - - kCoupleRatio = 3.5; - - kDriveGearRatio = 4.41; - kSteerGearRatio = 11.3142; - kWheelRadius = Centimeter.of(5.1); - - kInvertLeftSide = true; - kInvertRightSide = true; - - kPigeonId = 1; - - // These are only used for simulation - kSteerInertia = 0.004; - kDriveInertia = 0.025; - // Simulated voltage necessary to overcome friction - kSteerFrictionVoltage = Volts.of(0.25); - kDriveFrictionVoltage = Volts.of(0.25); - - // Front Left - kFrontLeftDriveMotorId = 2; - kFrontLeftSteerMotorId = 1; - kFrontLeftEncoderId = 10; - kFrontLeftEncoderOffset = Radians.of(-offsets[0]); - kFrontLeftSteerMotorInverted = true; - kFrontLeftCANcoderInverted = false; - - kFrontLeftXPos = Meters.of(0.24); - kFrontLeftYPos = Meters.of(0.24); - - // Front Right - kFrontRightDriveMotorId = 4; - kFrontRightSteerMotorId = 3; - kFrontRightEncoderId = 20; - kFrontRightEncoderOffset = Radians.of(-offsets[1]); - kFrontRightSteerMotorInverted = true; - kFrontRightCANcoderInverted = false; - - kFrontRightXPos = Meters.of(0.24); - kFrontRightYPos = Meters.of(-0.24); - - // Back Left - kBackLeftDriveMotorId = 6; - kBackLeftSteerMotorId = 5; - kBackLeftEncoderId = 30; - kBackLeftEncoderOffset = Radians.of(-offsets[2]); - kBackLeftSteerMotorInverted = true; - kBackLeftCANcoderInverted = false; - - kBackLeftXPos = Meters.of(-0.24); - kBackLeftYPos = Meters.of(0.24); - - // Back Right - kBackRightDriveMotorId = 8; - kBackRightSteerMotorId = 7; - kBackRightEncoderId = 40; - kBackRightEncoderOffset = Radians.of(-offsets[3]); - kBackRightSteerMotorInverted = true; - kBackRightCANcoderInverted = false; - - kBackRightXPos = Meters.of(-0.24); - kBackRightYPos = Meters.of(-0.24); - } else { // TODO: calibrate - offsets = - new double[] { - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 - }; - - steerGains = - new Slot0Configs() - .withKP(330) - .withKI(1) - .withKD(20) - .withKS(0) - .withKV(0) - .withKA(0) - .withStaticFeedforwardSign( - StaticFeedforwardSignValue.UseClosedLoopSign); - driveGains = - new Slot0Configs() - .withKP(30) - .withKI(0) - .withKD(0) - .withKS(2.66447) - .withKV(1.18028); - - kSteerClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; - kDriveClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; - - kSlipCurrent = Amps.of(80.0); - - driveInitialConfigs = new TalonFXConfiguration(); - steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withStatorCurrentLimit(60) - .withStatorCurrentLimitEnable(true)); - - kCANBus = new CANBus("rio", "./logs/example.hoot"); - - kSpeedAt12Volts = MetersPerSecond.of(4.5); - - kCoupleRatio = 3.5; - - kDriveGearRatio = 4.41; - kSteerGearRatio = 11.3142; - kWheelRadius = Centimeter.of(5.1); - - kInvertLeftSide = true; - kInvertRightSide = true; - - kPigeonId = 1; - - // These are only used for simulation - kSteerInertia = 0.004; - kDriveInertia = 0.025; - // Simulated voltage necessary to overcome friction - kSteerFrictionVoltage = Volts.of(0.25); - kDriveFrictionVoltage = Volts.of(0.25); - - // Front Left - kFrontLeftDriveMotorId = 2; - kFrontLeftSteerMotorId = 1; - kFrontLeftEncoderId = 10; - kFrontLeftEncoderOffset = Radians.of(-offsets[0]); - kFrontLeftSteerMotorInverted = true; - kFrontLeftCANcoderInverted = false; - - kFrontLeftXPos = Meters.of(0.24); - kFrontLeftYPos = Meters.of(0.24); - - // Front Right - kFrontRightDriveMotorId = 4; - kFrontRightSteerMotorId = 3; - kFrontRightEncoderId = 20; - kFrontRightEncoderOffset = Radians.of(-offsets[1]); - kFrontRightSteerMotorInverted = true; - kFrontRightCANcoderInverted = false; - - kFrontRightXPos = Meters.of(0.24); - kFrontRightYPos = Meters.of(-0.24); - - // Back Left - kBackLeftDriveMotorId = 6; - kBackLeftSteerMotorId = 5; - kBackLeftEncoderId = 30; - kBackLeftEncoderOffset = Radians.of(-offsets[2]); - kBackLeftSteerMotorInverted = true; - kBackLeftCANcoderInverted = false; - - kBackLeftXPos = Meters.of(-0.24); - kBackLeftYPos = Meters.of(0.24); - - // Back Right - kBackRightDriveMotorId = 8; - kBackRightSteerMotorId = 7; - kBackRightEncoderId = 40; - kBackRightEncoderOffset = Radians.of(-offsets[3]); - kBackRightSteerMotorInverted = true; - kBackRightCANcoderInverted = false; - - kBackRightXPos = Meters.of(-0.24); - kBackRightYPos = Meters.of(-0.24); - } - } + private static final Voltage kSteerFrictionVoltage = Volts.of(0.25); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.25); public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() @@ -315,48 +128,48 @@ public static void init() { .withDriveFrictionVoltage(kDriveFrictionVoltage); // Front Left - private static int kFrontLeftDriveMotorId; - private static int kFrontLeftSteerMotorId; - private static int kFrontLeftEncoderId; - private static Angle kFrontLeftEncoderOffset; - private static boolean kFrontLeftSteerMotorInverted; - private static boolean kFrontLeftCANcoderInverted; + private static final int kFrontLeftDriveMotorId = 2; + private static final int kFrontLeftSteerMotorId = 1; + private static final int kFrontLeftEncoderId = 10; + private static final Angle kFrontLeftEncoderOffset = Radians.of(-offsets[0]); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftCANcoderInverted = false; - private static Distance kFrontLeftXPos; - private static Distance kFrontLeftYPos; + private static final Distance kFrontLeftXPos = Meters.of(0.24); + private static final Distance kFrontLeftYPos = Meters.of(0.24); // Front Right - private static int kFrontRightDriveMotorId; - private static int kFrontRightSteerMotorId; - private static int kFrontRightEncoderId; - private static Angle kFrontRightEncoderOffset; - private static boolean kFrontRightSteerMotorInverted; - private static boolean kFrontRightCANcoderInverted; + private static final int kFrontRightDriveMotorId = 4; + private static final int kFrontRightSteerMotorId = 3; + private static final int kFrontRightEncoderId = 20; + private static final Angle kFrontRightEncoderOffset = Radians.of(-offsets[1]); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightCANcoderInverted = false; - private static Distance kFrontRightXPos; - private static Distance kFrontRightYPos; + private static final Distance kFrontRightXPos = Meters.of(0.24); + private static final Distance kFrontRightYPos = Meters.of(-0.24); // Back Left - private static int kBackLeftDriveMotorId; - private static int kBackLeftSteerMotorId; - private static int kBackLeftEncoderId; - private static Angle kBackLeftEncoderOffset; - private static boolean kBackLeftSteerMotorInverted; - private static boolean kBackLeftCANcoderInverted; + private static final int kBackLeftDriveMotorId = 6; + private static final int kBackLeftSteerMotorId = 5; + private static final int kBackLeftEncoderId = 30; + private static final Angle kBackLeftEncoderOffset = Radians.of(-offsets[2]); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftCANcoderInverted = false; - private static Distance kBackLeftXPos; - private static Distance kBackLeftYPos; + private static final Distance kBackLeftXPos = Meters.of(-0.24); + private static final Distance kBackLeftYPos = Meters.of(0.24); // Back Right - private static int kBackRightDriveMotorId; - private static int kBackRightSteerMotorId; - private static int kBackRightEncoderId; - private static Angle kBackRightEncoderOffset; - private static boolean kBackRightSteerMotorInverted; - private static boolean kBackRightCANcoderInverted; - - private static Distance kBackRightXPos; - private static Distance kBackRightYPos; + private static final int kBackRightDriveMotorId = 8; + private static final int kBackRightSteerMotorId = 7; + private static final int kBackRightEncoderId = 40; + private static final Angle kBackRightEncoderOffset = Radians.of(-offsets[3]); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightCANcoderInverted = false; + + private static final Distance kBackRightXPos = Meters.of(-0.24); + private static final Distance kBackRightYPos = Meters.of(-0.24); public static final SwerveModuleConstants FrontLeft = ConstantCreator.createModuleConstants( diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/Elevator.kt b/src/main/kotlin/frc/robot/subsystems/elevator/Elevator.kt new file mode 100644 index 0000000..c7a33d4 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/Elevator.kt @@ -0,0 +1,46 @@ +package frc.robot.subsystems.elevator + +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger +import java.util.function.DoubleSupplier + +class Elevator private constructor(private val io: ElevatorIO) : SubsystemBase() { + + companion object { + @Volatile + private var instance: Elevator? = null + + fun initialize(io: ElevatorIO) { + synchronized(this) { + if (Elevator.instance == null) { + instance = Elevator(io) + } + } + } + fun getInstance(): Elevator { + return Elevator.instance ?: throw IllegalStateException( + "Elevator has not been initialized. Call initialize(io: ElevatorIO) first." + ) + } + } + + fun setPosition(position: Distance): Command { + return run({ io.setHeight(position) }) + } + + fun setPower(percentOutput: DoubleSupplier): Command { + return run({ io.setPower(percentOutput.asDouble) }) + } + + fun reset(): Command { + return Commands.runOnce({ io.reset() }, this) + } + + override fun periodic() { + io.updateInputs() + Logger.processInputs(this.name, io.inputs) + } +} diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt new file mode 100644 index 0000000..a2bd26d --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt @@ -0,0 +1,20 @@ +package frc.robot.subsystems.elevator + +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Distance +import frc.robot.lib.Gains +import frc.robot.lib.selectGainsBasedOnMode +import kotlin.math.PI + +const val MAX_HEIGHT = 1.3 +const val GEAR_RATIO = (1.0 / 12.0) * (42.0 / 48.0) +const val FIRST_STAGE_RATIO = 2.0 +val SPROCKET_RADIUS: Distance = Units.Millimeters.of(36.4 / 2.0) +val ROTATIONS_TO_CENTIMETER = GEAR_RATIO * FIRST_STAGE_RATIO * (SPROCKET_RADIUS.`in`(Units.Centimeter) * 2 * PI) +val GAINS = selectGainsBasedOnMode( + Gains( + 20.0, + kD = 1.0 + ), + Gains() +) diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIO.kt new file mode 100644 index 0000000..52ca71d --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIO.kt @@ -0,0 +1,25 @@ +package frc.robot.subsystems.elevator + +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Voltage +import org.team9432.annotation.Logged + +interface ElevatorIO { + val inputs: LoggedElevatorInputs + + fun setHeight(position: Distance) {} + + fun setPower(percentOutput: Double) {} + + fun reset() {} + + fun updateInputs() {} + + @Logged + open class ElevatorInputs { + var carriageHeight: Distance = Units.Meters.of(0.0) + var heightSetpoint: Distance = Units.Meters.of(0.0) + var appliedVoltege: Voltage = Units.Volts.of(0.0) + } +} diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt new file mode 100644 index 0000000..23628dc --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt @@ -0,0 +1,63 @@ +package frc.robot.subsystems.elevator + +import MOTOR_ID +import com.ctre.phoenix6.configs.CurrentLimitsConfigs +import com.ctre.phoenix6.configs.FeedbackConfigs +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.PositionVoltage +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Distance + +class +ElevatorIOReal : ElevatorIO { + override val inputs = LoggedElevatorInputs() + private val motor = TalonFX(MOTOR_ID) + private val motorPosititonRequest = PositionVoltage(0.0) + + init { + val motorConfig = TalonFXConfiguration().apply { + MotorOutput = MotorOutputConfigs().apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.Clockwise_Positive + } + Feedback = FeedbackConfigs().apply { + RotorToSensorRatio = 1.0 + } + Slot0 = Slot0Configs().apply { + kP = GAINS.kP + kI = GAINS.kI + kD = GAINS.kD + } + CurrentLimits = CurrentLimitsConfigs().apply { + StatorCurrentLimitEnable = true + SupplyCurrentLimitEnable = true + StatorCurrentLimit = 80.0 + SupplyCurrentLimit = 40.0 + } + } + motor.configurator.apply(motorConfig) + } + + override fun setHeight(position: Distance) { + inputs.heightSetpoint = position + val rotationalPosition = Units.Rotations.of(position.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER) + motor.setControl(motorPosititonRequest.withPosition(rotationalPosition)) + } + + override fun setPower(percentOutput: Double) { + motor.set(percentOutput) + } + + override fun reset() { + motor.setPosition(0.0) + } + override fun updateInputs() { + inputs.appliedVoltege = motor.motorVoltage.value + inputs.carriageHeight = Units.Centimeter.of(motor.position.value.`in`(Units.Rotations) * ROTATIONS_TO_CENTIMETER) + } +} diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOSim.kt new file mode 100644 index 0000000..0e0b542 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOSim.kt @@ -0,0 +1,31 @@ +package frc.robot.subsystems.elevator + +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionVoltage +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Distance +import frc.robot.lib.motors.TalonFXSim + +class ElevatorIOSim : ElevatorIO { + override val inputs = LoggedElevatorInputs() + private val motorPosititonRequest = PositionVoltage(0.0) + private val motor = TalonFXSim( + 1, + GEAR_RATIO, + 0.3, + 1.0 + ) + + override fun setHeight(position: Distance) { + val rotationalPosition = Units.Rotations.of(position.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER) + motor.setControl(motorPosititonRequest.withPosition(rotationalPosition)) + } + override fun setPower(percentOutput: Double) { + motor.setControl(DutyCycleOut(percentOutput)) + } + + override fun updateInputs() { + inputs.appliedVoltege = Units.Volts.of(motor.appliedVoltage) + inputs.carriageHeight = Units.Centimeter.of(motor.position * ROTATIONS_TO_CENTIMETER) + } +} diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorPorts.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorPorts.kt new file mode 100644 index 0000000..ec3681e --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorPorts.kt @@ -0,0 +1 @@ +const val MOTOR_ID = 12