diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt index 7ef3197..d88daa5 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/Arm.kt @@ -1,6 +1,10 @@ package com.frcteam3636.frc2024.subsystems.arm import com.ctre.phoenix6.SignalLogger +import com.frcteam3636.frc2024.Robot +import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOPrototype +import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOReal +import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOSim import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.Degrees @@ -14,7 +18,11 @@ import org.littletonrobotics.junction.Logger object Arm : Subsystem { - private var io = ArmIOReal() + private var io: ArmIO = when (Robot.model) { + Robot.Model.SIMULATION -> ArmIOSim() + Robot.Model.COMPETITION -> ArmIOReal() + Robot.Model.PROTOTYPE -> ArmIOPrototype() + } var inputs = ArmIO.ArmInputs() diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt index 5fe01fc..bdc5cd1 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -6,17 +6,18 @@ import com.ctre.phoenix6.signals.GravityTypeValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.frcteam3636.frc2024.CTREMotorControllerId +import com.frcteam3636.frc2024.Robot import com.frcteam3636.frc2024.TalonFX -import com.frcteam3636.frc2024.utils.math.MotorFFGains -import com.frcteam3636.frc2024.utils.math.PIDGains -import com.frcteam3636.frc2024.utils.math.motorFFGains -import com.frcteam3636.frc2024.utils.math.pidGains +import com.frcteam3636.frc2024.utils.math.* +import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.units.Angle import edu.wpi.first.units.Measure import edu.wpi.first.units.Units.* import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.DutyCycleEncoder +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.inputs.LoggableInputs @@ -165,4 +166,48 @@ interface ArmIO{ private const val PROFILE_VELOCITY = 0.0 } + } + + class ArmIOSim : ArmIO { + val armSim = SingleJointedArmSim( + DCMotor.getKrakenX60(1), + 3.0, + 0.0301907551, + 0.331414, + -0.142921, + 2.32814365359, + true, + -0.142921 + ) + + val pidController = PIDController(ArmIOReal.PID_GAINS) + var setPoint = 0.0 + + override fun updateInputs(inputs: ArmIO.ArmInputs) { + armSim.update(Robot.period) + inputs.rightVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) + inputs.leftVelocity = RadiansPerSecond.of(armSim.velocityRadPerSec) + val pidVoltage = pidController.calculate(inputs.position.`in`(Radians),setPoint) + armSim.setInputVoltage(pidVoltage) + } + + override fun pivotToPosition(position: Measure) { + setPoint = position.`in`(Radians) + } + + override fun setVoltage(volts: Measure) { + armSim.setInputVoltage(volts.`in`(Volts)) + } + } + + class ArmIOPrototype: ArmIO { + //placeholder + override fun updateInputs(inputs: ArmIO.ArmInputs) { + } + + override fun pivotToPosition(position: Measure) { + } + + override fun setVoltage(volts: Measure) { + } } \ No newline at end of file