Skip to content

Commit

Permalink
wrist sim
Browse files Browse the repository at this point in the history
  • Loading branch information
@MatthewChoulas
MatthewChoulas committed Jan 20, 2024
1 parent 5d7ab13 commit 4962cdb
Show file tree
Hide file tree
Showing 5 changed files with 245 additions and 101 deletions.
42 changes: 27 additions & 15 deletions src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
View file
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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

Expand All @@ -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

Expand All @@ -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
Expand All @@ -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<Velocity<Radian>, Volt> =
0.001.volts / 1.0.rotations.perMinute
val SHOOTER_WRIST_KI: IntegralGain<Velocity<Radian>, Volt> =
0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
val SHOOTER_WRIST_KD: DerivativeGain<Velocity<Radian>, 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<Radian, Volt> =
0.001.volts / 1.0.degrees
val REAL_KI: IntegralGain<Radian, Volt> =
0.0.volts / (1.0.degrees * 1.0.seconds)
val REAL_KD: DerivativeGain<Radian, Volt> =
0.0.volts / (1.0.rotations / 1.0.seconds)

val SIM_KP: ProportionalGain<Radian, Volt> =
0.001.volts / 1.0.degrees
val SIM_KI: IntegralGain<Radian, Volt> =
0.0.volts / (1.0.degrees * 1.0.seconds)
val SIM_KD: DerivativeGain<Radian, Volt> =
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

Expand Down
48 changes: 41 additions & 7 deletions src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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<Radian, Volt>(flywheelkS.get(), flywheelkV.get(), flywheelkA.get())
val rightFlyWheelFeedForward = SimpleMotorFeedforward<Radian, Volt>(flywheelkS.get(), flywheelkV.get(), flywheelkA.get())
var leftFlyWheelFeedForward: SimpleMotorFeedforward<Radian, Volt>
var rightFlyWheelFeedForward: SimpleMotorFeedforward<Radian, Volt>


var lastFlywheelRunTime = 0.0.seconds
Expand Down Expand Up @@ -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 -> {
Expand Down
43 changes: 29 additions & 14 deletions src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 })
Expand Down Expand Up @@ -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(
Expand Down
70 changes: 5 additions & 65 deletions src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -23,54 +19,19 @@ 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)
table.put("wristStatorCurrent", wristStatorCurrent.inAmperes)
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 {
Expand All @@ -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){
Expand All @@ -141,7 +81,7 @@ interface WristIO {

}

fun configWristPID(
fun configPID(
kP: ProportionalGain <Radian, Volt>,
kI: IntegralGain <Radian, Volt>,
kD: DerivativeGain <Radian, Volt>
Expand Down
Loading

0 comments on commit 4962cdb

Please sign in to comment.