diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index e3e10729..19680ea9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -15,6 +15,7 @@ import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inInches import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -27,6 +28,7 @@ import org.team4099.lib.units.derived.perInchSeconds import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.perSecond +import kotlin.time.Duration.Companion.seconds import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest class Elevator(val io: ElevatorIO) { @@ -49,6 +51,22 @@ class Elevator(val io: ElevatorIO) { LoggedTunableValue( "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) + private val kS = + LoggedTunableValue( + "Elevator/kS", Pair({ it.inVolts}, {it.volts}) + ) + private val kG = + LoggedTunableValue( + "Elevator/kG", Pair({ it.inVolts}, {it.volts}) + ) + private val kV = + LoggedTunableValue( + "Elevator/kG", Pair({it.inVoltsPerInchPerSecond}, {it.volts / 1.0.inches.perSecond}) + ) + private val kA = + LoggedTunableValue( + "Elevator/kA", Pair({it.inVolts.perMetersPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond}) + ) object TunableElevatorHeights { val enableElevator = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index fd3907d1..c5c97865 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -18,6 +18,7 @@ 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.base.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.ctreLinearMechanismSensor import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential @@ -156,16 +157,21 @@ object ElevatorIOKraken : ElevatorIO { } override fun setOutputVoltage(voltage: ElectricalPotential) { - elevatorLeaderKraken.setVoltage(voltage.inVolts) + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))) { + elevatorLeaderKraken.setVoltage(0.0) + } + else { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } } override fun setPosition(position: Length, feedForward: ElectricalPotential) { elevatorLeaderKraken.setControl( - PositionDutyCycle( + PositionVoltage( leaderSensor.positionToRawUnits(position), - 0.0, true, - feedForward.inVolts, + feedForward, 0, false, false, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 7aa8c0c5..d0c9eb43 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -16,6 +16,7 @@ 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.base.inPercentOutputPerSecond +import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -146,7 +147,13 @@ object ElevatorIONEO : ElevatorIO { */ override fun setOutputVoltage(voltage: ElectricalPotential) { // divide by 2 cause full power elevator is scary - leaderSparkMax.setVoltage(voltage.inVolts) + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) + ) { + leaderSparkMax.setVoltage(0.0) + } else { + leaderSparkMax.setVoltage(voltage.inVolts) + } } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index 915c87ac..db706340 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -18,6 +18,7 @@ import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inKilograms import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential @@ -99,16 +100,19 @@ object ElevatorIOSim : ElevatorIO { * @param voltage the voltage to set the motor to */ override fun setOutputVoltage(voltage: ElectricalPotential) { - val clampedVoltage = - clamp( - voltage, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - lastAppliedVoltage = clampedVoltage - - elevatorSim.setInputVoltage(clampedVoltage.inVolts) + if (!((elevatorSim.positionMeters.meters < 0.5.inches) && (voltage < 0.volts)) && + !(elevatorSim.positionMeters.meters > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) + ) { + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } } /** @@ -127,13 +131,12 @@ object ElevatorIOSim : ElevatorIO { ) val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) - lastAppliedVoltage = ff + feedback + setOutputVoltage(ff + feedback) elevatorSim.setInputVoltage((ff + feedback).inVolts) } /** set the current encoder position to be the encoders zero value */ override fun zeroEncoder() { - println("don't work right now") } /**