-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
6 changed files
with
95 additions
and
360 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
184 changes: 78 additions & 106 deletions
184
src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,138 +1,110 @@ | ||
| package com.team4099.robot2023.subsystems.intake | ||
|
|
||
| import com.team4099.lib.hal.Clock | ||
| import com.team4099.lib.logging.LoggedTunableNumber | ||
| import com.team4099.lib.logging.LoggedTunableValue | ||
| import com.team4099.robot2023.config.constants.Constants | ||
| import com.team4099.robot2023.config.constants.IntakeConstants | ||
| 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 | ||
| 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.Angle | ||
| import org.team4099.lib.units.derived.ElectricalPotential | ||
| import org.team4099.lib.units.derived.Radian | ||
| import org.team4099.lib.units.derived.degrees | ||
| import org.team4099.lib.units.derived.inDegrees | ||
| import org.team4099.lib.units.derived.inVolts | ||
| import org.team4099.lib.units.derived.inVoltsPerDegree | ||
| import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond | ||
| import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds | ||
| import org.team4099.lib.units.derived.perDegree | ||
| import org.team4099.lib.units.derived.perDegreePerSecond | ||
| import org.team4099.lib.units.derived.perDegreeSeconds | ||
| import org.team4099.lib.units.derived.volts | ||
| import org.team4099.lib.units.inDegreesPerSecond | ||
| import org.team4099.lib.units.perSecond | ||
|
|
||
| class Intake(private val io: IntakeIO) { | ||
| val inputs = IntakeIO.IntakeIOInputs() | ||
| val inputs = IntakeIO.IntakeIOInputs() | ||
|
|
||
| var rollerVoltageTarget: ElectricalPotential = 0.0.volts | ||
| var rollerVoltageTarget: ElectricalPotential = 0.0.volts | ||
|
|
||
| var isZeroed = false | ||
| var isZeroed = false | ||
|
|
||
| var lastIntakeRuntime = Clock.fpgaTime | ||
| var currentState: IntakeStake = IntakeStake.UNINITIALIZED | ||
| var currentState: IntakeStake = IntakeStake.UNINITIALIZED | ||
|
|
||
| var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() | ||
| set(value) { | ||
| rollerVoltageTarget = when (value) { | ||
| is Request.IntakeRequest.OpenLoop -> { | ||
| value.rollerVoltage | ||
| } | ||
|
|
||
| is Request.IntakeRequest.Idle -> { | ||
| IntakeConstants.IDLE_ROLLER_VOLTAGE | ||
| } | ||
| } | ||
|
|
||
| field = value | ||
| var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() | ||
| set(value) { | ||
| rollerVoltageTarget = | ||
| when (value) { | ||
| is Request.IntakeRequest.OpenLoop -> { | ||
| value.rollerVoltage | ||
| } | ||
| is Request.IntakeRequest.Idle -> { | ||
| IntakeConstants.IDLE_ROLLER_VOLTAGE | ||
| } | ||
| } | ||
|
|
||
| private var timeProfileGeneratedAt = Clock.fpgaTime | ||
|
|
||
| fun periodic() { | ||
| io.updateInputs(inputs) | ||
|
|
||
| Logger.processInputs("GroundIntake", inputs) | ||
| field = value | ||
| } | ||
|
|
||
| Logger.recordOutput("GroundIntake/currentState", currentState.name) | ||
| fun periodic() { | ||
| io.updateInputs(inputs) | ||
|
|
||
| Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) | ||
| Logger.processInputs("Intake", inputs) | ||
|
|
||
| Logger.recordOutput("GroundIntake/isZeroed", isZeroed) | ||
| Logger.recordOutput("Intake/currentState", currentState.name) | ||
|
|
||
| if (Constants.Tuning.DEBUGING_MODE) { | ||
| Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) | ||
| Logger.recordOutput("Intake/requestedState", currentRequest.javaClass.simpleName) | ||
|
|
||
| Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) | ||
| Logger.recordOutput("Intake/isZeroed", isZeroed) | ||
|
|
||
| Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) | ||
| } | ||
| if (Constants.Tuning.DEBUGING_MODE) { | ||
| Logger.recordOutput( | ||
| "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) | ||
| ) | ||
|
|
||
| var nextState = currentState | ||
| when (currentState) { | ||
| IntakeStake.UNINITIALIZED -> { | ||
| // Outputs | ||
| // No designated output functionality because targeting position will take care of it next | ||
| // loop cycle | ||
|
|
||
| // Transitions | ||
| nextState = IntakeStake.IDLE | ||
| } | ||
| IntakeStake.IDLE -> { | ||
| setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) | ||
|
|
||
| // Transitions | ||
| nextState = fromRequestToState(currentRequest) | ||
| } | ||
| IntakeStake.OPEN_LOOP_REQUEST -> { | ||
| setRollerVoltage(rollerVoltageTarget) | ||
|
|
||
| // Transitions | ||
| nextState = fromRequestToState(currentRequest) | ||
| } | ||
| } | ||
|
|
||
| // The next loop cycle, we want to run ground intake at the state that was requested. setting | ||
| // current state to the next state ensures that we run the logic for the state we want in the | ||
| // next loop cycle. | ||
| currentState = nextState | ||
| Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) | ||
| } | ||
|
|
||
| /** @param appliedVoltage Represents the applied voltage of the roller motor */ | ||
| fun setRollerVoltage(appliedVoltage: ElectricalPotential) { | ||
| io.setRollerVoltage(appliedVoltage) | ||
| var nextState = currentState | ||
| when (currentState) { | ||
| IntakeStake.UNINITIALIZED -> { | ||
| // Outputs | ||
| // No designated output functionality because targeting position will take care of it next | ||
| // loop cycle | ||
|
|
||
| // Transitions | ||
| nextState = IntakeStake.IDLE | ||
| } | ||
| IntakeStake.IDLE -> { | ||
| setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) | ||
|
|
||
| // Transitions | ||
| nextState = fromRequestToState(currentRequest) | ||
| } | ||
| IntakeStake.OPEN_LOOP -> { | ||
| setRollerVoltage(rollerVoltageTarget) | ||
|
|
||
| // Transitions | ||
| nextState = fromRequestToState(currentRequest) | ||
| } | ||
| } | ||
|
|
||
| fun zeroArm() { | ||
| io.zeroEncoder() | ||
| // The next loop cycle, we want to run ground intake at the state that was requested. setting | ||
| // current state to the next state ensures that we run the logic for the state we want in the | ||
| // next loop cycle. | ||
| currentState = nextState | ||
| } | ||
|
|
||
| /** @param appliedVoltage Represents the applied voltage of the roller motor */ | ||
| fun setRollerVoltage(appliedVoltage: ElectricalPotential) { | ||
| io.setRollerVoltage(appliedVoltage) | ||
| } | ||
|
|
||
| companion object { | ||
| enum class IntakeStake { | ||
| UNINITIALIZED, | ||
| IDLE, | ||
| OPEN_LOOP; | ||
|
|
||
| inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { | ||
| return ( | ||
| (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || | ||
| (request is Request.IntakeRequest.Idle && this == IDLE) | ||
| ) | ||
| } | ||
| } | ||
|
|
||
| companion object { | ||
| enum class IntakeStake { | ||
| UNINITIALIZED, | ||
| IDLE, | ||
| OPEN_LOOP_REQUEST; | ||
|
|
||
| inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { | ||
| return ( | ||
| (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || | ||
| (request is Request.IntakeRequest.Idle && this == IDLE) | ||
| ) | ||
| } | ||
| } | ||
|
|
||
| inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { | ||
| return when (request) { | ||
| is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST | ||
| is Request.IntakeRequest.Idle -> IntakeStake.IDLE | ||
| } | ||
| } | ||
| inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { | ||
| return when (request) { | ||
| is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP | ||
| is Request.IntakeRequest.Idle -> IntakeStake.IDLE | ||
| } | ||
| } | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.