Skip to content

Commit

Permalink
Removed flywheel and added PID constants
Browse files Browse the repository at this point in the history
  • Loading branch information
lakelandspark committed Jan 19, 2024
1 parent d5e1c2a commit 355f9bd
Show file tree
Hide file tree
Showing 4 changed files with 85 additions and 67 deletions.
27 changes: 19 additions & 8 deletions src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,28 @@
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.volts
import org.team4099.lib.units.derived.rotations
import org.team4099.lib.units.derived.perRotation
import org.team4099.lib.units.derived.ProportionalGain
import org.team4099.lib.units.derived.IntegralGain
import org.team4099.lib.units.derived.DerivativeGain
import org.team4099.lib.units.derived.Radian
import org.team4099.lib.units.derived.Volt
import org.team4099.lib.units.perMinute
import org.team4099.lib.units.perSecond

object FeederConstants {
val FLYWHEEL_VOLTAGE_COMPENSATION = 0.0.volts
val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps

val FEEDER_VOLTAGE_COMPENSATION = 0.0.volts
val FEEDER_STATOR_CURRENT_LIMIT = 0.0.amps

val FLYWHEEL_INIT_VOLTAGE = 0.0.volts
val FEEDER_INIT_VOLTAGE = 0.0.volts

val FLYWHEEL_SHOOTING_VOLTAGE = 0.0.volts
// TODO: Tune PID variables
val FEEDER_KS = 0.001.volts
val FEEDER_KV = 0.01.volts.perRotation.perMinute
val FEEDER_KA = 0.01.volts.perRotation.perMinute.perSecond

val FEEDER_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.001.volts / 1.0.rotations.perMinute
val FEEDER_KI: IntegralGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
val FEEDER_KD: DerivativeGain<Velocity<Radian>, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds)
}
70 changes: 46 additions & 24 deletions src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
View file
Original file line number Diff line number Diff line change
@@ -1,41 +1,59 @@
package com.team4099.robot2023.subsystems.feeder

import com.team4099.lib.hal.Clock
import com.team4099.lib.logging.LoggedTunableValue
import com.team4099.robot2023.config.constants.FeederConstants
import com.team4099.robot2023.config.constants.FlywheelConstants
import com.team4099.robot2023.subsystems.superstructure.Request
import edu.wpi.first.wpilibj2.command.SubsystemBase
import org.team4099.lib.controller.SimpleMotorFeedforward
import org.team4099.lib.units.base.Meter
import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.derived.ElectricalPotential
import org.team4099.lib.units.derived.Volt
import org.team4099.lib.units.derived.inVolts
import org.team4099.lib.units.derived.volts
import org.team4099.lib.units.derived.*

