From 9840e8b62e682efcd8b91aaefeb095be3f2d295b Mon Sep 17 00:00:00 2001 From: njoy3663 Date: Wed, 20 Nov 2024 18:21:35 -0800 Subject: [PATCH 1/2] arm sim --- .../frcteam3636/frc2024/subsystems/arm/Arm.kt | 10 ++- .../frc2024/subsystems/arm/ArmIO.kt | 61 ++++++++++++++++--- 2 files changed, 62 insertions(+), 9 deletions(-) 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..6e88d04 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 @@ -88,12 +89,12 @@ interface ArmIO{ inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) - inputs.leftPosition = Rotations.of(leftMotor.position.value) - inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) + inputs.leftPosition = Radians.of(leftMotor.position.value) + inputs.leftVelocity = RadiansPerSecond.of(leftMotor.velocity.value) - inputs.rightPosition = Rotations.of(rightMotor.position.value) - inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) + inputs.rightPosition = Radians.of(rightMotor.position.value) + inputs.rightVelocity = RadiansPerSecond.of(rightMotor.position.value) inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) @@ -165,4 +166,48 @@ interface ArmIO{ private const val PROFILE_VELOCITY = 0.0 } + } + + class ArmIOSim : ArmIO { + val armSim = SingleJointedArmSim( + DCMotor.getKrakenX60(1), + 1.0, + 1.0, + 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 From d118bf6a8f9cc6f95c245dfaeed1cc3ce81a65d6 Mon Sep 17 00:00:00 2001 From: njoy3663 Date: Wed, 20 Nov 2024 19:09:55 -0800 Subject: [PATCH 2/2] arm sim --- .../com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 6e88d04..bdc5cd1 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt @@ -89,12 +89,12 @@ interface ArmIO{ inputs.absoluteEncoderPosition = Rotations.of(absoluteEncoder.absolutePosition) - inputs.leftPosition = Radians.of(leftMotor.position.value) - inputs.leftVelocity = RadiansPerSecond.of(leftMotor.velocity.value) + inputs.leftPosition = Rotations.of(leftMotor.position.value) + inputs.leftVelocity = RotationsPerSecond.of(leftMotor.velocity.value) - inputs.rightPosition = Radians.of(rightMotor.position.value) - inputs.rightVelocity = RadiansPerSecond.of(rightMotor.position.value) + inputs.rightPosition = Rotations.of(rightMotor.position.value) + inputs.rightVelocity = RotationsPerSecond.of(rightMotor.position.value) inputs.rightCurrent = Volts.of(leftMotor.motorVoltage.value) @@ -171,8 +171,8 @@ interface ArmIO{ class ArmIOSim : ArmIO { val armSim = SingleJointedArmSim( DCMotor.getKrakenX60(1), - 1.0, - 1.0, + 3.0, + 0.0301907551, 0.331414, -0.142921, 2.32814365359,