diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt index 7b6a577..19f9a4d 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt @@ -1,12 +1,17 @@ package com.frcteam3636.frc2024.subsystems.indexer +import com.frcteam3636.frc2024.Robot import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger object Indexer: Subsystem { - private var io: IndexerIO = IndexerIOReal() + private var io: IndexerIO = when (Robot.model) { + Robot.Model.SIMULATION -> IndexerIOSim() + Robot.Model.COMPETITION -> IndexerIOReal() + Robot.Model.PROTOTYPE -> IndexerIOPrototype() + } var inputs = IndexerIO.Inputs() diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt index 652f8d7..e3a1c30 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt @@ -5,8 +5,13 @@ import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import com.frcteam3636.frc2024.CANSparkFlex import com.frcteam3636.frc2024.REVMotorControllerId +import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.utils.LimelightHelpers import com.revrobotics.CANSparkLowLevel +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.units.Units.RadiansPerSecond +import edu.wpi.first.units.Units.RotationsPerSecond +import edu.wpi.first.wpilibj.simulation.FlywheelSim public enum class BalloonState { Blue, @@ -16,7 +21,7 @@ public enum class BalloonState { interface IndexerIO { class Inputs : LoggableInputs { - var indexerVelocity = Rotation2d() + var indexerVelocity = RotationsPerSecond.zero() var indexerCurrent: Double = 0.0 var balloonState: BalloonState = BalloonState.None @@ -27,7 +32,7 @@ interface IndexerIO { } override fun fromLog(table: LogTable) { - indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!![0] + indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!! indexerCurrent = table.get("Indexer Wheel Current", indexerCurrent) balloonState = table.get("Balloon Color", balloonState) } @@ -52,7 +57,7 @@ class IndexerIOReal : IndexerIO{ ) override fun updateInputs(inputs: IndexerIO.Inputs) { - inputs.indexerVelocity = Rotation2d(indexerMotor.encoder.velocity) + inputs.indexerVelocity = RadiansPerSecond.of(indexerMotor.encoder.velocity) inputs.indexerCurrent = indexerMotor.outputCurrent when (val colorClass = LimelightHelpers.getClassifierClass("limelight-sensor")) { @@ -67,4 +72,31 @@ class IndexerIOReal : IndexerIO{ assert(speed in -1.0..1.0) indexerMotor.set(speed) } +} + +class IndexerIOSim: IndexerIO { + val flywheelSim = FlywheelSim( + DCMotor.getNeoVortex(1), + 1.0, + 1.0 + ) + + override fun updateInputs(inputs: IndexerIO.Inputs) { + flywheelSim.update(Robot.period) + inputs.indexerVelocity = RadiansPerSecond.of(flywheelSim.angularVelocityRadPerSec) + } + + override fun setSpinSpeed(speed: Double) { + assert(speed in -1.0..1.0) + flywheelSim.setInputVoltage(speed*12.0) + } +} + +class IndexerIOPrototype: IndexerIO { + override fun updateInputs(inputs: IndexerIO.Inputs) { + } + + override fun setSpinSpeed(speed: Double) { + } + } \ No newline at end of file