class Feeder(val io: FeederIO) {
class Feeder(val io: FeederIO): SubsystemBase() {
val inputs = FeederIO.FeederIOInputs()
lateinit var flywheelFeedforward: SimpleMotorFeedforward<Meter, Volt>
var currentState = FeederStates.UNINITIALIZED
var flywheelTargetVoltage : ElectricalPotential= 0.0.volts

var feederTargetVoltage : ElectricalPotential = 0.0.volts

fun setFlywheelVoltage(appliedVoltage: ElectricalPotential){
io.setFlywheelVoltage(appliedVoltage)
}
var lastFeederRunTime = 0.0.seconds
var lastFeederVoltage = 0.0.volts

var isZeroed = false

private val kP = LoggedTunableValue("Feeder/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP)
private val kI = LoggedTunableValue("Feeder/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI)
private val kD = LoggedTunableValue("Feeder/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD)

private val kS = LoggedTunableValue("Feeder/kS", Pair({it.inVoltsPerInch}, {it.volts.perInch}))
private val kV = LoggedTunableValue("Feeder/kV", Pair({it.inVoltsPerInchSeconds}, {it.volts.perInchSeconds}))
private val kA = LoggedTunableValue("Feeder/kA", Pair({it.inVoltsPerInchPerSecond}, {it.volts.perInchPerSecond}))

var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", FeederConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
var timeProfileGeneratedAt = Clock.fpgaTime

var currentState = FeederStates.UNINITIALIZED

var currentRequest: Request.FeederRequest = Request.FeederRequest.Idle()
set (value) {
feederTargetVoltage = when (value) {
is Request.FeederRequest.OpenLoop -> {
value.feederVoltage
}

is Request.FeederRequest.Idle -> {
FeederConstants.FEEDER_INIT_VOLTAGE
}
}

field = value
}

fun setFeederVoltage(appliedVoltage: ElectricalPotential){
io.setFeederVoltage(appliedVoltage)
}

var lastFlywheelRunTime = 0.0.seconds
var lastFeederRunTime = 0.0.seconds
var lastFlywheelVoltage = 0.0.volts
var lastFeederVoltage = 0.0.volts
var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial Flywheel Voltage", FeederConstants.FLYWHEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", FeederConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts}))
var timeProfileGeneratedAt = 0.0.seconds
var currentRequest = Request.FeederRequest.OpenLoop(FeederConstants.FLYWHEEL_INIT_VOLTAGE, FeederConstants.FEEDER_INIT_VOLTAGE)

fun periodic() {
override fun periodic() {
io.updateInputs(inputs)

val nextState = when (currentState) {
Expand All @@ -44,12 +62,12 @@ class Feeder(val io: FeederIO) {
}

FeederStates.IDLE -> {
setFlywheelVoltage(FeederConstants.FEEDER_INIT_VOLTAGE)
setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE)
fromRequestToState(currentRequest)
}

FeederStates.OPEN_LOOP -> {
setFlywheelVoltage(flywheelTargetVoltage)
setFeederVoltage(feederTargetVoltage)
fromRequestToState(currentRequest)
}
}
Expand All @@ -61,7 +79,11 @@ class Feeder(val io: FeederIO) {
enum class FeederStates {
UNINITIALIZED,
IDLE,
OPEN_LOOP
OPEN_LOOP;

fun equivalentToRequest(request: Request.FeederRequest): Boolean {
return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE))
}
}

fun fromRequestToState(request: Request.FeederRequest): FeederStates {
Expand Down
53 changes: 19 additions & 34 deletions src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,56 +12,41 @@ import org.team4099.lib.units.derived.volts

interface FeederIO {
class FeederIOInputs: LoggableInputs {
var floorAppliedVoltage = 0.0.volts
var floorStatorCurrent = 0.0.amps
var floorSupplyCurrent = 0.0.amps
var floorTemp = 0.0.celsius
var verticalAppliedVoltage = 0.0.volts
var verticalStatorCurrent = 0.0.amps
var verticalSupplyCurrent = 0.0.amps
var verticalTemp = 0.0.celsius
var feederAppliedVoltage = 0.0.volts
var feederStatorCurrent = 0.0.amps
var feederSupplyCurrent = 0.0.amps
var feederTemp = 0.0.celsius

var isSimulated = false

override fun toLog(table: LogTable?) {
table?.put("floorAppliedVoltage", floorAppliedVoltage.inVolts)
table?.put("floorStatorCurrent", floorStatorCurrent.inAmperes)
table?.put("floorSupplyCurrent", floorSupplyCurrent.inAmperes)
table?.put("floorTempCelcius", floorTemp.inCelsius)
table?.put("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)
table?.put("verticalStatorCurrent", verticalStatorCurrent.inAmperes)
table?.put("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)
table?.put("verticalTempCelcius", verticalTemp.inCelsius)
table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts)
table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes)
table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes)
table?.put("feederTempCelcius", feederTemp.inCelsius)
}

override fun fromLog(table: LogTable?) {
table?.get("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let {
floorAppliedVoltage = it.volts
table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let {
feederAppliedVoltage = it.volts
}
table?.get("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let {
floorStatorCurrent = it.amps
}
table?.get("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let {
floorSupplyCurrent = it.amps
}
table?.get("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius }
table?.get("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let {
verticalAppliedVoltage = it.volts

table?.get("feederStatorCurrent", feederStatorCurrent.inAmperes)?.let {
feederStatorCurrent = it.amps
}
table?.get("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let {
verticalStatorCurrent = it.amps

table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let {
feederSupplyCurrent = it.amps
}
table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let {
verticalSupplyCurrent = it.amps

table?.get("feederTempCelcius", feederTemp.inCelsius)?.let {
feederTemp = it.celsius
}
table?.get("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius }
}
}

fun updateInputs(inputs: FeederIOInputs) {}

fun setFlywheelVoltage(voltage: ElectricalPotential) {}

fun setFeederVoltage(voltage: ElectricalPotential) {}

// fun setFloorVoltage(voltage: ElectricalPotential) {}
Expand Down
2 changes: 1 addition & 1 deletion src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ sealed interface Request {
}

sealed interface FeederRequest : Request {
class OpenLoop(flywheelVoltage: ElectricalPotential, feederVoltage: ElectricalPotential): FeederRequest {}
class OpenLoop(val feederVoltage: ElectricalPotential, val flywheelVoltage: ElectricalPotential): FeederRequest {}
class Idle(): FeederRequest {}
}
}

0 comments on commit 355f9bd

Please sign in to comment.