From 4962cdb0316edf0d8bc239546191dc0d621fbd87 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sat, 20 Jan 2024 18:20:31 -0500 Subject: [PATCH] wrist sim --- .../config/constants/WristConstants.kt | 42 +++-- .../robot2023/subsystems/flywheel/Flywheel.kt | 48 +++++- .../robot2023/subsystems/wrist/Wrist.kt | 43 ++++-- .../robot2023/subsystems/wrist/WristIO.kt | 70 +-------- .../robot2023/subsystems/wrist/WristIOSim.kt | 143 ++++++++++++++++++ 5 files changed, 245 insertions(+), 101 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 94bb7e49..84e4314c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -1,8 +1,7 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.base.* import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.DerivativeGain @@ -12,6 +11,7 @@ import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.volts import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.kilo import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond @@ -21,8 +21,10 @@ object WristConstants { // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps - + val VOLTAGE_COMPENSATION = 0.volts val ABSOLUTE_ENCODER_OFFSET = 0.degrees + val WRIST_LENGHT = 0.0.inches + val WRIST_INERTIA = 0.0.kilo.grams * 1.0.meters.squared val WRIST_ENCODER_GEAR_RATIO = 0.0 @@ -32,10 +34,7 @@ object WristConstants { val WRIST_MAX_ROTATION = 0.0.degrees val WRIST_MIN_ROTATION = 0.0.degrees - val WRRIST_KG = 0.0.volts - val WRIST_KV = 0.volts/1.0.degrees.perSecond - val WRIST_KA = 0.1.volts/1.0.degrees.perSecond.perSecond - val WRIST_KS = 0.0.volts + // val FEEDER_GEAR_RATIO = 0.0 @@ -51,14 +50,27 @@ object WristConstants { val WRIST_SOFTLIMIT_DOWNWARDSTURN = 0.0.degrees val MAX_WRIST_VELOCITY = 0.0.radians.perSecond - val MAX_WRIST_ACCELERATION =0.0.radians.perMinute.perSecond - - val SHOOTER_WRIST_KP: ProportionalGain, Volt> = - 0.001.volts / 1.0.rotations.perMinute - val SHOOTER_WRIST_KI: IntegralGain, Volt> = - 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val SHOOTER_WRIST_KD: DerivativeGain, Volt> = - 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + val MAX_WRIST_ACCELERATION = 0.0.radians.perMinute.perSecond + object PID { + val REAL_KP: ProportionalGain = + 0.001.volts / 1.0.degrees + val REAL_KI: IntegralGain = + 0.0.volts / (1.0.degrees * 1.0.seconds) + val REAL_KD: DerivativeGain = + 0.0.volts / (1.0.rotations / 1.0.seconds) + + val SIM_KP: ProportionalGain = + 0.001.volts / 1.0.degrees + val SIM_KI: IntegralGain = + 0.0.volts / (1.0.degrees * 1.0.seconds) + val SIM_KD: DerivativeGain = + 0.0.volts / (1.0.rotations / 1.0.seconds) + + val WRIST_KG = 0.0.volts + val WRIST_KV = 0.volts/1.0.degrees.perSecond + val WRIST_KA = 0.1.volts/1.0.degrees.perSecond.perSecond + val WRIST_KS = 0.0.volts + } val WRIST_TOLERANCE = 0.01.degrees diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index fbab424c..81418a1e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -3,7 +3,10 @@ package com.team4099.robot2023.subsystems.flywheel import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import org.team4099.lib.controller.ArmFeedforward import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.base.seconds import org.team4099.lib.units.perMinute @@ -27,18 +30,18 @@ class Flywheel (val io: FlywheelIO) { val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVolts }, { it.volts}) + LoggedTunableValue("Flywheel/kS", FlywheelConstants.PID.FLYWHEEL_KS, Pair({ it.inVolts }, { it.volts}) ) private val flywheelkV = LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts/ 1.0.rotations.perMinute }) + "Flywheel/kV", FlywheelConstants.PID.FLYWHEEL_KV, Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts/ 1.0.rotations.perMinute }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVoltsPerRotationsPerMinutePerSecond}, { it.volts/ 1.0.rotations.perMinute.perSecond }) + "Flywheel/kA", FlywheelConstants.PID.FLYWHEEL_KA, Pair({ it.inVoltsPerRotationsPerMinutePerSecond}, { it.volts/ 1.0.rotations.perMinute.perSecond }) ) - val leftFlyWheelFeedForward = SimpleMotorFeedforward(flywheelkS.get(), flywheelkV.get(), flywheelkA.get()) - val rightFlyWheelFeedForward = SimpleMotorFeedforward(flywheelkS.get(), flywheelkV.get(), flywheelkA.get()) + var leftFlyWheelFeedForward: SimpleMotorFeedforward + var rightFlyWheelFeedForward: SimpleMotorFeedforward var lastFlywheelRunTime = 0.0.seconds @@ -68,14 +71,45 @@ class Flywheel (val io: FlywheelIO) { } init{ - //TODO figure out what else needs to run in the init function + if (RobotBase.isReal()) { + kP.initDefault(FlywheelConstants.PID.REAL_KP) + kI.initDefault(FlywheelConstants.PID.REAL_KI) + kD.initDefault(FlywheelConstants.PID.REAL_KD) + } else { + kP.initDefault(FlywheelConstants.PID.SIM_KP) + kI.initDefault(FlywheelConstants.PID.SIM_KI) + kD.initDefault(FlywheelConstants.PID.SIM_KD) -} + } + + leftFlyWheelFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.FLYWHEEL_KS, + FlywheelConstants.PID.FLYWHEEL_KV, + FlywheelConstants.PID.FLYWHEEL_KA + ) + + rightFlyWheelFeedForward = + SimpleMotorFeedforward( + FlywheelConstants.PID.FLYWHEEL_KS, + FlywheelConstants.PID.FLYWHEEL_KV, + FlywheelConstants.PID.FLYWHEEL_KA + ) + } fun periodic(){ io.updateInputs(inputs) if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { io.configLeftPID(kP.get(), kI.get(), kD.get()) } + + if(flywheelkA.hasChanged()||flywheelkV.hasChanged()||flywheelkS.hasChanged()){ + leftFlyWheelFeedForward = SimpleMotorFeedforward( + flywheelkS.get(), + flywheelkV.get(), + flywheelkA.get() + ) + } + var nextState = currentState when (currentState) { Companion.FlywheelStates.UNINITIALIZED -> { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 7ab99c62..947645e9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -5,6 +5,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ArmFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -17,30 +18,24 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond class Wrist (val io: WristIO) { - val inputs = WristIO.ShooterIOInputs() + val inputs = WristIO.WristIOInputs() private val wristkS = - LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) + LoggedTunableValue("Wrist/kS",WristConstants.PID.WRIST_KS, Pair({ it.inVolts }, { it.volts}) ) private val wristkV = LoggedTunableValue( - "Wrist/kV", Pair({ it.inVoltsPerDegreePerSecond}, { it.volts.perDegreePerSecond }) + "Wrist/kV", WristConstants.PID.WRIST_KV, Pair({ it.inVoltsPerDegreePerSecond}, { it.volts.perDegreePerSecond }) ) private val wristkA = LoggedTunableValue( - "Wrist/kA", Pair({ it.inVoltsPerDegreePerSecondPerSecond}, { it.volts.perDegreePerSecondPerSecond })) - private val wristkG = LoggedTunableValue("Wrist/kG", Pair({ it.inVolts }, { it.volts} )) - - var wristFeedForward: ArmFeedforward = ArmFeedforward( - wristkS.get(), - wristkG.get(), - wristkV.get(), - wristkA.get() - ) + "Wrist/kA", WristConstants.PID.WRIST_KA, Pair({ it.inVoltsPerDegreePerSecondPerSecond}, { it.volts.perDegreePerSecondPerSecond })) + private val wristkG = LoggedTunableValue("Wrist/kG", WristConstants.PID.WRIST_KG, Pair({ it.inVolts }, { it.volts} )) + var wristFeedForward: ArmFeedforward private val wristkP = - LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) + LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) private val wristkI = LoggedTunableValue( "Wrist/kI", Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) @@ -103,11 +98,31 @@ class Wrist (val io: WristIO) { } + init { + if (RobotBase.isReal()) { + wristkP.initDefault(WristConstants.PID.REAL_KP) + wristkI.initDefault(WristConstants.PID.REAL_KI) + wristkD.initDefault(WristConstants.PID.REAL_KD) + } else { + wristkP.initDefault(WristConstants.PID.SIM_KP) + wristkI.initDefault(WristConstants.PID.SIM_KI) + wristkD.initDefault(WristConstants.PID.SIM_KD) + + } + + wristFeedForward = + ArmFeedforward( + WristConstants.PID.WRIST_KS, + WristConstants.PID.WRIST_KG, + WristConstants.PID.WRIST_KV, + WristConstants.PID.WRIST_KA + ) + } fun periodic() { io.updateInputs(inputs) if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { - io.configWristPID(wristkP.get(), wristkI.get(), wristkD.get()) + io.configPID(wristkP.get(), wristkI.get(), wristkD.get()) } if(wristkA.hasChanged()||wristkV.hasChanged()||wristkG.hasChanged()||wristkS.hasChanged()){ wristFeedForward = ArmFeedforward( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt index dd375c18..7bf1887a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -9,12 +9,8 @@ import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond interface WristIO { - class ShooterIOInputs : LoggableInputs { - var rollerVelocity = 0.rotations.perMinute - var rollerAppliedVoltage = 0.volts - var rollerStatorCurrent = 0.amps - var rollerSupplyCurrent = 0.amps - var rollerTempreature = 0.celsius + class WristIOInputs : LoggableInputs { + var wristPostion = 0.degrees var wristVelocity = 0.radians.perSecond @@ -23,19 +19,9 @@ interface WristIO { var wristSupplyCurrent = 0.amps var wristTemperature = 0.celsius - var feederVelocity = 0.rotations.perMinute - var feederAppliedVoltage = 0.volts - var feederStatorCurrent = 0.amps - var feederSupplyCurrent = 0.amps - var feederTemperature = 0.celsius + var isSimulated = false override fun toLog(table: LogTable) { - table.put("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond) - table.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) - table.put("rollerStatorCurrent", rollerStatorCurrent.inAmperes) - table.put("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes) - table.put("rollerTempreature", rollerTempreature.inCelsius) - table.put("wristPostion", wristPostion.inDegrees) table.put("wristVelocityRPM", wristVelocity.inRadiansPerSecond) table.put("wristAppliedVoltage", wristAppliedVoltage.inVolts) @@ -43,34 +29,9 @@ interface WristIO { table.put("wristSupplyCurrent", wristSupplyCurrent.inAmperes) table.put("wristTemperature", wristTemperature.inCelsius) - table.put("feederVelocityRPM", feederVelocity.inRadiansPerSecond) - table.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) - table.put("feederStatorCurrent", feederStatorCurrent.inAmperes) - table.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) - table.put("feederTemperature", feederTemperature.inCelsius) - } override fun fromLog(table: LogTable) { - //roller logs - table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let { - rollerVelocity = it.radians.perSecond - } - table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let { - rollerAppliedVoltage = it.volts - } - table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let { - rollerStatorCurrent = it.amps - } - table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let { - rollerSupplyCurrent = it.amps - - } - table.get("rollerTempreature", rollerTempreature.inCelsius).let { - rollerTempreature = it.celsius - } - - //wrist logs table.get("wristPostion", wristPostion.inDegrees).let { @@ -92,31 +53,10 @@ interface WristIO { table.get("wristTemperature", wristTemperature.inCelsius).let { wristTemperature = it.celsius } - - - - //feeder - table.get("feederVelocity", feederVelocity.inRadiansPerSecond).let { - feederVelocity = it.radians.perSecond - } - table.get("feederAppliedVoltage", feederAppliedVoltage.inVolts).let { - feederAppliedVoltage = it.volts - } - table.get("feederStatorCurrent", feederStatorCurrent.inAmperes).let { - feederStatorCurrent = it.amps - } - table.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes).let { - feederSupplyCurrent = it.amps - - } - table.get("feederTemperature", feederTemperature.inCelsius).let { - feederTemperature = it.celsius - } } -} - fun updateInputs(inputs: ShooterIOInputs){ + fun updateInputs(inputs: WristIOInputs){ } /*fun setRollerVoltage (voltage: ElectricalPotential){ @@ -141,7 +81,7 @@ interface WristIO { } - fun configWristPID( + fun configPID( kP: ProportionalGain , kI: IntegralGain , kD: DerivativeGain diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt new file mode 100644 index 00000000..e6c848c7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt @@ -0,0 +1,143 @@ +package com.team4099.robot2023.subsystems.wrist + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.WristConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object WristIOSim: WristIO { + + val wristSim = + SingleJointedArmSim( + DCMotor.getNEO(1), + WristConstants.WRIST_GEAR_RATIO, + WristConstants.WRIST_INERTIA.inKilogramsMeterSquared, + WristConstants.WRIST_LENGHT.inMeters, + WristConstants.WRIST_MIN_ROTATION.inRadians, + WristConstants.WRIST_MAX_ROTATION.inRadians, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Ground Intake", + "Rotation", + MotorCollection( + mutableListOf( + SimulatedMotor( + wristSim, + "Arm Motor", + ), + ), + 60.amps, + 10.celsius, + 45.amps, + 20.celsius + ) + ) + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf( + SimulatedMotor( + wristSim, + "Roller Motor", + ) + ), + 60.amps, + 10.celsius, + 45.amps, + 20.celsius + ) + ) + } + + private val wristController = + PIDController( + WristConstants.PID.SIM_KP, + WristConstants.PID.SIM_KI, + WristConstants.PID.SIM_KD + ) + + val appliedVoltage = 0.volts + + override fun updateInputs(inputs: WristIO.WristIOInputs) { + wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.wristPostion = wristSim.angleRads.radians + inputs.wristVelocity = wristSim.velocityRadPerSec.radians.perSecond + inputs.wristSupplyCurrent = 0.amps + inputs.wristAppliedVoltage = 0.volts + inputs.wristStatorCurrent = wristSim.currentDrawAmps.amps + inputs.wristTemperature = 0.0.celsius + + inputs.isSimulated = true + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setWristVoltage(voltage: ElectricalPotential) { + wristSim.setInputVoltage( + clamp( + voltage, + -WristConstants.VOLTAGE_COMPENSATION, + WristConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + } + + override fun setWristPosition(wristPosition: Angle, feedforward: ElectricalPotential) { + val feedback = wristController.calculate(wristSim.angleRads.radians, wristPosition) + setWristVoltage(feedback + feedforward) + } + + /** + * Updates the PID constants using the implementation controller, uses arm sensor to convert from + * PID constants to motor controller units + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + wristController.setPID(kP, kI, kD) + } + + + /** recalculates the current position of the neo encoder using value from the absolute encoder */ + override fun zeroEncoder() {} +} \ No newline at end of file