From c4c3d3006ffd711ec0be195f0f351b299ac70380 Mon Sep 17 00:00:00 2001 From: Nikola Jenkins Date: Tue, 21 Jan 2025 20:06:43 -0800 Subject: [PATCH 1/4] elevator simulation --- .../kotlin/com/frcteam3636/frc2025/Robot.kt | 2 +- .../frc2025/subsystems/elevator/ElevatorIO.kt | 89 ++++++++++++++++++- 2 files changed, 89 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt index 545e244..fb36751 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt @@ -43,7 +43,7 @@ import kotlin.io.path.exists * renaming the object or package, it will get changed everywhere.) */ object Robot : LoggedRobot() { - private val controller = CommandXboxController(2) + val controller = CommandXboxController(2) private val joystickLeft = Joystick(0) private val joystickRight = Joystick(1) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt index 4c003d7..73312ba 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt @@ -8,21 +8,36 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.ctre.phoenix6.signals.SensorDirectionValue +import com.ctre.phoenix6.sim.TalonFXSimState +import com.frcteam3636.frc2025.CANcoder import com.frcteam3636.frc2025.CTREDeviceId +import com.frcteam3636.frc2025.Robot +import com.frcteam3636.frc2025.Robot.controller import com.frcteam3636.frc2025.TalonFX -import com.frcteam3636.frc2025.CANcoder +import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain +import com.frcteam3636.frc2025.subsystems.elevator.ElevatorIOReal.Constants import com.frcteam3636.frc2025.utils.math.MotorFFGains import com.frcteam3636.frc2025.utils.math.PIDGains import com.frcteam3636.frc2025.utils.math.motorFFGains import com.frcteam3636.frc2025.utils.math.pidGains +import edu.wpi.first.math.controller.ElevatorFeedforward +import edu.wpi.first.math.controller.ProfiledPIDController +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.math.system.plant.LinearSystemId +import edu.wpi.first.math.trajectory.TrapezoidProfile import edu.wpi.first.units.Units.* import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.LinearVelocity import edu.wpi.first.units.measure.Voltage +import edu.wpi.first.wpilibj.Encoder +import edu.wpi.first.wpilibj.RobotController +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax +import edu.wpi.first.wpilibj.simulation.* import org.littletonrobotics.junction.Logger import org.team9432.annotation.Logged + @Logged open class ElevatorInputs { var height = Meters.zero()!! @@ -120,3 +135,75 @@ class ElevatorIOReal: ElevatorIO { } } + +class ElevatorIOSim: ElevatorIO { + // Simulation classes help us simulate what's going on, including gravity. + var motor = DCMotor.getKrakenX60Foc(2) + var motorSim = DCMotorSim( + LinearSystemId.createDCMotorSystem(motor,0.0000763789,25.0), + motor, + 0.0 + ) + + private val elevatorSim: ElevatorSim = ElevatorSim( + motor, + GEAR_RATIO, + CARRIAGE_MASS, + DRUM_RADIUS, + MIN_HEIGHT, + MAX_HEIGHT, + true, + 0.01, + 0.0 + ) + + var controller = ProfiledPIDController( + PID_GAINS.p, + PID_GAINS.i, + PID_GAINS.d, + TRAPEZOID_CONSTRAINTS + ) + + var feedforward = ElevatorFeedforward( + FF_GAINS.s, + FF_GAINS.v, + FF_GAINS.a + ) + + var encoder = Encoder(0,1) + var encoderSim = EncoderSim(encoder) + + override fun updateInputs(inputs: ElevatorInputs) { + elevatorSim.setInput(motorSim.angularVelocity * RobotController.getBatteryVoltage()) + elevatorSim.update(.020) + encoderSim.setDistance(elevatorSim.positionMeters) + RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps)) + } + + override fun runToHeight(height: Distance) { + var pidOutput = controller.calculate(encoder.distance) + var feedforwardOutput = feedforward.calculate(controller.setpoint.velocity) + motor.setVoltage(pidOutput + feedforwardOutput) + } + + override fun setVoltage(volts: Voltage) { + elevatorSim.setInputVoltage(volts.`in`(Volts)) + Logger.recordOutput("/Elevator/OutVolt", volts) + } + + internal companion object Constants { + val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!! + private const val GEAR_RATIO = 0.0 + private const val CARRIAGE_MASS = 1.62519701308126 + private const val MIN_HEIGHT = 0.254000 + private const val MAX_HEIGHT = 2.298700 + private const val DRUM_RADIUS = 0.028575/2 + val PID_GAINS = PIDGains(0.0, 0.0, 0.0) + val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0) + var TRAPEZOID_CONSTRAINTS = TrapezoidProfile.Constraints(1.0, 1.0) + private const val GRAVITY_GAIN = 0.0 + private const val PROFILE_ACCELERATION = 0.0 + private const val PROFILE_JERK = 0.0 + private const val PROFILE_VELOCITY = 0.0 + } +} \ No newline at end of file From 4931176afaece54d3c848aabe965699e2b875266 Mon Sep 17 00:00:00 2001 From: Nikola Jenkins Date: Wed, 22 Jan 2025 16:49:35 -0800 Subject: [PATCH 2/4] elevator simulation --- .../Build___Run_Simulate_Java.xml | 2 +- .../frc2025/subsystems/elevator/Elevator.kt | 16 +++---- .../frc2025/subsystems/elevator/ElevatorIO.kt | 47 +++++++------------ 3 files changed, 27 insertions(+), 38 deletions(-) diff --git a/.idea/runConfigurations/Build___Run_Simulate_Java.xml b/.idea/runConfigurations/Build___Run_Simulate_Java.xml index 595cb5b..0adc18d 100644 --- a/.idea/runConfigurations/Build___Run_Simulate_Java.xml +++ b/.idea/runConfigurations/Build___Run_Simulate_Java.xml @@ -4,7 +4,7 @@ diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt index 65b77c9..9aa163f 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt @@ -9,7 +9,7 @@ import org.littletonrobotics.junction.Logger object Elevator: Subsystem { private val io: ElevatorIO = when (Robot.model) { - Robot.Model.SIMULATION -> TODO() + Robot.Model.SIMULATION -> ElevatorIOSim() Robot.Model.COMPETITION -> ElevatorIOReal() Robot.Model.PROTOTYPE -> TODO() } @@ -35,12 +35,12 @@ object Elevator: Subsystem { // sysID.dynamic(direction)!! enum class Position(val height: Distance) { - Stowed(Meters.of(0.0)), - Trough(Meters.of(0.0)), - LowBar(Meters.of(0.0)), - MidBar(Meters.of(0.0)), - HighBar(Meters.of(0.0)), - LowAlgae(Meters.of(0.0)), - HighAlgae(Meters.of(0.0)), + Stowed(Meters.of(0.254000)), + Trough(Meters.of(0.254000)), + LowBar(Meters.of(0.050800)), + MidBar(Meters.of(0.254000)), + HighBar(Meters.of(1.219200)), +// LowAlgae(Meters.of(0.0)), +// HighAlgae(Meters.of(0.0)), } } \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt index 73312ba..dbdaf78 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt @@ -41,8 +41,8 @@ import org.team9432.annotation.Logged @Logged open class ElevatorInputs { var height = Meters.zero()!! - var rightCurrent = Volts.zero()!! - var leftCurrent = Volts.zero()!! + var rightCurrent = Amps.zero()!! + var leftCurrent = Amps.zero()!! var velocity = MetersPerSecond.zero()!! } @@ -104,8 +104,8 @@ class ElevatorIOReal: ElevatorIO { override fun updateInputs(inputs: ElevatorInputs) { inputs.height = (DISTANCE_PER_TURN * encoder.position.value) as Distance inputs.velocity = (DISTANCE_PER_TURN * encoder.velocity.value) as LinearVelocity - inputs.rightCurrent = rightElevatorMotor.motorVoltage.value - inputs.leftCurrent = leftElevatorMotor.motorVoltage.value + inputs.rightCurrent = rightElevatorMotor.torqueCurrent.value + inputs.leftCurrent = leftElevatorMotor.torqueCurrent.value } override fun runToHeight(height: Distance) { @@ -138,12 +138,7 @@ class ElevatorIOReal: ElevatorIO { class ElevatorIOSim: ElevatorIO { // Simulation classes help us simulate what's going on, including gravity. - var motor = DCMotor.getKrakenX60Foc(2) - var motorSim = DCMotorSim( - LinearSystemId.createDCMotorSystem(motor,0.0000763789,25.0), - motor, - 0.0 - ) + private var motor: DCMotor = DCMotor.getKrakenX60Foc(2) private val elevatorSim: ElevatorSim = ElevatorSim( motor, @@ -157,33 +152,32 @@ class ElevatorIOSim: ElevatorIO { 0.0 ) - var controller = ProfiledPIDController( + private var controller = ProfiledPIDController( PID_GAINS.p, PID_GAINS.i, PID_GAINS.d, TRAPEZOID_CONSTRAINTS ) - var feedforward = ElevatorFeedforward( + private var feedforward = ElevatorFeedforward( FF_GAINS.s, FF_GAINS.v, FF_GAINS.a ) - var encoder = Encoder(0,1) - var encoderSim = EncoderSim(encoder) - override fun updateInputs(inputs: ElevatorInputs) { - elevatorSim.setInput(motorSim.angularVelocity * RobotController.getBatteryVoltage()) - elevatorSim.update(.020) - encoderSim.setDistance(elevatorSim.positionMeters) + elevatorSim.update(Robot.period) + inputs.height = Meters.of(elevatorSim.positionMeters) + inputs.velocity = MetersPerSecond.of(elevatorSim.velocityMetersPerSecond) + inputs.leftCurrent = Amps.of(elevatorSim.currentDrawAmps) + inputs.rightCurrent = Amps.of(elevatorSim.currentDrawAmps) RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps)) } override fun runToHeight(height: Distance) { - var pidOutput = controller.calculate(encoder.distance) - var feedforwardOutput = feedforward.calculate(controller.setpoint.velocity) - motor.setVoltage(pidOutput + feedforwardOutput) + val pidOutput = controller.calculate(elevatorSim.positionMeters) + val feedforwardOutput = feedforward.calculate(controller.setpoint.velocity) + elevatorSim.setInputVoltage(pidOutput + feedforwardOutput) } override fun setVoltage(volts: Voltage) { @@ -192,18 +186,13 @@ class ElevatorIOSim: ElevatorIO { } internal companion object Constants { - val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!! - private const val GEAR_RATIO = 0.0 + private const val GEAR_RATIO = 1.0 private const val CARRIAGE_MASS = 1.62519701308126 private const val MIN_HEIGHT = 0.254000 private const val MAX_HEIGHT = 2.298700 private const val DRUM_RADIUS = 0.028575/2 val PID_GAINS = PIDGains(0.0, 0.0, 0.0) - val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0) - var TRAPEZOID_CONSTRAINTS = TrapezoidProfile.Constraints(1.0, 1.0) - private const val GRAVITY_GAIN = 0.0 - private const val PROFILE_ACCELERATION = 0.0 - private const val PROFILE_JERK = 0.0 - private const val PROFILE_VELOCITY = 0.0 + val FF_GAINS = MotorFFGains(0.0, 2.77, 0.05) + var TRAPEZOID_CONSTRAINTS = TrapezoidProfile.Constraints(4.16, 8.2119724) } } \ No newline at end of file From fe27a3392bf9ac1b4859ac848adec1ad89649ff3 Mon Sep 17 00:00:00 2001 From: Nikola Jenkins Date: Wed, 22 Jan 2025 16:55:37 -0800 Subject: [PATCH 3/4] elevator simulation --- src/main/kotlin/com/frcteam3636/frc2025/Robot.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt index fb36751..477b5a9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt @@ -2,6 +2,7 @@ package com.frcteam3636.frc2025 import com.ctre.phoenix6.StatusSignal import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain +import com.frcteam3636.frc2025.subsystems.elevator.Elevator import com.frcteam3636.frc2025.utils.Elastic import com.frcteam3636.frc2025.utils.ElasticNotification import com.frcteam3636.frc2025.utils.NotificationLevel @@ -127,6 +128,7 @@ object Robot : LoggedRobot() { /** Start robot subsystems so that their periodic tasks are run */ private fun configureSubsystems() { Drivetrain.register() + Elevator.register() } /** Expose commands for autonomous routines to use and display an auto picker in Shuffleboard. */ From 9f7255bdfc46b7690373df910a0cdd913d3d0e00 Mon Sep 17 00:00:00 2001 From: Nikola Jenkins Date: Wed, 22 Jan 2025 17:09:23 -0800 Subject: [PATCH 4/4] elevator simulation --- src/main/kotlin/com/frcteam3636/frc2025/Robot.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt index 477b5a9..3b8bf8e 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt @@ -44,7 +44,7 @@ import kotlin.io.path.exists * renaming the object or package, it will get changed everywhere.) */ object Robot : LoggedRobot() { - val controller = CommandXboxController(2) + private val controller = CommandXboxController(2) private val joystickLeft = Joystick(0) private val joystickRight = Joystick(1)