From ae9dec409d52747266c30f00bdeffb75c9b2179f Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Thu, 18 Jan 2024 20:54:25 -0500 Subject: [PATCH] Finished Wrist --- .../config/constants/ShooterConstants.kt | 31 --- .../config/constants/WristConstants.kt | 52 ++++ .../robot2023/subsystems/Shooter/Flywheel.kt | 2 +- .../robot2023/subsystems/Shooter/Wrist.kt | 236 +++++++++++++----- .../Shooter/{ShooterIO.kt => WristIO.kt} | 3 +- .../subsystems/Shooter/WristIONeo.kt | 22 +- 6 files changed, 235 insertions(+), 111 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt rename src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/{ShooterIO.kt => WristIO.kt} (98%) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt deleted file mode 100644 index 5e813dd4..00000000 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ /dev/null @@ -1,31 +0,0 @@ -package com.team4099.robot2023.config.constants - -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.volts - - -object ShooterConstants { - // val ROLLER_GEAR_RATIO = 0.0 - // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts - // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps - - val WRIST_GEAR_RATIO = 0.0 - val WRIST_VOLTAGE_COMPENSATION = 0.0.volts - val WRIST_STATOR_CURRENT_LIMIT = 0.0.amps - - // val FEEDER_GEAR_RATIO = 0.0 - //val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts - //val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps - - //val ROLLLER_INIT_VOLTAGE = 0.0.volts - //val FEEDER_INIT_VOLTAGE = 0.0.volts - val WRIST_INIT_VOLTAGE = 0.0.volts - - //val ROLLER_SHOOTING_VOLTAGE = 0.0.volts - val WRIST_SOFTLIMIT_UPWARDSTURN = 0.0.degrees - val WRIST_SOFTLIMIT_DOWNWARDSTURN = 0.0.degrees - - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt new file mode 100644 index 00000000..342af16b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -0,0 +1,52 @@ +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.derived.* +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + + +object WristConstants { + // val ROLLER_GEAR_RATIO = 0.0 + // val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + // val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps + + val WRIST_GEAR_RATIO = 0.0 + val WRIST_VOLTAGE_COMPENSATION = 0.0.volts + val WRIST_STATOR_CURRENT_LIMIT = 0.0.amps + + 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.radians.perSecond + val WRIST_KA = 0.1.volts/1.0.radians.perSecond.perSecond + val WRIST_KS = 0.0.volts + + + // val FEEDER_GEAR_RATIO = 0.0 + //val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts + //val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps + + //val ROLLLER_INIT_VOLTAGE = 0.0.volts + //val FEEDER_INIT_VOLTAGE = 0.0.volts + val WRIST_INIT_VOLTAGE = 0.0.volts + + //val ROLLER_SHOOTING_VOLTAGE = 0.0.volts + val WRIST_SOFTLIMIT_UPWARDSTURN = 0.0.degrees + 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 WRIST_TOLERANCE = 0.01.degrees + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt index 309b473c..b944af45 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -72,7 +72,7 @@ init{ nextState = fromRequestToState(currentRequest) } - //TODO do sensor logic (mayb ask p) + //TODO do sensor logic (mayb ask pranav) Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt index 698e3a57..33015e9b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt @@ -2,21 +2,24 @@ package com.team4099.robot2023.subsystems.Shooter import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.ShooterConstants -import com.team4099.robot2023.subsystems.Shooter.WristIONeo.setWristPosition +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.controller.ArmFeedforward import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond -class Wrist (val io: ShooterIO){ - val inputs = ShooterIO.ShooterIOInputs() +class Wrist (val io: WristIO) { + val inputs = WristIO.ShooterIOInputs() //TODO do feedforward + /* private val wristkS = LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) ) @@ -27,126 +30,228 @@ class Wrist (val io: ShooterIO){ private val wristkA = LoggedTunableValue( "Wrist/kA", Pair({ it.inVoltsPerDegreePerSecond.perSecond}, { it.volts.perDegreePerSecond.perSecond }) - ) - val wristFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) + )*/ + + //val wristFeedForward = singleJointedArmFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) - override fun () + var wristFeedForward: ArmFeedforward = + ArmFeedforward( + WristConstants.WRIST_KS, + WristConstants.WRRIST_KG, + WristConstants.WRIST_KV, + WristConstants.WRIST_KA + ) - private val wristflywheelkP = - LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val wristflywheelkI = + private val wristkP = + LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })) + private val wristkI = LoggedTunableValue( - "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + "Wrist/kI", Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) ) - private val wristflywheelkD = + private val wristkD = LoggedTunableValue( - "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) + "wrist/kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) ) var currentState = ShooterStates.UNINITIALIZED + //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts - var wristTargetVoltage : ElectricalPotential = 0.0.volts - var feederTargetVoltage : ElectricalPotential = 0.0.volts - /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + var wristTargetVoltage: ElectricalPotential = 0.0.volts + var feederTargetVoltage: ElectricalPotential = 0.0.volts + + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ io.setflywheelVoltage(appliedVoltage) }*/ - fun setWristVoltage(appliedVoltage: ElectricalPotential){ + fun setWristVoltage(appliedVoltage: ElectricalPotential) { io.setWristVoltage(appliedVoltage) } + /*fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) }*/ /*var lastFlywheelRunTime = 0.0.seconds var lastFeederRunTime = 0.0.seconds*/ var lastWristRunTime = 0.0.seconds - var isZeroed : Boolean = false + var isZeroed: Boolean = false private var lastflywheelVoltage = 0.0.volts + //TODO ask what to set this too private var wristPositionTarget = 0.0.degrees private var lastWristPositionTarget = 0.0.degrees + /*private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", ShooterConstants.ROLLLER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) private var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", ShooterConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) - */private var wristInitVoltage = LoggedTunableValue ("Shooter/Initial Wrist Voltage", ShooterConstants.WRIST_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) + */private var wristInitVoltage = LoggedTunableValue( + "Shooter/Initial Wrist Voltage", + WristConstants.WRIST_INIT_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) private var timeProfileGeneratedAt = 0.0.seconds var currentRequest = Request.ShooterRequest.OpenLoop( - ShooterConstants.WRIST_INIT_VOLTAGE, + WristConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, //ShooterConstants.FEEDER_INIT_VOLTAGE + ) + private var wristConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + WristConstants.MAX_WRIST_VELOCITY, WristConstants.MAX_WRIST_ACCELERATION ) private var wristProfile = TrapezoidProfile( + //TODO lets fix this one later wristConstraints, TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) - fun setWristPosition(setPoint : TrapezoidProfile.State){ - - } -fun periodic(){ - io.updateInputs(inputs) - var nextState = currentState - when (currentState) { - ShooterStates.UNINITIALIZED -> { - nextState = fromRequestToState(currentRequest) + private var prevWristSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + val forwardLimitReached: Boolean + get() = inputs.wristPostion >= WristConstants.WRIST_MAX_ROTATION + val reverseLimitReached: Boolean + get() = inputs.wristPostion <= WristConstants.WRIST_MIN_ROTATION + + private fun isOutOfBounds(velocity: AngularVelocity): Boolean { + return (velocity > 0.0.degrees.perSecond && forwardLimitReached) || + (velocity < 0.0.degrees.perSecond && reverseLimitReached) + } + + + + private fun setWristPosition(setPoint: TrapezoidProfile.State) { + val WristAngularAcceleration = + (setPoint.velocity - prevWristSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + prevWristSetpoint = setPoint + val feedforward = + wristFeedForward.calculate(setPoint.position, setPoint.velocity, WristAngularAcceleration) + + // When the forward or reverse limit is reached, set the voltage to 0 + // Else move the arm to the setpoint position + if (isOutOfBounds(setPoint.velocity)) { + io.setWristVoltage(wristFeedForward.calculate(inputs.wristPostion, 0.degrees.perSecond)) + } else { + io.setWristPosition(setPoint.position, feedforward) } - ShooterStates.ZERO ->{ - nextState = fromRequestToState(currentRequest) + + Logger.recordOutput("Wrist/profileIsOutOfBounds", isOutOfBounds(setPoint.velocity)) + Logger.recordOutput("Wrist/armFeedForward", feedforward.inVolts) + Logger.recordOutput("Wrist/armTargetPosition", setPoint.position.inDegrees) + Logger.recordOutput("Wrist/armTargetVelocity", setPoint.velocity.inDegreesPerSecond) + + } + + val isAtTargetedPosition: Boolean + get() = + ( + currentState == ShooterStates.TARGETING_POSITION && + wristProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.wristPostion - wristPositionTarget).absoluteValue <= + WristConstants.WRIST_TOLERANCE + + ) + + fun periodic() { + io.updateInputs(inputs) + if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { + io.configWristPID(wristkP.get(), wristkI.get(), wristkD.get()) + } + Logger.processInputs("Wrist", inputs) + + Logger.recordOutput("Wrist/currentState", currentState.name) + + Logger.recordOutput("Wrist/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("Wrist/isAtTargetedPosition", isAtTargetedPosition) + + Logger.recordOutput("Wrist/isZeroed", isZeroed) + + if (Constants.Tuning.DEBUGING_MODE) { + } - ShooterStates.OPEN_LOOP ->{ - /* + var nextState = currentState + when (currentState) { + ShooterStates.UNINITIALIZED -> { + nextState = fromRequestToState(currentRequest) + } + + ShooterStates.ZERO -> { + nextState = fromRequestToState(currentRequest) + } + + ShooterStates.OPEN_LOOP -> { + /* setflywheelVoltage(flywheelTargetVoltage) lastflywheelRunTime = Clock.fpgaTime */ - setWristVoltage(wristTargetVoltage) - lastWristRunTime = Clock.fpgaTime + setWristVoltage(wristTargetVoltage) + lastWristRunTime = Clock.fpgaTime - /* setFeederVoltage(feederTargetVoltage) + /* setFeederVoltage(feederTargetVoltage) lastFeederRunTime = Clock.fpgaTime*/ - if (isZeroed == true ){ + if (isZeroed == true) { + nextState = fromRequestToState(currentRequest) + } nextState = fromRequestToState(currentRequest) + } - nextState = fromRequestToState(currentRequest) - } + ShooterStates.TARGETING_POSITION -> { - ShooterStates.TARGETING_POSITION ->{ - - if (wristPositionTarget!=lastWristPositionTarget){ - val preProfileGenerate = Clock.fpgaTime - //TODO figure out how to implment feedforward here. - wristProfile = TrapezoidProfile( - wristConstraints, - TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), - TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) - ) - val postProfileGenerate = Clock.fpgaTime - Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) - timeProfileGeneratedAt = Clock.fpgaTime - lastWristPositionTarget = wristPositionTarget + if (wristPositionTarget != lastWristPositionTarget) { + val preProfileGenerate = Clock.fpgaTime + wristProfile = TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + ) + val postProfileGenerate = Clock.fpgaTime + Logger.recordOutput( + "/Shooter/ProfileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + timeProfileGeneratedAt = Clock.fpgaTime + lastWristPositionTarget = wristPositionTarget - } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - val setPoint: TrapezoidProfile.State= wristProfile.calculate(timeElapsed) - setWristPosition(setPoint) - //TODO fix this error - Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) - nextState = fromRequestToState(currentRequest) - } + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val setPoint: TrapezoidProfile.State = wristProfile.calculate(timeElapsed) + setWristPosition(setPoint) + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) + // if we're transitioning out of targeting position, we want to make sure the next time we + // enter targeting position, we regenerate profile (even if the arm setpoint is the same as + // the previous time we ran it) + if (!(currentState.equivalentToRequest(currentRequest))) { + // setting the last target to something unreasonable so the profile is generated next loop + // cycle + lastWristPositionTarget = (-1337).degrees + } + } } } - companion object{ - enum class ShooterStates{ + + companion object { + enum class ShooterStates { UNINITIALIZED, ZERO, OPEN_LOOP, - TARGETING_POSITION, + TARGETING_POSITION; + + inline fun equivalentToRequest(request: Request.ShooterRequest): Boolean { + return ( + (request is Request.ShooterRequest.OpenLoop && this == ShooterStates.OPEN_LOOP) || + (request is Request.ShooterRequest.TargetingPosition && this == ShooterStates.TARGETING_POSITION) + ) + } + + } + inline fun fromRequestToState(request: Request.ShooterRequest): ShooterStates { return when (request) { is Request.ShooterRequest.OpenLoop -> ShooterStates.OPEN_LOOP @@ -156,7 +261,6 @@ fun periodic(){ } } } - } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIO.kt similarity index 98% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIO.kt index 0cccccd9..d72f1843 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIO.kt @@ -4,12 +4,11 @@ import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.units.base.* import org.team4099.lib.units.derived.* -import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond -interface ShooterIO { +interface WristIO { class ShooterIOInputs : LoggableInputs { var rollerVelocity = 0.rotations.perMinute var rollerAppliedVoltage = 0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt index 6ac7cff9..a71f4e22 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt @@ -5,14 +5,14 @@ import com.revrobotics.CANSparkMaxLowLevel import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.ShooterConstants +import com.team4099.robot2023.config.constants.WristConstants import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue -object WristIONeo : ShooterIO{ +object WristIONeo : WristIO{ //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, //ShooterConstants.ROLLER_GEAR_RATIO, @@ -21,8 +21,8 @@ object WristIONeo : ShooterIO{ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val wristSensor = sparkMaxAngularMechanismSensor( wristSparkMax, - ShooterConstants.WRIST_GEAR_RATIO, - ShooterConstants.WRIST_VOLTAGE_COMPENSATION + WristConstants.WRIST_GEAR_RATIO, + WristConstants.WRIST_VOLTAGE_COMPENSATION ) private val wristPIDController : SparkMaxPIDController = wristSparkMax.pidController //private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) @@ -45,14 +45,14 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID // rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) //rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) - wristSparkMax.enableVoltageCompensation(ShooterConstants.WRIST_VOLTAGE_COMPENSATION.inVolts) - wristSparkMax.setSmartCurrentLimit(ShooterConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + wristSparkMax.enableVoltageCompensation(WristConstants.WRIST_VOLTAGE_COMPENSATION.inVolts) + wristSparkMax.setSmartCurrentLimit(WristConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes.toInt()) // feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) // feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) } - override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ + override fun updateInputs (inputs: WristIO.ShooterIOInputs){ /* inputs.rollerVelocity = rollerSensor.velocity inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps @@ -98,8 +98,8 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID wristSensor.positionToRawUnits( clamp( position, - ShooterConstants.WRIST_SOFTLIMIT_DOWNWARDSTURN, - ShooterConstants.WRIST_SOFTLIMIT_UPWARDSTURN + WristConstants.WRIST_SOFTLIMIT_DOWNWARDSTURN, + WristConstants.WRIST_SOFTLIMIT_UPWARDSTURN ) ), CANSparkMax.ControlType.kPosition, @@ -112,8 +112,8 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID wristSparkMax.setVoltage( clamp( voltage, - -ShooterConstants.WRIST_VOLTAGE_COMPENSATION , - ShooterConstants.WRIST_VOLTAGE_COMPENSATION + -WristConstants.WRIST_VOLTAGE_COMPENSATION , + WristConstants.WRIST_VOLTAGE_COMPENSATION ) .inVolts )