From 99853d7ddb3165eaf0ba9d82187f21f6b159050c Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Tue, 9 Jan 2024 18:26:29 -0500 Subject: [PATCH 01/58] Revert "create shooter branch" --- .../com/team4099/robot2023/subsystems/Shooter/Shooter.kt | 4 ---- 1 file changed, 4 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt deleted file mode 100644 index df493fe8..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -class Shooter { -} \ No newline at end of file From 3e06a122a79c7e063580fa9de6bc8237cc5524d5 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Tue, 9 Jan 2024 18:32:02 -0500 Subject: [PATCH 02/58] Revert "create feeder branch" --- .../com/team4099/robot2023/subsystems/feeder/Feeder.kt | 6 ------ .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 4 ---- 2 files changed, 10 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt deleted file mode 100644 index bb51b835..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ /dev/null @@ -1,6 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt deleted file mode 100644 index 41ddbf24..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -interface FeederIO { -} \ No newline at end of file From 9f55d221a9866645bd9a7b5ac0e0f428c1b68e4f Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Tue, 9 Jan 2024 19:00:46 -0500 Subject: [PATCH 03/58] Added Feeder IO File --- build.gradle | 4 +- .../robot2023/subsystems/feeder/FeederIO.kt | 66 +++++++++++++++++++ 2 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/build.gradle b/build.gradle index a21179a3..1f2fe330 100644 --- a/build.gradle +++ b/build.gradle @@ -84,9 +84,9 @@ dependencies { implementation 'org.jetbrains.kotlin:kotlin-test-junit5' implementation 'com.github.team4099:FalconUtils:1.1.26' - implementation 'org.apache.commons:commons-collections4:4.0' + implementation 'org.apache.commons:commons-collections4:4.4' implementation 'com.google.code.gson:gson:2.10.1' - implementation "io.javalin:javalin:5.3.2" + implementation 'io.javalin:javalin:5.4.2' // We need to add the Kotlin stdlib in order to use most Kotlin language features. // compile "org.jetbrains.kotlin:kotlin-stdlib" diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt new file mode 100644 index 00000000..15eec3c3 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -0,0 +1,66 @@ +package com.team4099.robot2023.subsystems.feeder + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +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 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) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { + floorAppliedVoltage = it.volts + } + table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { + floorStatorCurrent = it.amps + } + table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { + floorSupplyCurrent = it.amps + } + table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { + verticalAppliedVoltage = it.volts + } + table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + verticalStatorCurrent = it.amps + } + table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + verticalSupplyCurrent = it.amps + } + table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } + } + + fun updateInputs(inputs: FeederIOInputs) {} + + fun setFloorVoltage(voltage: ElectricalPotential) {} + + fun setVerticalVoltage(voltage: ElectricalPotential) {} + } +} \ No newline at end of file From 21f0652e04955bf7a305cda561cf998388893c9d Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Wed, 10 Jan 2024 18:13:36 -0500 Subject: [PATCH 04/58] Started IO File --- .../robot2023/subsystems/Shooter/ShooterIO.kt | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt new file mode 100644 index 00000000..887217ca --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -0,0 +1,34 @@ +package com.team4099.robot2023.subsystems.Shooter + +interface ShooterIO () { + class ShooterIOInputs : LoggableInputs { + var rollerVelocity = 0.rotations.perMinute + var rollerAppliedVoltage = 0.volts + var rollerStatorCurrent = 0.amps + var rollerSupplyCurrent = 0.amps + var rollerTempreature = 0.celsius + } + fun toLog(table:LogTable){ + table?.put("rollerVelocityRPM", rollerVelocity.inRadians.perSecond) + table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + table?.put("rollerStatorCurrent", rollerStatorCurrent.inamps) + table?.put("rollerVelocityRPM", rollerSupplyCurrent.inamps) + table?.put("rollerTempreature", rollerTempreature.celsisus) + + } + fun fromLog(table:LogTable){ + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond)?.let{ + rollerVelocity = it.radians.perSecond + } + table?.getDouble("rollerAppliedVoltage", rollerAppliedVoltage.involts)?.let { + rollerAppliedVoltage = it.volts + } + table?.getDouble("rollerStatorCurrent", rollerStatorCurrent.inRadiansPerSecond)?.let{ + + } + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + + + } +} \ No newline at end of file From a1f5cc869a2542d042abce13b6a664b9006446bf Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Wed, 10 Jan 2024 18:51:32 -0500 Subject: [PATCH 05/58] Finished IO File --- .../robot2023/subsystems/Shooter/ShooterIO.kt | 69 ++++++++++++++----- .../subsystems/Shooter/ShooterIONeo.kt | 5 ++ 2 files changed, 56 insertions(+), 18 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 887217ca..73c0b53d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -1,34 +1,67 @@ package com.team4099.robot2023.subsystems.Shooter -interface ShooterIO () { +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.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface ShooterIO { class ShooterIOInputs : LoggableInputs { var rollerVelocity = 0.rotations.perMinute var rollerAppliedVoltage = 0.volts var rollerStatorCurrent = 0.amps var rollerSupplyCurrent = 0.amps var rollerTempreature = 0.celsius - } - fun toLog(table:LogTable){ - table?.put("rollerVelocityRPM", rollerVelocity.inRadians.perSecond) - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) - table?.put("rollerStatorCurrent", rollerStatorCurrent.inamps) - table?.put("rollerVelocityRPM", rollerSupplyCurrent.inamps) - table?.put("rollerTempreature", rollerTempreature.celsisus) - } - fun fromLog(table:LogTable){ - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond)?.let{ - rollerVelocity = it.radians.perSecond - } - table?.getDouble("rollerAppliedVoltage", rollerAppliedVoltage.involts)?.let { - rollerAppliedVoltage = it.volts + 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?.getDouble("rollerStatorCurrent", rollerStatorCurrent.inRadiansPerSecond)?.let{ + + override fun fromLog(table: LogTable) { + 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 + } + } - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + } + fun updateInputs(inputs: ShooterIOInputs) + fun setRollerPower (voltage: ElectricalPotential){ + + } + fun setRollerBrakeMode (brake: Boolean){ } + fun configPID( + kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain , + ){} + fun zeroEncoder(){ + + } + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt new file mode 100644 index 00000000..6ee6352b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -0,0 +1,5 @@ +package com.team4099.robot2023.subsystems.Shooter + +class ShooterIONeo { + val +} \ No newline at end of file From 1eb31a1f12013d2e11de32d7de7787ebb51c53a5 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Thu, 11 Jan 2024 19:46:41 -0500 Subject: [PATCH 06/58] Did work on IO and IONeo for shooter also changed global mainpulator constants to be for shooter --- .../robot2023/config/constants/Constants.kt | 5 +-- .../config/constants/ShooterConstants.kt | 11 +++++ .../robot2023/subsystems/Shooter/ShooterIO.kt | 4 +- .../subsystems/Shooter/ShooterIONeo.kt | 42 ++++++++++++++++++- 4 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4f1f2add..c5761a42 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -115,9 +115,8 @@ object Constants { const val PIGEON_2_ID = 1 } - object Manipulator { - const val INTAKE_MOTOR_ID = 51 - const val ARM_MOTOR_ID = 52 + object Shooter { + const val ROLLER_MOTOR_ID = 51 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt new file mode 100644 index 00000000..b9ff868c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -0,0 +1,11 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.gearRatio + +object ShooterConstants { + val ROLLER_GEAR_RATIO = 0.0.gearRatio + val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 73c0b53d..99fecfb5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -48,7 +48,9 @@ interface ShooterIO { } } - fun updateInputs(inputs: ShooterIOInputs) + fun updateInputs(inputs: ShooterIOInputs){ + + } fun setRollerPower (voltage: ElectricalPotential){ } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 6ee6352b..bd7db0bf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -1,5 +1,43 @@ package com.team4099.robot2023.subsystems.Shooter -class ShooterIONeo { - val +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ShooterConstants +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.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object ShooterIONeo : ShooterIO{ + private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + ) + init{ + //reset the motors + rollerSparkMax.restoreFactoryDefaults() + rollerSparkMax.clearFaults() + + //voltage and current limits + rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) + rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + } + + override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + } + } \ No newline at end of file From ec012faa6c0b8d91c0a5d7a008cf3e03c360c8f5 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Fri, 12 Jan 2024 21:22:36 -0500 Subject: [PATCH 07/58] added feeder and wrist and did requests and started states --- .../robot2023/config/constants/Constants.kt | 3 + .../config/constants/ShooterConstants.kt | 12 ++- .../robot2023/subsystems/Shooter/Shooter.kt | 92 +++++++++++++++++ .../robot2023/subsystems/Shooter/ShooterIO.kt | 99 ++++++++++++++++--- .../subsystems/Shooter/ShooterIONeo.kt | 48 ++++++++- .../subsystems/superstructure/Request.kt | 11 +++ 6 files changed, 248 insertions(+), 17 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index c5761a42..694d144d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -117,6 +117,9 @@ object Constants { object Shooter { const val ROLLER_MOTOR_ID = 51 + //TODO find wrist motor id + const val SHOOTER_WRIST_MOTOR_ID = 999 + const val FEEDER_MOTOR_ID = 999 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index b9ff868c..a3a394b5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -2,10 +2,18 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.derived.gearRatio + object ShooterConstants { - val ROLLER_GEAR_RATIO = 0.0.gearRatio + 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 } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt new file mode 100644 index 00000000..10c504c1 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -0,0 +1,92 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.units.Voltage +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.* + +class Shooter (val io: ShooterIO){ + val inputs = ShooterIO.ShooterIOInputs() + private var RollerFeedforward: SimpleMotorFeedforward + private var WristFeedforward: SimpleMotorFeedforward + + + private val wristRollerkP = + LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val wristRollerkI = + LoggedTunableValue( + "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val wristRollerkD = + LoggedTunableValue( + "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) + ) + var currentState = ShooterStates.UNINITIALIZED + var rollerTargetVoltage : ElectricalPotential= 0.0.volts + var wristTargetVoltage : ElectricalPotential = 0.0.volts + var feederTargetVoltage : ElectricalPotential = 0.0.volts + fun setRollerVoltage(appliedVoltage: ElectricalPotential){ + io.setRollerVoltage(appliedVoltage) + } + fun setWristVoltage(appliedVoltage: ElectricalPotential){ + io.setWristVoltage(appliedVoltage) + } + fun setFeederVoltage(appliedVoltage: ElectricalPotential){ + io.setFeederVoltage(appliedVoltage) + } + var lastRollerRunTime = 0.0.seconds + var lastWristRunTime = 0.0.seconds + var lastFeederRunTime = 0.0.seconds + var isZeroed : Boolean = false + var currentRequest = Request.ShooterRequest.OpenLoop +fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + ShooterStates.UNINITIALIZED -> { + nextState = ShooterStates.ZERO + } + ShooterStates.ZERO ->{ +//TODO write zero function for shooter + } + ShooterStates.IDLE ->{ +//TODO figure out if were + } + ShooterStates.OPEN_LOOP ->{ + setRollerVoltage(rollerTargetVoltage) + lastRollerRunTime = Clock.fpgaTime + + setWristVoltage(wristTargetVoltage) + lastWristRunTime = Clock.fpgaTime + + setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime + if (isZeroed == true ){ + nextState = fromRequestToState(currentRequest) + } + + } + + ShooterStates.TARGETING_POSITION ->{ + + + } + + } + +} + + companion object { + enum class ShooterStates{ + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_POSITION, + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 99fecfb5..f1ab9e6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -16,48 +16,121 @@ interface ShooterIO { var rollerSupplyCurrent = 0.amps var rollerTempreature = 0.celsius + var wristPostion = 0.degrees + var wristAppliedVoltage = 0.volts + var wristStatorCurrent = 0.amps + 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 + 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("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("wristAppliedVoltage", wristAppliedVoltage.inVolts) + table.put("wristStatorCurrent", wristStatorCurrent.inAmperes) + table.put("wristSupplyCurrent", wristSupplyCurrent.inAmperes) + table.put("wristTemperature", wristTemperature.inCelsius) + + table.put("feederVelocity", 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) { - table?.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond)?.let { + //roller logs + table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let { rollerVelocity = it.radians.perSecond } - table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { + table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let { rollerAppliedVoltage = it.volts } - table?.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes)?.let { + table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let { rollerStatorCurrent = it.amps } - table?.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes)?.let { + table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let { rollerSupplyCurrent = it.amps } - - table?.get("rollerTempreature", rollerTempreature.inCelsius)?.let { + table.get("rollerTempreature", rollerTempreature.inCelsius).let { rollerTempreature = it.celsius } +//wrist logs + table.get("wristPostion", wristPostion.inDegrees).let { + wristPostion = it.degrees + } + table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { + wristAppliedVoltage = it.volts + } + table.get("wristStatorCurrent", wristStatorCurrent.inAmperes).let { + wristStatorCurrent = it.amps + } + table.get("wristSupplyCurrent", wristSupplyCurrent.inAmperes).let { + wristSupplyCurrent = it.amps + } + 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 setRollerPower (voltage: ElectricalPotential){ + fun setRollerVoltage (voltage: ElectricalPotential){ + + } + fun setWristVoltage (voltage: ElectricalPotential){ + + } + fun setFeederVoltage (voltage: ElectricalPotential){ + + } + fun setWristPosition (voltage: ElectricalPotential){ } fun setRollerBrakeMode (brake: Boolean){ } - fun configPID( + fun setFeederBrakeMode (brake: Boolean){ + + } + + + fun configWristPID( kP: ProportionalGain , kI: IntegralGain , kD: DerivativeGain , diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index bd7db0bf..b88f3cad 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -7,25 +7,49 @@ import com.team4099.robot2023.config.constants.ShooterConstants 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.inDegrees import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue - +//TODO write a kraken file object ShooterIONeo : ShooterIO{ private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ShooterConstants.ROLLER_GEAR_RATIO, ShooterConstants.ROLLER_VOLTAGE_COMPENSATION ) + +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 + ) + private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, + ShooterConstants.FEEDER_GEAR_RATIO, + ShooterConstants.FEEDER_VOLTAGE_COMPENSATION + ) init{ //reset the motors rollerSparkMax.restoreFactoryDefaults() rollerSparkMax.clearFaults() + wristSparkMax.restoreFactoryDefaults() + wristSparkMax.clearFaults() + + feederSparkMax.restoreFactoryDefaults() + feederSparkMax.clearFaults() + //voltage and current limits 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()) + + feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) + feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) } override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ @@ -38,6 +62,26 @@ object ShooterIONeo : ShooterIO{ // percentOutput * statorCurrent inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + + inputs.wristPostion = wristSensor.position + inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput + inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue + inputs.wristTemperature = wristSparkMax.motorTemperature.celsius + + inputs.feederVelocity = feederSensor.velocity + inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput + inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue + inputs.feederTemperature = feederSparkMax.motorTemperature.celsius } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 65fe9e78..695d8d7b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -5,6 +5,7 @@ import com.team4099.robot2023.config.constants.NodeTier import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.Angle @@ -81,4 +82,14 @@ sealed interface Request { class ZeroSensors : DrivetrainRequest class Idle : DrivetrainRequest } + sealed interface ShooterRequest : Request { + class OpenLoop(wristVoltage : ElectricalPotential, + rollerVoltage: ElectricalPotential, + feederVoltage: ElectricalPotential):ShooterRequest{} + class TargetingPosition (val wristPosition : Angle, + val rollerVelocity: AngularVelocity, + val feederVelocity: AngularVelocity + ):ShooterRequest{} + + } } From a51f5a570ce73efd850e12450687db930090ef4d Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 13 Jan 2024 19:53:27 -0500 Subject: [PATCH 08/58] Did more targeting position state and open loop state --- .../config/constants/ShooterConstants.kt | 8 +++ .../robot2023/subsystems/Shooter/Shooter.kt | 70 ++++++++++++++++--- .../robot2023/subsystems/Shooter/ShooterIO.kt | 14 +++- .../subsystems/Shooter/ShooterIONeo.kt | 1 + .../subsystems/superstructure/Request.kt | 1 + 5 files changed, 84 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index a3a394b5..887f95fb 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -1,6 +1,7 @@ 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.volts @@ -16,4 +17,11 @@ object ShooterConstants { 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 + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 10c504c1..ba8c20e3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -2,13 +2,20 @@ 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.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.units.Voltage +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() @@ -43,7 +50,26 @@ class Shooter (val io: ShooterIO){ var lastWristRunTime = 0.0.seconds var lastFeederRunTime = 0.0.seconds var isZeroed : Boolean = false - var currentRequest = Request.ShooterRequest.OpenLoop + private var lastRollerVoltage = 0.0.volts + //TODO ask what to set this too + private var wristPositionTarget = 0.0.degrees + private var lastWristPositionTarget = 0.0.degrees + private var rollerInitVoltage = LoggedTunableValue ("Shooter/Initial Roller 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 timeProfileGeneratedAt = 0.0.seconds + //TODO ask aanshi wtf to pass in for init parameters + var currentRequest = Request.ShooterRequest.OpenLoop( + ShooterConstants.WRIST_INIT_VOLTAGE, + ShooterConstants.ROLLLER_INIT_VOLTAGE, + ShooterConstants.FEEDER_INIT_VOLTAGE) + private var wristProfile = + TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) + ) + fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -52,11 +78,9 @@ fun periodic(){ nextState = ShooterStates.ZERO } ShooterStates.ZERO ->{ -//TODO write zero function for shooter - } - ShooterStates.IDLE ->{ -//TODO figure out if were +//TODO create zero encoder if we get one } + ShooterStates.OPEN_LOOP ->{ setRollerVoltage(rollerTargetVoltage) lastRollerRunTime = Clock.fpgaTime @@ -74,19 +98,47 @@ fun periodic(){ ShooterStates.TARGETING_POSITION ->{ - + 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 + setwristPosition(wristProfile.calculate(timeElapsed)) + //TODO implement set wrist pos function + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } - } -} - companion object { + + } + + } + companion object{ enum class ShooterStates{ UNINITIALIZED, ZERO, OPEN_LOOP, TARGETING_POSITION, } + inline fun fromRequestToState(request: Request.ShooterRequest): ShooterStates { + return when (request) { + is Request.ShooterRequest.OpenLoop -> ShooterStates.OPEN_LOOP + is Request.ShooterRequest.TargetingPosition -> ShooterStates.TARGETING_POSITION + is Request.ShooterRequest.Zero -> ShooterStates.ZERO + + } + } } + } + + diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index f1ab9e6f..d1d98b98 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -4,6 +4,7 @@ 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 @@ -17,6 +18,7 @@ interface ShooterIO { var rollerTempreature = 0.celsius var wristPostion = 0.degrees + var wristVelocity = 0.radians.perSecond var wristAppliedVoltage = 0.volts var wristStatorCurrent = 0.amps var wristSupplyCurrent = 0.amps @@ -36,12 +38,13 @@ interface ShooterIO { 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("feederVelocity", feederVelocity.inRadiansPerSecond) + table.put("feederVelocityRPM", feederVelocity.inRadiansPerSecond) table.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) table.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) @@ -67,10 +70,16 @@ interface ShooterIO { table.get("rollerTempreature", rollerTempreature.inCelsius).let { rollerTempreature = it.celsius } + + + //wrist logs table.get("wristPostion", wristPostion.inDegrees).let { wristPostion = it.degrees } + table.get("wristVelocity", wristVelocity.inRadiansPerSecond).let { + wristVelocity = it.radians.perSecond + } table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { wristAppliedVoltage = it.volts } @@ -84,6 +93,9 @@ interface ShooterIO { table.get("wristTemperature", wristTemperature.inCelsius).let { wristTemperature = it.celsius } + + + //feeder table.get("feederVelocity", feederVelocity.inRadiansPerSecond).let { feederVelocity = it.radians.perSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index b88f3cad..8f6044c3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -84,4 +84,5 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.feederTemperature = feederSparkMax.motorTemperature.celsius } + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 695d8d7b..f042ac1f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -90,6 +90,7 @@ sealed interface Request { val rollerVelocity: AngularVelocity, val feederVelocity: AngularVelocity ):ShooterRequest{} + class Zero () : ShooterRequest{} } } From d84ce3cad4d5dae28c92ffaa9cdf7ea29299e4ad Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:46:01 -0500 Subject: [PATCH 09/58] Implemeted functions from IO in IONEO --- .../config/constants/ShooterConstants.kt | 22 +++-- .../robot2023/subsystems/Shooter/Shooter.kt | 58 +++++------ .../robot2023/subsystems/Shooter/ShooterIO.kt | 20 ++-- .../subsystems/Shooter/ShooterIONeo.kt | 98 ++++++++++++++----- .../subsystems/superstructure/Request.kt | 9 +- 5 files changed, 131 insertions(+), 76 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index 887f95fb..5e813dd4 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -2,26 +2,30 @@ 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 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 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 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 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/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index ba8c20e3..f9b1c3d7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -3,66 +3,65 @@ 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.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState +import com.team4099.robot2023.subsystems.Shooter.ShooterIONeo.setWristPosition import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.units.Voltage import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.controller.TrapezoidProfile + import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() - private var RollerFeedforward: SimpleMotorFeedforward + //TODO do feedforward private var WristFeedforward: SimpleMotorFeedforward - - private val wristRollerkP = +/* + private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val wristRollerkI = + private val wristflywheelkI = LoggedTunableValue( "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) - private val wristRollerkD = + private val wristflywheelkD = LoggedTunableValue( "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - ) + )*/ var currentState = ShooterStates.UNINITIALIZED - var rollerTargetVoltage : ElectricalPotential= 0.0.volts + var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts - fun setRollerVoltage(appliedVoltage: ElectricalPotential){ - io.setRollerVoltage(appliedVoltage) - } + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setflywheelVoltage(appliedVoltage) + }*/ fun setWristVoltage(appliedVoltage: ElectricalPotential){ io.setWristVoltage(appliedVoltage) } - fun setFeederVoltage(appliedVoltage: ElectricalPotential){ + /*fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) - } - var lastRollerRunTime = 0.0.seconds + }*/ + /*var lastFlywheelRunTime = 0.0.seconds + var lastFeederRunTime = 0.0.seconds*/ var lastWristRunTime = 0.0.seconds - var lastFeederRunTime = 0.0.seconds var isZeroed : Boolean = false - private var lastRollerVoltage = 0.0.volts + 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 rollerInitVoltage = LoggedTunableValue ("Shooter/Initial Roller Voltage", ShooterConstants.ROLLLER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) + /*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", ShooterConstants.WRIST_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) private var timeProfileGeneratedAt = 0.0.seconds //TODO ask aanshi wtf to pass in for init parameters var currentRequest = Request.ShooterRequest.OpenLoop( ShooterConstants.WRIST_INIT_VOLTAGE, - ShooterConstants.ROLLLER_INIT_VOLTAGE, - ShooterConstants.FEEDER_INIT_VOLTAGE) + //ShooterConstants.ROLLLER_INIT_VOLTAGE, + //ShooterConstants.FEEDER_INIT_VOLTAGE + ) private var wristProfile = TrapezoidProfile( wristConstraints, @@ -82,14 +81,15 @@ fun periodic(){ } ShooterStates.OPEN_LOOP ->{ - setRollerVoltage(rollerTargetVoltage) - lastRollerRunTime = Clock.fpgaTime - + /* + setflywheelVoltage(flywheelTargetVoltage) + lastflywheelRunTime = Clock.fpgaTime +*/ setWristVoltage(wristTargetVoltage) lastWristRunTime = Clock.fpgaTime - setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime + /* setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime*/ if (isZeroed == true ){ nextState = fromRequestToState(currentRequest) } @@ -111,7 +111,7 @@ fun periodic(){ lastWristPositionTarget = wristPositionTarget } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setwristPosition(wristProfile.calculate(timeElapsed)) + setWristPosition(wristProfile.calculate(timeElapsed)) //TODO implement set wrist pos function Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index d1d98b98..5fb6b2d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -122,25 +122,27 @@ interface ShooterIO { fun updateInputs(inputs: ShooterIOInputs){ } - fun setRollerVoltage (voltage: ElectricalPotential){ + /*fun setRollerVoltage (voltage: ElectricalPotential){ - } + }*/ fun setWristVoltage (voltage: ElectricalPotential){ } - fun setFeederVoltage (voltage: ElectricalPotential){ + //fun setFeederVoltage (voltage: ElectricalPotential){ - } - fun setWristPosition (voltage: ElectricalPotential){ +// } + fun setWristPosition (position : Angle, feedforward : ElectricalPotential){ } - fun setRollerBrakeMode (brake: Boolean){ + //fun setRollerBrakeMode (brake: Boolean){ - } - fun setFeederBrakeMode (brake: Boolean){ + //} + //fun setFeederBrakeMode (brake: Boolean){ - } + // } + fun setWristBrakeMode (brake: Boolean){ + } fun configWristPID( kP: ProportionalGain , diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 8f6044c3..060434f7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -2,58 +2,60 @@ package com.team4099.robot2023.subsystems.Shooter import com.revrobotics.CANSparkMax 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 org.team4099.lib.units.base.Meter 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.inDegrees -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue //TODO write a kraken file object ShooterIONeo : ShooterIO{ - private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - ShooterConstants.ROLLER_GEAR_RATIO, - ShooterConstants.ROLLER_VOLTAGE_COMPENSATION - ) + //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + //ShooterConstants.ROLLER_GEAR_RATIO, + // ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + // ) 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 ) - private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, - ShooterConstants.FEEDER_GEAR_RATIO, - ShooterConstants.FEEDER_VOLTAGE_COMPENSATION - ) + private val wristPIDController : SparkMaxPIDController = wristSparkMax.pidController + //private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + // private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, + //ShooterConstants.FEEDER_GEAR_RATIO, + //ShooterConstants.FEEDER_VOLTAGE_COMPENSATION + //) init{ //reset the motors - rollerSparkMax.restoreFactoryDefaults() - rollerSparkMax.clearFaults() + //rollerSparkMax.restoreFactoryDefaults() + // rollerSparkMax.clearFaults() wristSparkMax.restoreFactoryDefaults() wristSparkMax.clearFaults() - feederSparkMax.restoreFactoryDefaults() - feederSparkMax.clearFaults() + //feederSparkMax.restoreFactoryDefaults() + // feederSparkMax.clearFaults() //voltage and current limits - rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) - rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + // 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()) - feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) - feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_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){ - inputs.rollerVelocity = rollerSensor.velocity + /* inputs.rollerVelocity = rollerSensor.velocity inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent @@ -62,7 +64,7 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID // percentOutput * statorCurrent inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius - +*/ inputs.wristPostion = wristSensor.position inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps @@ -73,7 +75,7 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue inputs.wristTemperature = wristSparkMax.motorTemperature.celsius - inputs.feederVelocity = feederSensor.velocity + /* inputs.feederVelocity = feederSensor.velocity inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent @@ -81,8 +83,54 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = // percentOutput * statorCurrent inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue - inputs.feederTemperature = feederSparkMax.motorTemperature.celsius + inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ + } + override fun configWristPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { +//TODO fix this please + wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) + wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) + wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) + } + + override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + wristPIDController.setReference( + wristSensor.positionToRawUnits( + clamp( + position, + ShooterConstants.WRIST_SOFTLIMIT_DOWNWARDSTURN, + ShooterConstants.WRIST_SOFTLIMIT_UPWARDSTURN + ) + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts + ) + } + + override fun setWristVoltage(voltage: ElectricalPotential) { + wristSparkMax.setVoltage( + clamp( + voltage, + -ShooterConstants.WRIST_VOLTAGE_COMPENSATION , + ShooterConstants.WRIST_VOLTAGE_COMPENSATION + ) + .inVolts + ) } + override fun setWristBrakeMode(brake: Boolean) { + if(brake){ + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) + }else{ + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast) + } + } + override fun zeroEncoder() { + wristSparkMax.encoder.position = 0.0 + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index f042ac1f..5cabace1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -84,11 +84,12 @@ sealed interface Request { } sealed interface ShooterRequest : Request { class OpenLoop(wristVoltage : ElectricalPotential, - rollerVoltage: ElectricalPotential, - feederVoltage: ElectricalPotential):ShooterRequest{} + //rollerVoltage: ElectricalPotential, + //feederVoltage: ElectricalPotential + ):ShooterRequest{} class TargetingPosition (val wristPosition : Angle, - val rollerVelocity: AngularVelocity, - val feederVelocity: AngularVelocity + //val flywheelVelocity: AngularVelocity, + //val feederVelocity: AngularVelocity ):ShooterRequest{} class Zero () : ShooterRequest{} From 4bc6dbf3ae3f2cf897eecc44e65c5d9d79a4fc96 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sun, 14 Jan 2024 21:43:47 -0500 Subject: [PATCH 10/58] Added flywheel subsystem --- .../robot2023/subsystems/Shooter/Flywheel.kt | 67 ++++++++++++++ .../subsystems/Shooter/FlywheelConstants.kt | 44 ++++++++++ .../subsystems/Shooter/FlywheelIO.kt | 63 ++++++++++++++ .../subsystems/Shooter/FlywheelIOFalcon.kt | 87 +++++++++++++++++++ .../robot2023/subsystems/Shooter/Shooter.kt | 6 +- .../subsystems/Shooter/ShooterIONeo.kt | 1 - 6 files changed, 263 insertions(+), 5 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt new file mode 100644 index 00000000..dd364355 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -0,0 +1,67 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* + +class Flywheel (val io: FlywheelIO) { + var feedforward = SimpleMotorFeedforward() + val inputs = FlywheelIO.FlywheelIOInputs() + private val flywheelkS = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val flywheelkkV = + LoggedTunableValue( + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + ) + private val flywheelkA = + LoggedTunableValue( + "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + ) + + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts + fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { + io.setFlywheelVoltage(appliedVoltage) } + + var lastFlywheelRunTime = 0.0.seconds + private var lastFlywheelVoltage = 0.0.volts + private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) + private var hasNote:Boolean = true + var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + Flywheel.Companion.FlywheelStates.UNINITIALIZED -> { + nextState = Flywheel.Companion.FlywheelStates.ZERO + } + Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelTargetVoltage) + lastFlywheelRunTime = Clock.fpgaTime + } + Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ + if (flywheelTargetVoltage != lastFlywheelVoltage){ + if(hasNote){ + + } + } + } + Flywheel.Companion.FlywheelStates.ZERO ->{ + + } + } + + } + + companion object { + enum class FlywheelStates { + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_VELOCITY, + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt new file mode 100644 index 00000000..64bff2c7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt @@ -0,0 +1,44 @@ +package com.team4099.robot2023.subsystems.Shooter + +import org.team4099.lib.units.derived.volts + +object FlywheelConstants { + val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts + val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts + val ROLLER_GEAR_RATIO = 0.0 + + object PID { + + val AUTO_POS_KP: ProportionalGain> + get() { + if (RobotBase.isReal()) { + return 8.0.meters.perSecond / 1.0.meters + } else { + return 7.0.meters.perSecond / 1.0.meters + } + } + val AUTO_POS_KI: IntegralGain> + get() { + if (RobotBase.isReal()) { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } else { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } + } + + val AUTO_POS_KD: DerivativeGain> + get() { + if (RobotBase.isReal()) { + return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } else { + return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } + + } + } + val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds + val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt new file mode 100644 index 00000000..084077f8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -0,0 +1,63 @@ +package com.team4099.robot2023.subsystems.Shooter + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface FlywheelIO { + + class FlywheelIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + var rollerAppliedVoltage = 0.volts + var rollerStatorCurrent = 0.amps + var rollerSupplyCurrent = 0.amps + var rollerTempreature = 0.celsius + + 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) + } + 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 + } + } + } + fun setFlywheelVoltage (voltage: ElectricalPotential){ + + } + fun setFlywheelBrakeMode (brake: Boolean){ + + } + fun updateInputs(inputs:FlywheelIOInputs){ + + } + fun zeroEncoder(){ + + } + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt new file mode 100644 index 00000000..f66967d0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -0,0 +1,87 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.ctreAngularMechanismSensor + +class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ +private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelSensor = + ctreAngularMechanismSensor( + flywheelFalcon, + FlywheelConstants.ROLLER_GEAR_RATIO, + FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, + + ) + val flywheelStatorCurrentSignal: StatusSignal + val flywheelSupplyCurrentSignal: StatusSignal + val flywheelTempSignal: StatusSignal + init { + flywheelFalcon.configurator.apply(TalonFXConfiguration()) + + flywheelFalcon.clearStickyFaults() + flywheelFalcon.configurator.apply(flywheelConfiguration) +//TODO fix PID + flywheelConfiguration.Slot0.kP = + flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelConfiguration.Slot0.kI = + flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelConfiguration.Slot0.kD = + flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelConfiguration.Slot0.kV = 0.05425 + // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) + flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + flywheelFalcon.configurator.apply(flywheelConfiguration) + + flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent + flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent + flywheelTempSignal = flywheelFalcon.deviceTemp + + MotorChecker.add( + "flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + } + + + + + + override fun setFlywheelBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + + if (brake) { + motorOutputConfig.NeutralMode = NeutralModeValue.Brake + } else { + motorOutputConfig.NeutralMode = NeutralModeValue.Coast + } + flywheelFalcon.configurator.apply(motorOutputConfig) + } + override fun zeroEncoder(){ + //TODO finish zero encoder fun (ask sumone what the encoder for falcon is) + flywheelFalcon + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index f9b1c3d7..8064d58f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -32,7 +32,7 @@ class Shooter (val io: ShooterIO){ "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) )*/ var currentState = ShooterStates.UNINITIALIZED - var flywheelTargetVoltage : ElectricalPotential= 0.0.volts + //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ @@ -56,7 +56,6 @@ class Shooter (val io: ShooterIO){ 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 timeProfileGeneratedAt = 0.0.seconds - //TODO ask aanshi wtf to pass in for init parameters var currentRequest = Request.ShooterRequest.OpenLoop( ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, @@ -77,7 +76,6 @@ fun periodic(){ nextState = ShooterStates.ZERO } ShooterStates.ZERO ->{ -//TODO create zero encoder if we get one } ShooterStates.OPEN_LOOP ->{ @@ -112,7 +110,7 @@ fun periodic(){ } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO implement set wrist pos function + //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 060434f7..9100063d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -13,7 +13,6 @@ import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue -//TODO write a kraken file object ShooterIONeo : ShooterIO{ //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, From 829a07586e50bf71782342ae9623c560237b502b Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Mon, 15 Jan 2024 18:56:09 -0500 Subject: [PATCH 11/58] Started flywheel PID and did feedforward --- .../constants}/FlywheelConstants.kt | 3 +- .../robot2023/subsystems/Shooter/Flywheel.kt | 23 +++++++---- .../subsystems/Shooter/FlywheelIO.kt | 7 ++++ .../subsystems/Shooter/FlywheelIOFalcon.kt | 40 ++++++++++++++++--- .../robot2023/subsystems/Shooter/Shooter.kt | 2 +- .../robot2023/subsystems/Shooter/ShooterIO.kt | 6 +-- .../subsystems/Shooter/ShooterIONeo.kt | 7 ++-- .../subsystems/superstructure/Request.kt | 6 +++ 8 files changed, 72 insertions(+), 22 deletions(-) rename src/main/kotlin/com/team4099/robot2023/{subsystems/Shooter => config/constants}/FlywheelConstants.kt (94%) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt similarity index 94% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt rename to src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 64bff2c7..74f5903f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,5 +1,6 @@ -package com.team4099.robot2023.subsystems.Shooter +package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object FlywheelConstants { 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 dd364355..9573314f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -2,25 +2,30 @@ package com.team4099.robot2023.subsystems.Shooter import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute class Flywheel (val io: FlywheelIO) { - var feedforward = SimpleMotorFeedforward() + val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val flywheelkkV = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian })) + private val flywheelkV = LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond }) ) + val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { @@ -30,7 +35,9 @@ class Flywheel (val io: FlywheelIO) { private var lastFlywheelVoltage = 0.0.volts private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) private var hasNote:Boolean = true + val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -44,9 +51,9 @@ class Flywheel (val io: FlywheelIO) { } Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ - if(hasNote){ + val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - } + io.setFlywheelVoltage(controlEffort) } } Flywheel.Companion.FlywheelStates.ZERO ->{ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt index 084077f8..e0920b36 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.Shooter import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes @@ -47,6 +48,7 @@ interface FlywheelIO { } } } + fun setFlywheelVoltage (voltage: ElectricalPotential){ } @@ -59,5 +61,10 @@ interface FlywheelIO { fun zeroEncoder(){ } + fun configPID(kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){ + + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt index f66967d0..73484e57 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -2,16 +2,26 @@ package com.team4099.robot2023.subsystems.Shooter import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter 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.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ -private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelPIDController = TalonFX + private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelSensor = ctreAngularMechanismSensor( flywheelFalcon, @@ -22,6 +32,15 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() val flywheelStatorCurrentSignal: StatusSignal val flywheelSupplyCurrentSignal: StatusSignal val flywheelTempSignal: StatusSignal + private val kP = + LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val kD = + LoggedTunableValue( + "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -29,11 +48,11 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() flywheelFalcon.configurator.apply(flywheelConfiguration) //TODO fix PID flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelSensor.proportionalVelocityGainToRawUnits(kP) flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelSensor.integralVelocityGainToRawUnits(kI) flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelSensor.derivativeVelocityGainToRawUnits(kD) flywheelConfiguration.Slot0.kV = 0.05425 // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = @@ -65,9 +84,20 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() ) ) } + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + val PIDConfig = Slot0Configs() + PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP) + PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI) + PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD) + PIDConfig.kV = 0.05425 - + flywheelFalcon.configurator.apply(PIDConfig) + } override fun setFlywheelBrakeMode(brake: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 8064d58f..b80b67f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,7 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward + private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) /* private val wristflywheelkP = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 5fb6b2d1..757a55f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -145,9 +145,9 @@ interface ShooterIO { } fun configWristPID( - kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain , + kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){} fun zeroEncoder(){ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 9100063d..4fd485dc 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -85,11 +85,10 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ } override fun configWristPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain ) { -//TODO fix this please wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 5cabace1..02093c3c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier +import com.team4099.robot2023.subsystems.Shooter.Flywheel import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity @@ -94,4 +95,9 @@ sealed interface Request { class Zero () : ShooterRequest{} } + sealed interface FlywheelRequest : Request { + class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} + class TargetingVelocity (flywheelVelocity: AngularVelocity) + class Zero ():FlywheelRequest{} + } } From c0a3c45ad963beb9d907094b530d6cb2aa448584 Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Mon, 15 Jan 2024 19:48:52 -0500 Subject: [PATCH 12/58] Fixed some issues --- .../com/team4099/robot2023/subsystems/Shooter/Flywheel.kt | 8 +++++--- .../com/team4099/robot2023/subsystems/Shooter/Shooter.kt | 2 +- 2 files changed, 6 insertions(+), 4 deletions(-) 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 dd364355..da92ac6a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -7,19 +7,21 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.seconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* +import org.team4099.lib.units.* class Flywheel (val io: FlywheelIO) { - var feedforward = SimpleMotorFeedforward() + // TODO: Make shooter constants (kS, kV, kA) + var feedforward = SimpleMotorFeedforward() val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) private val flywheelkkV = LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + "Flywheel/kV", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + "Flywheel/kA", Pair({ it.inVoltsPerInchPerSecond}, { it.volts.perInchPerSecond }) ) var flywheelTargetVoltage: ElectricalPotential = 0.0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 8064d58f..6720537e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,7 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward + private var wristFeedforward: SimpleMotorFeedforward /* private val wristflywheelkP = From 5491f51c888eead50b8179cdb843b7d03a3cb119 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:12:10 -0500 Subject: [PATCH 13/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../robot2023/config/constants/Constants.kt | 2 +- .../config/constants/FlywheelConstants.kt | 13 +++++- .../robot2023/subsystems/Shooter/Flywheel.kt | 26 ++++++++++-- .../subsystems/Shooter/FlywheelIO.kt | 13 +++--- .../subsystems/Shooter/FlywheelIOFalcon.kt | 41 +++++++++---------- .../robot2023/subsystems/Shooter/Shooter.kt | 4 +- 6 files changed, 63 insertions(+), 36 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 694d144d..31afc6c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -116,7 +116,7 @@ object Constants { } object Shooter { - const val ROLLER_MOTOR_ID = 51 + const val FLYWHEEL_MOTOR_ID = 51 //TODO find wrist motor id const val SHOOTER_WRIST_MOTOR_ID = 999 const val FEEDER_MOTOR_ID = 999 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 74f5903f..8e96e0af 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,7 +1,10 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perMinute object FlywheelConstants { val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts @@ -42,4 +45,12 @@ object FlywheelConstants { val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps + val SHOOTER_FLYWHEEL_KP: ProportionalGain, Volt> = + 0.001.volts / 1.0.rotations.perMinute + val SHOOTER_FLYWHEEL_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val SHOOTER_FLYWHEEL_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + + } \ 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 9573314f..960c48fe 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -1,8 +1,12 @@ package com.team4099.robot2023.subsystems.Shooter +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.ControlModeValue import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.Meter @@ -12,19 +16,30 @@ import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perMinute class Flywheel (val io: FlywheelIO) { + val motor = TalonFX(Constants.Shooter.FLYWHEEL_MOTOR_ID) + private val kP = + LoggedTunableValue("Flywheel/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP) + private val kI = + LoggedTunableValue( + "Flywheel/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI) + private val kD = + LoggedTunableValue( + "Flywheel/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD) + val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian })) + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian }) + ) private val flywheelkV = LoggedTunableValue( "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond }) + "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perRadianPerSecond }) ) - val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) + val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) var flywheelTargetVoltage: ElectricalPotential = 0.0.volts @@ -37,7 +52,10 @@ class Flywheel (val io: FlywheelIO) { private var hasNote:Boolean = true val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED +init{ + io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) +} fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -52,7 +70,7 @@ class Flywheel (val io: FlywheelIO) { Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - + io.setFlywheelVelocity()//TODO talk to anshi ab a current velocity var io.setFlywheelVoltage(controlEffort) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt index e0920b36..78d9a9b7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -3,14 +3,12 @@ package com.team4099.robot2023.subsystems.Shooter import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.* 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.inCelsius import org.team4099.lib.units.derived.* -import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.perMinute -import org.team4099.lib.units.perSecond interface FlywheelIO { @@ -51,6 +49,9 @@ interface FlywheelIO { fun setFlywheelVoltage (voltage: ElectricalPotential){ + } + fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ + } fun setFlywheelBrakeMode (brake: Boolean){ @@ -61,9 +62,9 @@ interface FlywheelIO { fun zeroEncoder(){ } - fun configPID(kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain ){ + fun configPID(kP: ProportionalGain , Volt>, + kI: IntegralGain , Volt>, + kD: DerivativeGain , Volt>){ } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt index 73484e57..fdd8df4b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -11,16 +11,15 @@ import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.Meter -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.* import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ - private val flywheelPIDController = TalonFX + private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelSensor = ctreAngularMechanismSensor( @@ -29,18 +28,9 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, ) - val flywheelStatorCurrentSignal: StatusSignal - val flywheelSupplyCurrentSignal: StatusSignal - val flywheelTempSignal: StatusSignal - private val kP = - LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val kI = - LoggedTunableValue( - "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) - ) - private val kD = - LoggedTunableValue( - "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) + var flywheelStatorCurrentSignal: StatusSignal + var flywheelSupplyCurrentSignal: StatusSignal + var flywheelTempSignal: StatusSignal init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -48,11 +38,11 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelFalcon.configurator.apply(flywheelConfiguration) //TODO fix PID flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(kP) + flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(kI) + flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(kD) + flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) flywheelConfiguration.Slot0.kV = 0.05425 // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = @@ -74,7 +64,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelTempSignal = flywheelFalcon.deviceTemp MotorChecker.add( - "flywheel", + "Shooter","Flywheel", MotorCollection( mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, @@ -84,7 +74,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ ) ) } - override fun configureDrivePID( + override fun configPID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt> @@ -98,6 +88,13 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelFalcon.configurator.apply(PIDConfig) } + override fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ + flywheelFalcon.setControl(0, + flywheelSensor.velocityToRawUnits(angularVelocity), + DemandType.ArbitraryFeedForward, + feedforward.inVolts + ) + } override fun setFlywheelBrakeMode(brake: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index b80b67f6..3bd688df 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,7 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) + private var WristFeedforward: SimpleMotorFeedforward() /* private val wristflywheelkP = @@ -109,7 +109,7 @@ fun periodic(){ lastWristPositionTarget = wristPositionTarget } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition(wristProfile.calculate(timeElapsed)) + setWristPosition( WristFeedforward, wristProfile.calculate(timeElapsed)) //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } From c761b7cae080d5a6e756c98a44cafbf23a797f08 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:13:23 -0500 Subject: [PATCH 14/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../team4099/robot2023/config/constants/FlywheelConstants.kt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 8e96e0af..2b1afafe 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,10 +1,14 @@ package com.team4099.robot2023.config.constants +import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.meters 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 FlywheelConstants { val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts From 6b69d8055e7206a85e8be3013de5aa49a31e9015 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:17:56 -0500 Subject: [PATCH 15/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt | 1 - 1 file changed, 1 deletion(-) 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 960c48fe..26677ff0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -54,7 +54,6 @@ class Flywheel (val io: FlywheelIO) { var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED init{ io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) - } fun periodic(){ io.updateInputs(inputs) From e5df1b0a2f95fa3433172be3b4cdf9faaccedbdd Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Tue, 9 Jan 2024 19:00:46 -0500 Subject: [PATCH 16/58] Added Feeder IO File --- build.gradle | 4 +- .../robot2023/subsystems/feeder/FeederIO.kt | 66 +++++++++++++++++++ 2 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/build.gradle b/build.gradle index a21179a3..1f2fe330 100644 --- a/build.gradle +++ b/build.gradle @@ -84,9 +84,9 @@ dependencies { implementation 'org.jetbrains.kotlin:kotlin-test-junit5' implementation 'com.github.team4099:FalconUtils:1.1.26' - implementation 'org.apache.commons:commons-collections4:4.0' + implementation 'org.apache.commons:commons-collections4:4.4' implementation 'com.google.code.gson:gson:2.10.1' - implementation "io.javalin:javalin:5.3.2" + implementation 'io.javalin:javalin:5.4.2' // We need to add the Kotlin stdlib in order to use most Kotlin language features. // compile "org.jetbrains.kotlin:kotlin-stdlib" diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt new file mode 100644 index 00000000..15eec3c3 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -0,0 +1,66 @@ +package com.team4099.robot2023.subsystems.feeder + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +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 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) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { + floorAppliedVoltage = it.volts + } + table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { + floorStatorCurrent = it.amps + } + table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { + floorSupplyCurrent = it.amps + } + table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { + verticalAppliedVoltage = it.volts + } + table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + verticalStatorCurrent = it.amps + } + table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + verticalSupplyCurrent = it.amps + } + table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } + } + + fun updateInputs(inputs: FeederIOInputs) {} + + fun setFloorVoltage(voltage: ElectricalPotential) {} + + fun setVerticalVoltage(voltage: ElectricalPotential) {} + } +} \ No newline at end of file From 44bfc2fca73cc15ce68bc871bd47cddaf905c909 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:14:33 -0500 Subject: [PATCH 17/58] create telescoping arm branch --- .../robot2023/subsystems/TelescopingArm/TelescopingArm.kt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt new file mode 100644 index 00000000..a1af62e2 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.TelescopingArm + +class TelescopingArm { +} \ No newline at end of file From 6a4a99bf53dfea61d8d46d0ff893ce9d2f699ab0 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:10:42 -0500 Subject: [PATCH 18/58] create ground intake branch --- .../robot2023/subsystems/GroundIntake/GroundIntake.kt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt new file mode 100644 index 00000000..5fdc7677 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.GroundIntake + +class GroundIntake { +} \ No newline at end of file From a26ab3066c3f6e7922d31bbd6adcf3758b5cf690 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:05:35 -0500 Subject: [PATCH 19/58] create feeder branch --- .../robot2023/subsystems/feeder/FeederIO.kt | 62 ------------------- 1 file changed, 62 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 15eec3c3..41ddbf24 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,66 +1,4 @@ package com.team4099.robot2023.subsystems.feeder -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -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.inCelsius -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -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 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) - } - - override fun fromLog(table: LogTable?) { - table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { - floorAppliedVoltage = it.volts - } - table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { - floorStatorCurrent = it.amps - } - table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { - floorSupplyCurrent = it.amps - } - table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } - table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { - verticalAppliedVoltage = it.volts - } - table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { - verticalStatorCurrent = it.amps - } - table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { - verticalSupplyCurrent = it.amps - } - table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } - } - - fun updateInputs(inputs: FeederIOInputs) {} - - fun setFloorVoltage(voltage: ElectricalPotential) {} - - fun setVerticalVoltage(voltage: ElectricalPotential) {} - } } \ No newline at end of file From 72d1ce5530eda5a5e215224978da673ab40001e4 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:08:24 -0500 Subject: [PATCH 20/58] Create Feeder.kt --- .../com/team4099/robot2023/subsystems/feeder/Feeder.kt | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt new file mode 100644 index 00000000..bb51b835 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -0,0 +1,6 @@ +package com.team4099.robot2023.subsystems.feeder + +import edu.wpi.first.wpilibj2.command.SubsystemBase +class Feeder(val io: FeederIO) : SubsystemBase() { + +} \ No newline at end of file From afeb5b0ae347115f98d046b291f7a3806d20abee Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Tue, 9 Jan 2024 18:32:02 -0500 Subject: [PATCH 21/58] Revert "create feeder branch" --- .../com/team4099/robot2023/subsystems/feeder/Feeder.kt | 6 ------ .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 4 ---- 2 files changed, 10 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt deleted file mode 100644 index bb51b835..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ /dev/null @@ -1,6 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt deleted file mode 100644 index 41ddbf24..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -interface FeederIO { -} \ No newline at end of file From e118fffb0f78577b207b9e5dfbe85eacd9068ce7 Mon Sep 17 00:00:00 2001 From: Hanish Rajan <141093134+fyrhanish@users.noreply.github.com> Date: Tue, 16 Jan 2024 20:11:49 -0500 Subject: [PATCH 22/58] Added Feeder class --- .../kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt | 4 ++++ .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 4 ++++ 2 files changed, 8 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt new file mode 100644 index 00000000..41603955 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.feeder + +class Feeder { +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt new file mode 100644 index 00000000..41ddbf24 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.feeder + +interface FeederIO { +} \ No newline at end of file From 525a6cd0155c7349dcd403de46f810ad12b81b25 Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Wed, 17 Jan 2024 18:19:21 -0500 Subject: [PATCH 23/58] Update Shooter.kt --- .../team4099/robot2023/subsystems/Shooter/Shooter.kt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index ede97530..2bf0c01d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,11 +18,11 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward -<<<<<<< HEAD + private var wristFeedforward: SimpleMotorFeedforward -======= + private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) ->>>>>>> 829a07586e50bf71782342ae9623c560237b502b +// >>>>>>> 829a07586e50bf71782342ae9623c560237b502b /* private val wristflywheelkP = @@ -67,7 +67,7 @@ class Shooter (val io: ShooterIO){ ) private var wristProfile = TrapezoidProfile( - wristConstraints, + TrapezoidProfile.Constraints, TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) @@ -103,7 +103,7 @@ fun periodic(){ if (wristPositionTarget!=lastWristPositionTarget){ val preProfileGenerate = Clock.fpgaTime wristProfile = TrapezoidProfile( - wristConstraints, + TrapezoidProfile.Constraints, TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) ) From 64ff6b3e259e71e8b8a77a3af1242f6ac26d0b19 Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Wed, 17 Jan 2024 18:28:39 -0500 Subject: [PATCH 24/58] Update Shooter.kt --- .../robot2023/subsystems/Shooter/Shooter.kt | 118 ++++++++++-------- 1 file changed, 65 insertions(+), 53 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 2bf0c01d..eec37766 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inSeconds @@ -18,13 +19,22 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward + private val wristkS = + LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) + ) + private val wristlkV = + LoggedTunableValue( + "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + ) + private val wristkA = + LoggedTunableValue( + "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }) + ) + val flywheelFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) + - private var wristFeedforward: SimpleMotorFeedforward - private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) -// >>>>>>> 829a07586e50bf71782342ae9623c560237b502b -/* private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) private val wristflywheelkI = @@ -34,14 +44,14 @@ class Shooter (val io: ShooterIO){ private val wristflywheelkD = LoggedTunableValue( "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - )*/ + ) 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){ - io.setflywheelVoltage(appliedVoltage) - }*/ + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setflywheelVoltage(appliedVoltage) + }*/ fun setWristVoltage(appliedVoltage: ElectricalPotential){ io.setWristVoltage(appliedVoltage) } @@ -64,59 +74,63 @@ class Shooter (val io: ShooterIO){ ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, //ShooterConstants.FEEDER_INIT_VOLTAGE - ) + ) private var wristProfile = TrapezoidProfile( - TrapezoidProfile.Constraints, + wristConstraints, TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) -fun periodic(){ - io.updateInputs(inputs) - var nextState = currentState - when (currentState) { - ShooterStates.UNINITIALIZED -> { - nextState = ShooterStates.ZERO - } - ShooterStates.ZERO ->{ - } - - ShooterStates.OPEN_LOOP ->{ - /* - setflywheelVoltage(flywheelTargetVoltage) - lastflywheelRunTime = Clock.fpgaTime -*/ - setWristVoltage(wristTargetVoltage) - lastWristRunTime = Clock.fpgaTime - - /* setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime*/ - if (isZeroed == true ){ + fun periodic(){ + io.updateInputs(inputs) + 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 + + /* setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime*/ + if (isZeroed == true ){ + nextState = fromRequestToState(currentRequest) + } + nextState = fromRequestToState(currentRequest) - ShooterStates.TARGETING_POSITION ->{ - - if (wristPositionTarget!=lastWristPositionTarget){ - val preProfileGenerate = Clock.fpgaTime - wristProfile = TrapezoidProfile( - TrapezoidProfile.Constraints, - 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 - setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO fix this error - Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) - } + + 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 + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + setWristPosition(wristProfile.calculate(timeElapsed)) + //TODO fix this error + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) + } @@ -141,6 +155,4 @@ fun periodic(){ } } -} - - +} \ No newline at end of file From f30fe02019403cae04807600f6b945d445c692be Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Wed, 17 Jan 2024 18:45:26 -0500 Subject: [PATCH 25/58] Made feeder skeleton --- .../config/constants/FeederConstants.kt | 17 +++++ .../robot2023/subsystems/feeder/Feeder.kt | 72 ++++++++++++++++++- .../robot2023/subsystems/feeder/FeederIO.kt | 32 ++++----- .../subsystems/superstructure/Request.kt | 21 ++---- 4 files changed, 108 insertions(+), 34 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt new file mode 100644 index 00000000..5c911459 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -0,0 +1,17 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts + +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 +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 41603955..82a69f6d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,4 +1,74 @@ package com.team4099.robot2023.subsystems.feeder -class Feeder { +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FeederConstants +import com.team4099.robot2023.subsystems.superstructure.Request +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 + +class Feeder(val io: FeederIO) { + val inputs = FeederIO.FeederIOInputs() + lateinit var flywheelFeedforward: SimpleMotorFeedforward + var currentState = FeederStates.UNINITIALIZED + var flywheelTargetVoltage : ElectricalPotential= 0.0.volts + var feederTargetVoltage : ElectricalPotential = 0.0.volts + + fun setFlywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setFlywheelVoltage(appliedVoltage) + } + + 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() { + io.updateInputs(inputs) + + val nextState = when (currentState) { + FeederStates.UNINITIALIZED -> { + FeederStates.IDLE + } + + FeederStates.IDLE -> { + setFlywheelVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) + fromRequestToState(currentRequest) + } + + FeederStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelTargetVoltage) + fromRequestToState(currentRequest) + } + } + + currentState = nextState + } + + companion object { + enum class FeederStates { + UNINITIALIZED, + IDLE, + OPEN_LOOP + } + + fun fromRequestToState(request: Request.FeederRequest): FeederStates { + return when (request) { + is Request.FeederRequest.Idle -> FeederStates.IDLE + is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP + } + } + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index e7baab4e..358dc038 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -11,7 +11,7 @@ import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts interface FeederIO { - class FeederIOInputs : LoggableInputs { + class FeederIOInputs: LoggableInputs { var floorAppliedVoltage = 0.0.volts var floorStatorCurrent = 0.0.amps var floorSupplyCurrent = 0.0.amps @@ -35,36 +35,36 @@ interface FeederIO { } override fun fromLog(table: LogTable?) { - table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { + table?.get("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { floorAppliedVoltage = it.volts } - table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { + table?.get("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { floorStatorCurrent = it.amps } - table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { + table?.get("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { floorSupplyCurrent = it.amps } - table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } - table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { + table?.get("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.get("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { verticalAppliedVoltage = it.volts } - table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + table?.get("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { verticalStatorCurrent = it.amps } - table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { verticalSupplyCurrent = it.amps } - table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } + table?.get("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } } + } - fun updateInputs(inputs: FeederIOInputs) {} + fun updateInputs(inputs: FeederIOInputs) {} - fun setFloorVoltage(voltage: ElectricalPotential) {} + fun setFlywheelVoltage(voltage: ElectricalPotential) {} - fun setVerticalVoltage(voltage: ElectricalPotential) {} - } + fun setFeederVoltage(voltage: ElectricalPotential) {} - interface FeederIO { -// >>>>>>> e118fffb0f78577b207b9e5dfbe85eacd9068ce7 - } + // fun setFloorVoltage(voltage: ElectricalPotential) {} + + // fun setVerticalVoltage(voltage: ElectricalPotential) {} } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 02093c3c..0620a8ec 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,12 +1,11 @@ package com.team4099.robot2023.subsystems.superstructure +import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier -import com.team4099.robot2023.subsystems.Shooter.Flywheel import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity -import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.Angle @@ -83,21 +82,9 @@ sealed interface Request { class ZeroSensors : DrivetrainRequest class Idle : DrivetrainRequest } - sealed interface ShooterRequest : Request { - class OpenLoop(wristVoltage : ElectricalPotential, - //rollerVoltage: ElectricalPotential, - //feederVoltage: ElectricalPotential - ):ShooterRequest{} - class TargetingPosition (val wristPosition : Angle, - //val flywheelVelocity: AngularVelocity, - //val feederVelocity: AngularVelocity - ):ShooterRequest{} - class Zero () : ShooterRequest{} - } - sealed interface FlywheelRequest : Request { - class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} - class TargetingVelocity (flywheelVelocity: AngularVelocity) - class Zero ():FlywheelRequest{} + sealed interface FeederRequest : Request { + class OpenLoop(flywheelVoltage: ElectricalPotential, feederVoltage: ElectricalPotential): FeederRequest {} + class Idle(): FeederRequest {} } } From 7184cdc9ec4b91758e01217a80d6eb4659a25bc0 Mon Sep 17 00:00:00 2001 From: 00magikarp <94652654+00magikarp@users.noreply.github.com> Date: Thu, 18 Jan 2024 18:39:07 -0500 Subject: [PATCH 26/58] celsius is spelled wrong :-1: --- .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 358dc038..f91f61e9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -27,11 +27,11 @@ interface FeederIO { table?.put("floorAppliedVoltage", floorAppliedVoltage.inVolts) table?.put("floorStatorCurrent", floorStatorCurrent.inAmperes) table?.put("floorSupplyCurrent", floorSupplyCurrent.inAmperes) - table?.put("floorTempCelcius", floorTemp.inCelsius) + table?.put("floorTempCelsius", floorTemp.inCelsius) table?.put("verticalAppliedVoltage", verticalAppliedVoltage.inVolts) table?.put("verticalStatorCurrent", verticalStatorCurrent.inAmperes) table?.put("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes) - table?.put("verticalTempCelcius", verticalTemp.inCelsius) + table?.put("verticalTempCelsius", verticalTemp.inCelsius) } override fun fromLog(table: LogTable?) { @@ -44,7 +44,7 @@ interface FeederIO { table?.get("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { floorSupplyCurrent = it.amps } - table?.get("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.get("floorTempCelsius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } table?.get("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { verticalAppliedVoltage = it.volts } @@ -54,7 +54,7 @@ interface FeederIO { table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { verticalSupplyCurrent = it.amps } - table?.get("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } + table?.get("verticalTempCelsius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } } } From 355f9bd9d5bbcae2b7af0d0a146575e3d582d1b6 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Thu, 18 Jan 2024 20:16:12 -0500 Subject: [PATCH 27/58] Removed flywheel and added PID constants --- .../config/constants/FeederConstants.kt | 27 ++++--- .../robot2023/subsystems/feeder/Feeder.kt | 70 ++++++++++++------- .../robot2023/subsystems/feeder/FeederIO.kt | 53 +++++--------- .../subsystems/superstructure/Request.kt | 2 +- 4 files changed, 85 insertions(+), 67 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 5c911459..362970ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -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, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 82a69f6d..418840d9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -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 - 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) { @@ -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) } } @@ -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 { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 358dc038..9189a251 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -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) {} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 0620a8ec..28bdf459 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -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 {} } } From f309b3580063da9d9cc3fc9993e3d7e4e3747967 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:00:45 -0500 Subject: [PATCH 28/58] Completed FeederIO --- .../robot2023/subsystems/feeder/FeederIO.kt | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 9189a251..8de21e7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,17 +1,18 @@ package com.team4099.robot2023.subsystems.feeder +import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.* 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.inCelsius -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.* interface FeederIO { class FeederIOInputs: LoggableInputs { + var feederVelocity = 0.0.rotations.perMinute var feederAppliedVoltage = 0.0.volts var feederStatorCurrent = 0.0.amps var feederSupplyCurrent = 0.0.amps @@ -20,6 +21,7 @@ interface FeederIO { var isSimulated = false override fun toLog(table: LogTable?) { + table?.put("feederVelocity", feederVelocity.inRadiansPerSecond) table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) @@ -27,6 +29,10 @@ interface FeederIO { } override fun fromLog(table: LogTable?) { + table?.get("feederVelocity", feederVelocity.inRadiansPerSecond)?.let { + feederVelocity = it.radians.perSecond + } + table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let { feederAppliedVoltage = it.volts } @@ -49,6 +55,12 @@ interface FeederIO { fun setFeederVoltage(voltage: ElectricalPotential) {} + fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {} + + fun setFeederBrakeMode(brake: Boolean) {} + + fun configPID(kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>) {} + // fun setFloorVoltage(voltage: ElectricalPotential) {} // fun setVerticalVoltage(voltage: ElectricalPotential) {} From e815efe57079244da18bf5859970498923d16606 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:09:51 -0500 Subject: [PATCH 29/58] Completed Feeder and FeederIO --- .../config/constants/FeederConstants.kt | 13 ++- .../robot2023/subsystems/feeder/Feeder.kt | 84 +++++++++++-------- .../subsystems/superstructure/Request.kt | 3 +- 3 files changed, 61 insertions(+), 39 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 362970ed..9bd6e98d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -17,12 +17,19 @@ import org.team4099.lib.units.perSecond object FeederConstants { val FEEDER_INIT_VOLTAGE = 0.0.volts + // TODO: Add value to Feeder target velocity + val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute + // 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, Volt> = 0.001.volts / 1.0.rotations.perMinute - val FEEDER_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val FEEDER_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + val FEEDER_REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_REAL_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + + val FEEDER_SIM_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_SIM_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_SIM_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 418840d9..95604908 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -3,83 +3,96 @@ 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.wpilibj.RobotBase 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.* import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import kotlin.Pair class Feeder(val io: FeederIO): SubsystemBase() { - val inputs = FeederIO.FeederIOInputs() - - var feederTargetVoltage : ElectricalPotential = 0.0.volts - - var lastFeederRunTime = 0.0.seconds - var lastFeederVoltage = 0.0.volts - - var isZeroed = false + val kP = LoggedTunableValue("Feeder/kP", Pair({it.inVoltsPerRotationPerMinute}, {it.volts / 1.0.rotations.perMinute})) + val kI = LoggedTunableValue("Feeder/kI", Pair({it.inVoltsPerRotations}, {it.volts / (1.0.rotations.perMinute * 1.0.seconds)})) + val kD = LoggedTunableValue("Feeder/kD", Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts / 1.0.rotations.perMinute.perSecond})) - 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) + val kS = LoggedTunableValue("Feeder/kS", FeederConstants.FEEDER_KS, Pair({it.inVolts}, {it.volts})) + val kV = LoggedTunableValue("Feeder/kV", FeederConstants.FEEDER_KV, Pair({it.inVoltsPerRotationPerMinute}, {it.volts.perRotation.perMinute})) + val kA = LoggedTunableValue("Feeder/kA", FeederConstants.FEEDER_KA, Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts.perRotation.perMinute.perSecond})) - 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 + val inputs = FeederIO.FeederIOInputs() - var currentState = FeederStates.UNINITIALIZED + var feederFeedForward: SimpleMotorFeedforward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + var feederTargetVoltage: ElectricalPotential = 0.0.volts + var feederTargetVelocity: AngularVelocity = FeederConstants.FEED_NOTE_TARGET_VELOCITY + var lastFeederRunTime = 0.0.seconds + var currentState: FeederStates = 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 + init { + if (RobotBase.isReal()) { + kP.initDefault(FeederConstants.FEEDER_REAL_KP) + kI.initDefault(FeederConstants.FEEDER_REAL_KI) + kD.initDefault(FeederConstants.FEEDER_REAL_KD) + } else { + kP.initDefault(FeederConstants.FEEDER_SIM_KP) + kI.initDefault(FeederConstants.FEEDER_SIM_KI) + kD.initDefault(FeederConstants.FEEDER_SIM_KD) } + } fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) } + fun setFeederVelocity(velocity: AngularVelocity) { + io.setFeederVelocity(velocity, feederFeedForward.calculate(velocity)) + } + override fun periodic() { io.updateInputs(inputs) - val nextState = when (currentState) { + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + + if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged()) { + feederFeedForward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + } + + val nextState: FeederStates = when (currentState) { FeederStates.UNINITIALIZED -> { FeederStates.IDLE } FeederStates.IDLE -> { setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } FeederStates.OPEN_LOOP -> { setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } - } - currentState = nextState + FeederStates.TARGETING_VELOCITY -> { + setFeederVelocity(feederTargetVelocity) + lastFeederRunTime = Clock.fpgaTime + fromRequestToState(currentRequest) + } + } } companion object { enum class FeederStates { UNINITIALIZED, IDLE, - OPEN_LOOP; + OPEN_LOOP, + TARGETING_VELOCITY; fun equivalentToRequest(request: Request.FeederRequest): Boolean { return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE)) @@ -90,6 +103,7 @@ class Feeder(val io: FeederIO): SubsystemBase() { return when (request) { is Request.FeederRequest.Idle -> FeederStates.IDLE is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP + is Request.FeederRequest.TargetingVelocity -> FeederStates.TARGETING_VELOCITY } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 28bdf459..66bfbe0b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -84,7 +84,8 @@ sealed interface Request { } sealed interface FeederRequest : Request { - class OpenLoop(val feederVoltage: ElectricalPotential, val flywheelVoltage: ElectricalPotential): FeederRequest {} class Idle(): FeederRequest {} + class OpenLoop(val feederVoltage: ElectricalPotential): FeederRequest {} + class TargetingVelocity(val feederVelocity: AngularVelocity): FeederRequest {} } } From 6dd280241314c6033e76f10828b77dc0e2081152 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:33:14 -0500 Subject: [PATCH 30/58] Rebased feeder --- .../kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 8de21e7c..d436f71c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -63,5 +63,6 @@ interface FeederIO { // fun setFloorVoltage(voltage: ElectricalPotential) {} + // fun setVerticalVoltage(voltage: ElectricalPotential) {} } \ No newline at end of file From 9ed42d4d26019ab29007fd041a8ef20c6adae647 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:35:33 -0500 Subject: [PATCH 31/58] Rebased feeder --- .../kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index d436f71c..8de21e7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -63,6 +63,5 @@ interface FeederIO { // fun setFloorVoltage(voltage: ElectricalPotential) {} - // fun setVerticalVoltage(voltage: ElectricalPotential) {} } \ No newline at end of file From 44dbde2bedf981024079570a6acab2715cca8649 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Wed, 10 Jan 2024 18:13:36 -0500 Subject: [PATCH 32/58] Started IO File --- .../robot2023/subsystems/Shooter/ShooterIO.kt | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt new file mode 100644 index 00000000..887217ca --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -0,0 +1,34 @@ +package com.team4099.robot2023.subsystems.Shooter + +interface ShooterIO () { + class ShooterIOInputs : LoggableInputs { + var rollerVelocity = 0.rotations.perMinute + var rollerAppliedVoltage = 0.volts + var rollerStatorCurrent = 0.amps + var rollerSupplyCurrent = 0.amps + var rollerTempreature = 0.celsius + } + fun toLog(table:LogTable){ + table?.put("rollerVelocityRPM", rollerVelocity.inRadians.perSecond) + table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + table?.put("rollerStatorCurrent", rollerStatorCurrent.inamps) + table?.put("rollerVelocityRPM", rollerSupplyCurrent.inamps) + table?.put("rollerTempreature", rollerTempreature.celsisus) + + } + fun fromLog(table:LogTable){ + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond)?.let{ + rollerVelocity = it.radians.perSecond + } + table?.getDouble("rollerAppliedVoltage", rollerAppliedVoltage.involts)?.let { + rollerAppliedVoltage = it.volts + } + table?.getDouble("rollerStatorCurrent", rollerStatorCurrent.inRadiansPerSecond)?.let{ + + } + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + + + } +} \ No newline at end of file From 5f33d2c9dc32f6400a3f6df710cc80272f5b44f2 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Wed, 10 Jan 2024 18:51:32 -0500 Subject: [PATCH 33/58] Finished IO File --- .../robot2023/subsystems/Shooter/ShooterIO.kt | 69 ++++++++++++++----- .../subsystems/Shooter/ShooterIONeo.kt | 5 ++ 2 files changed, 56 insertions(+), 18 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 887217ca..73c0b53d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -1,34 +1,67 @@ package com.team4099.robot2023.subsystems.Shooter -interface ShooterIO () { +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.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface ShooterIO { class ShooterIOInputs : LoggableInputs { var rollerVelocity = 0.rotations.perMinute var rollerAppliedVoltage = 0.volts var rollerStatorCurrent = 0.amps var rollerSupplyCurrent = 0.amps var rollerTempreature = 0.celsius - } - fun toLog(table:LogTable){ - table?.put("rollerVelocityRPM", rollerVelocity.inRadians.perSecond) - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) - table?.put("rollerStatorCurrent", rollerStatorCurrent.inamps) - table?.put("rollerVelocityRPM", rollerSupplyCurrent.inamps) - table?.put("rollerTempreature", rollerTempreature.celsisus) - } - fun fromLog(table:LogTable){ - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond)?.let{ - rollerVelocity = it.radians.perSecond - } - table?.getDouble("rollerAppliedVoltage", rollerAppliedVoltage.involts)?.let { - rollerAppliedVoltage = it.volts + 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?.getDouble("rollerStatorCurrent", rollerStatorCurrent.inRadiansPerSecond)?.let{ + + override fun fromLog(table: LogTable) { + 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 + } + } - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) - table?.getDouble("rollerVelocityRPM", rollerVelocityRPM.inRadiansPerSecond) + } + fun updateInputs(inputs: ShooterIOInputs) + fun setRollerPower (voltage: ElectricalPotential){ + + } + fun setRollerBrakeMode (brake: Boolean){ } + fun configPID( + kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain , + ){} + fun zeroEncoder(){ + + } + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt new file mode 100644 index 00000000..6ee6352b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -0,0 +1,5 @@ +package com.team4099.robot2023.subsystems.Shooter + +class ShooterIONeo { + val +} \ No newline at end of file From c14efddcc968749c6c8962abf629fdc5bb8f4fbc Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Thu, 11 Jan 2024 19:46:41 -0500 Subject: [PATCH 34/58] Did work on IO and IONeo for shooter also changed global mainpulator constants to be for shooter --- .../robot2023/config/constants/Constants.kt | 5 +-- .../config/constants/ShooterConstants.kt | 11 +++++ .../robot2023/subsystems/Shooter/ShooterIO.kt | 4 +- .../subsystems/Shooter/ShooterIONeo.kt | 42 ++++++++++++++++++- 4 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 4f1f2add..c5761a42 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -115,9 +115,8 @@ object Constants { const val PIGEON_2_ID = 1 } - object Manipulator { - const val INTAKE_MOTOR_ID = 51 - const val ARM_MOTOR_ID = 52 + object Shooter { + const val ROLLER_MOTOR_ID = 51 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt new file mode 100644 index 00000000..b9ff868c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -0,0 +1,11 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.gearRatio + +object ShooterConstants { + val ROLLER_GEAR_RATIO = 0.0.gearRatio + val ROLLER_VOLTAGE_COMPENSATION = 0.0.volts + val ROLLER_STATOR_CURRENT_LIMIT = 0.0.amps +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 73c0b53d..99fecfb5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -48,7 +48,9 @@ interface ShooterIO { } } - fun updateInputs(inputs: ShooterIOInputs) + fun updateInputs(inputs: ShooterIOInputs){ + + } fun setRollerPower (voltage: ElectricalPotential){ } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 6ee6352b..bd7db0bf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -1,5 +1,43 @@ package com.team4099.robot2023.subsystems.Shooter -class ShooterIONeo { - val +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ShooterConstants +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.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object ShooterIONeo : ShooterIO{ + private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + ) + init{ + //reset the motors + rollerSparkMax.restoreFactoryDefaults() + rollerSparkMax.clearFaults() + + //voltage and current limits + rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) + rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + } + + override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + } + } \ No newline at end of file From 14b7e897c637ef3060ee27515473e0533c1b4340 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Fri, 12 Jan 2024 21:22:36 -0500 Subject: [PATCH 35/58] added feeder and wrist and did requests and started states --- .../robot2023/config/constants/Constants.kt | 3 + .../config/constants/ShooterConstants.kt | 12 ++- .../robot2023/subsystems/Shooter/Shooter.kt | 93 +++++++++++++++++ .../robot2023/subsystems/Shooter/ShooterIO.kt | 99 ++++++++++++++++--- .../subsystems/Shooter/ShooterIONeo.kt | 48 ++++++++- .../subsystems/superstructure/Request.kt | 11 +++ 6 files changed, 249 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index c5761a42..694d144d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -117,6 +117,9 @@ object Constants { object Shooter { const val ROLLER_MOTOR_ID = 51 + //TODO find wrist motor id + const val SHOOTER_WRIST_MOTOR_ID = 999 + const val FEEDER_MOTOR_ID = 999 } object Alert { diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index b9ff868c..a3a394b5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -2,10 +2,18 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.derived.gearRatio + object ShooterConstants { - val ROLLER_GEAR_RATIO = 0.0.gearRatio + 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 } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index c1ef2b6f..a26d7169 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -1,3 +1,96 @@ package com.team4099.robot2023.subsystems.Shooter +<<<<<<< HEAD class Shooter +======= +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.units.Voltage +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.* + +class Shooter (val io: ShooterIO){ + val inputs = ShooterIO.ShooterIOInputs() + private var RollerFeedforward: SimpleMotorFeedforward + private var WristFeedforward: SimpleMotorFeedforward + + + private val wristRollerkP = + LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val wristRollerkI = + LoggedTunableValue( + "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val wristRollerkD = + LoggedTunableValue( + "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) + ) + var currentState = ShooterStates.UNINITIALIZED + var rollerTargetVoltage : ElectricalPotential= 0.0.volts + var wristTargetVoltage : ElectricalPotential = 0.0.volts + var feederTargetVoltage : ElectricalPotential = 0.0.volts + fun setRollerVoltage(appliedVoltage: ElectricalPotential){ + io.setRollerVoltage(appliedVoltage) + } + fun setWristVoltage(appliedVoltage: ElectricalPotential){ + io.setWristVoltage(appliedVoltage) + } + fun setFeederVoltage(appliedVoltage: ElectricalPotential){ + io.setFeederVoltage(appliedVoltage) + } + var lastRollerRunTime = 0.0.seconds + var lastWristRunTime = 0.0.seconds + var lastFeederRunTime = 0.0.seconds + var isZeroed : Boolean = false + var currentRequest = Request.ShooterRequest.OpenLoop +fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + ShooterStates.UNINITIALIZED -> { + nextState = ShooterStates.ZERO + } + ShooterStates.ZERO ->{ +//TODO write zero function for shooter + } + ShooterStates.IDLE ->{ +//TODO figure out if were + } + ShooterStates.OPEN_LOOP ->{ + setRollerVoltage(rollerTargetVoltage) + lastRollerRunTime = Clock.fpgaTime + + setWristVoltage(wristTargetVoltage) + lastWristRunTime = Clock.fpgaTime + + setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime + if (isZeroed == true ){ + nextState = fromRequestToState(currentRequest) + } + + } + + ShooterStates.TARGETING_POSITION ->{ + + + } + + } + +} + + companion object { + enum class ShooterStates{ + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_POSITION, + } + } +} +>>>>>>> ec012fa (added feeder and wrist and did requests and started states) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 99fecfb5..f1ab9e6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -16,48 +16,121 @@ interface ShooterIO { var rollerSupplyCurrent = 0.amps var rollerTempreature = 0.celsius + var wristPostion = 0.degrees + var wristAppliedVoltage = 0.volts + var wristStatorCurrent = 0.amps + 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 + 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("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("wristAppliedVoltage", wristAppliedVoltage.inVolts) + table.put("wristStatorCurrent", wristStatorCurrent.inAmperes) + table.put("wristSupplyCurrent", wristSupplyCurrent.inAmperes) + table.put("wristTemperature", wristTemperature.inCelsius) + + table.put("feederVelocity", 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) { - table?.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond)?.let { + //roller logs + table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let { rollerVelocity = it.radians.perSecond } - table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { + table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let { rollerAppliedVoltage = it.volts } - table?.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes)?.let { + table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let { rollerStatorCurrent = it.amps } - table?.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes)?.let { + table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let { rollerSupplyCurrent = it.amps } - - table?.get("rollerTempreature", rollerTempreature.inCelsius)?.let { + table.get("rollerTempreature", rollerTempreature.inCelsius).let { rollerTempreature = it.celsius } +//wrist logs + table.get("wristPostion", wristPostion.inDegrees).let { + wristPostion = it.degrees + } + table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { + wristAppliedVoltage = it.volts + } + table.get("wristStatorCurrent", wristStatorCurrent.inAmperes).let { + wristStatorCurrent = it.amps + } + table.get("wristSupplyCurrent", wristSupplyCurrent.inAmperes).let { + wristSupplyCurrent = it.amps + } + 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 setRollerPower (voltage: ElectricalPotential){ + fun setRollerVoltage (voltage: ElectricalPotential){ + + } + fun setWristVoltage (voltage: ElectricalPotential){ + + } + fun setFeederVoltage (voltage: ElectricalPotential){ + + } + fun setWristPosition (voltage: ElectricalPotential){ } fun setRollerBrakeMode (brake: Boolean){ } - fun configPID( + fun setFeederBrakeMode (brake: Boolean){ + + } + + + fun configWristPID( kP: ProportionalGain , kI: IntegralGain , kD: DerivativeGain , diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index bd7db0bf..b88f3cad 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -7,25 +7,49 @@ import com.team4099.robot2023.config.constants.ShooterConstants 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.inDegrees import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue - +//TODO write a kraken file object ShooterIONeo : ShooterIO{ private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - ShooterConstants.ROLLER_GEAR_RATIO.asDrivenOverDriving, + ShooterConstants.ROLLER_GEAR_RATIO, ShooterConstants.ROLLER_VOLTAGE_COMPENSATION ) + +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 + ) + private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, + ShooterConstants.FEEDER_GEAR_RATIO, + ShooterConstants.FEEDER_VOLTAGE_COMPENSATION + ) init{ //reset the motors rollerSparkMax.restoreFactoryDefaults() rollerSparkMax.clearFaults() + wristSparkMax.restoreFactoryDefaults() + wristSparkMax.clearFaults() + + feederSparkMax.restoreFactoryDefaults() + feederSparkMax.clearFaults() + //voltage and current limits 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()) + + feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) + feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) } override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ @@ -38,6 +62,26 @@ object ShooterIONeo : ShooterIO{ // percentOutput * statorCurrent inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius + + inputs.wristPostion = wristSensor.position + inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput + inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue + inputs.wristTemperature = wristSparkMax.motorTemperature.celsius + + inputs.feederVelocity = feederSensor.velocity + inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput + inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps + // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue + inputs.feederTemperature = feederSparkMax.motorTemperature.celsius } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 200f125b..116dbc1e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -5,6 +5,7 @@ import com.team4099.robot2023.config.constants.NodeTier import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential @@ -31,4 +32,14 @@ sealed interface Request { class ZeroSensors : DrivetrainRequest class Idle : DrivetrainRequest } + sealed interface ShooterRequest : Request { + class OpenLoop(wristVoltage : ElectricalPotential, + rollerVoltage: ElectricalPotential, + feederVoltage: ElectricalPotential):ShooterRequest{} + class TargetingPosition (val wristPosition : Angle, + val rollerVelocity: AngularVelocity, + val feederVelocity: AngularVelocity + ):ShooterRequest{} + + } } From 3eb7677002150393ff9db6026709d11a8dc1bb9f Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 13 Jan 2024 19:53:27 -0500 Subject: [PATCH 36/58] Did more targeting position state and open loop state --- .../config/constants/ShooterConstants.kt | 8 ++ .../robot2023/subsystems/Shooter/Shooter.kt | 74 +++++++++++++++---- .../robot2023/subsystems/Shooter/ShooterIO.kt | 14 +++- .../subsystems/Shooter/ShooterIONeo.kt | 1 + .../subsystems/superstructure/Request.kt | 1 + 5 files changed, 84 insertions(+), 14 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index a3a394b5..887f95fb 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -1,6 +1,7 @@ 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.volts @@ -16,4 +17,11 @@ object ShooterConstants { 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 + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index a26d7169..ba8c20e3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -1,17 +1,21 @@ package com.team4099.robot2023.subsystems.Shooter -<<<<<<< HEAD -class 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.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.units.Voltage +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() @@ -46,7 +50,26 @@ class Shooter (val io: ShooterIO){ var lastWristRunTime = 0.0.seconds var lastFeederRunTime = 0.0.seconds var isZeroed : Boolean = false - var currentRequest = Request.ShooterRequest.OpenLoop + private var lastRollerVoltage = 0.0.volts + //TODO ask what to set this too + private var wristPositionTarget = 0.0.degrees + private var lastWristPositionTarget = 0.0.degrees + private var rollerInitVoltage = LoggedTunableValue ("Shooter/Initial Roller 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 timeProfileGeneratedAt = 0.0.seconds + //TODO ask aanshi wtf to pass in for init parameters + var currentRequest = Request.ShooterRequest.OpenLoop( + ShooterConstants.WRIST_INIT_VOLTAGE, + ShooterConstants.ROLLLER_INIT_VOLTAGE, + ShooterConstants.FEEDER_INIT_VOLTAGE) + private var wristProfile = + TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), + TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) + ) + fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -55,11 +78,9 @@ fun periodic(){ nextState = ShooterStates.ZERO } ShooterStates.ZERO ->{ -//TODO write zero function for shooter - } - ShooterStates.IDLE ->{ -//TODO figure out if were +//TODO create zero encoder if we get one } + ShooterStates.OPEN_LOOP ->{ setRollerVoltage(rollerTargetVoltage) lastRollerRunTime = Clock.fpgaTime @@ -77,20 +98,47 @@ fun periodic(){ ShooterStates.TARGETING_POSITION ->{ - + 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 + setwristPosition(wristProfile.calculate(timeElapsed)) + //TODO implement set wrist pos function + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } - } -} - companion object { + + } + + } + companion object{ enum class ShooterStates{ UNINITIALIZED, ZERO, OPEN_LOOP, TARGETING_POSITION, } + inline fun fromRequestToState(request: Request.ShooterRequest): ShooterStates { + return when (request) { + is Request.ShooterRequest.OpenLoop -> ShooterStates.OPEN_LOOP + is Request.ShooterRequest.TargetingPosition -> ShooterStates.TARGETING_POSITION + is Request.ShooterRequest.Zero -> ShooterStates.ZERO + + } + } } + } ->>>>>>> ec012fa (added feeder and wrist and did requests and started states) + + diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index f1ab9e6f..d1d98b98 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -4,6 +4,7 @@ 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 @@ -17,6 +18,7 @@ interface ShooterIO { var rollerTempreature = 0.celsius var wristPostion = 0.degrees + var wristVelocity = 0.radians.perSecond var wristAppliedVoltage = 0.volts var wristStatorCurrent = 0.amps var wristSupplyCurrent = 0.amps @@ -36,12 +38,13 @@ interface ShooterIO { 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("feederVelocity", feederVelocity.inRadiansPerSecond) + table.put("feederVelocityRPM", feederVelocity.inRadiansPerSecond) table.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) table.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) @@ -67,10 +70,16 @@ interface ShooterIO { table.get("rollerTempreature", rollerTempreature.inCelsius).let { rollerTempreature = it.celsius } + + + //wrist logs table.get("wristPostion", wristPostion.inDegrees).let { wristPostion = it.degrees } + table.get("wristVelocity", wristVelocity.inRadiansPerSecond).let { + wristVelocity = it.radians.perSecond + } table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { wristAppliedVoltage = it.volts } @@ -84,6 +93,9 @@ interface ShooterIO { table.get("wristTemperature", wristTemperature.inCelsius).let { wristTemperature = it.celsius } + + + //feeder table.get("feederVelocity", feederVelocity.inRadiansPerSecond).let { feederVelocity = it.radians.perSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index b88f3cad..8f6044c3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -84,4 +84,5 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.feederTemperature = feederSparkMax.motorTemperature.celsius } + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 116dbc1e..a184539c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -40,6 +40,7 @@ sealed interface Request { val rollerVelocity: AngularVelocity, val feederVelocity: AngularVelocity ):ShooterRequest{} + class Zero () : ShooterRequest{} } } From 681044974b57e2f0ba96bcc0cbbbfb7364bbe128 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:46:01 -0500 Subject: [PATCH 37/58] Implemeted functions from IO in IONEO --- .../config/constants/ShooterConstants.kt | 22 +++-- .../robot2023/subsystems/Shooter/Shooter.kt | 58 +++++------ .../robot2023/subsystems/Shooter/ShooterIO.kt | 20 ++-- .../subsystems/Shooter/ShooterIONeo.kt | 98 ++++++++++++++----- .../subsystems/superstructure/Request.kt | 9 +- 5 files changed, 131 insertions(+), 76 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index 887f95fb..5e813dd4 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -2,26 +2,30 @@ 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 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 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 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 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/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index ba8c20e3..f9b1c3d7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -3,66 +3,65 @@ 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.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain.Companion.fromRequestToState +import com.team4099.robot2023.subsystems.Shooter.ShooterIONeo.setWristPosition import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.units.Voltage import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.controller.TrapezoidProfile + import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() - private var RollerFeedforward: SimpleMotorFeedforward + //TODO do feedforward private var WristFeedforward: SimpleMotorFeedforward - - private val wristRollerkP = +/* + private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val wristRollerkI = + private val wristflywheelkI = LoggedTunableValue( "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) - private val wristRollerkD = + private val wristflywheelkD = LoggedTunableValue( "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - ) + )*/ var currentState = ShooterStates.UNINITIALIZED - var rollerTargetVoltage : ElectricalPotential= 0.0.volts + var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts - fun setRollerVoltage(appliedVoltage: ElectricalPotential){ - io.setRollerVoltage(appliedVoltage) - } + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setflywheelVoltage(appliedVoltage) + }*/ fun setWristVoltage(appliedVoltage: ElectricalPotential){ io.setWristVoltage(appliedVoltage) } - fun setFeederVoltage(appliedVoltage: ElectricalPotential){ + /*fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) - } - var lastRollerRunTime = 0.0.seconds + }*/ + /*var lastFlywheelRunTime = 0.0.seconds + var lastFeederRunTime = 0.0.seconds*/ var lastWristRunTime = 0.0.seconds - var lastFeederRunTime = 0.0.seconds var isZeroed : Boolean = false - private var lastRollerVoltage = 0.0.volts + 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 rollerInitVoltage = LoggedTunableValue ("Shooter/Initial Roller Voltage", ShooterConstants.ROLLLER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) + /*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", ShooterConstants.WRIST_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) private var timeProfileGeneratedAt = 0.0.seconds //TODO ask aanshi wtf to pass in for init parameters var currentRequest = Request.ShooterRequest.OpenLoop( ShooterConstants.WRIST_INIT_VOLTAGE, - ShooterConstants.ROLLLER_INIT_VOLTAGE, - ShooterConstants.FEEDER_INIT_VOLTAGE) + //ShooterConstants.ROLLLER_INIT_VOLTAGE, + //ShooterConstants.FEEDER_INIT_VOLTAGE + ) private var wristProfile = TrapezoidProfile( wristConstraints, @@ -82,14 +81,15 @@ fun periodic(){ } ShooterStates.OPEN_LOOP ->{ - setRollerVoltage(rollerTargetVoltage) - lastRollerRunTime = Clock.fpgaTime - + /* + setflywheelVoltage(flywheelTargetVoltage) + lastflywheelRunTime = Clock.fpgaTime +*/ setWristVoltage(wristTargetVoltage) lastWristRunTime = Clock.fpgaTime - setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime + /* setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime*/ if (isZeroed == true ){ nextState = fromRequestToState(currentRequest) } @@ -111,7 +111,7 @@ fun periodic(){ lastWristPositionTarget = wristPositionTarget } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setwristPosition(wristProfile.calculate(timeElapsed)) + setWristPosition(wristProfile.calculate(timeElapsed)) //TODO implement set wrist pos function Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index d1d98b98..5fb6b2d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -122,25 +122,27 @@ interface ShooterIO { fun updateInputs(inputs: ShooterIOInputs){ } - fun setRollerVoltage (voltage: ElectricalPotential){ + /*fun setRollerVoltage (voltage: ElectricalPotential){ - } + }*/ fun setWristVoltage (voltage: ElectricalPotential){ } - fun setFeederVoltage (voltage: ElectricalPotential){ + //fun setFeederVoltage (voltage: ElectricalPotential){ - } - fun setWristPosition (voltage: ElectricalPotential){ +// } + fun setWristPosition (position : Angle, feedforward : ElectricalPotential){ } - fun setRollerBrakeMode (brake: Boolean){ + //fun setRollerBrakeMode (brake: Boolean){ - } - fun setFeederBrakeMode (brake: Boolean){ + //} + //fun setFeederBrakeMode (brake: Boolean){ - } + // } + fun setWristBrakeMode (brake: Boolean){ + } fun configWristPID( kP: ProportionalGain , diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 8f6044c3..060434f7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -2,58 +2,60 @@ package com.team4099.robot2023.subsystems.Shooter import com.revrobotics.CANSparkMax 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 org.team4099.lib.units.base.Meter 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.inDegrees -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue //TODO write a kraken file object ShooterIONeo : ShooterIO{ - private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - ShooterConstants.ROLLER_GEAR_RATIO, - ShooterConstants.ROLLER_VOLTAGE_COMPENSATION - ) + //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, + //ShooterConstants.ROLLER_GEAR_RATIO, + // ShooterConstants.ROLLER_VOLTAGE_COMPENSATION + // ) 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 ) - private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, - ShooterConstants.FEEDER_GEAR_RATIO, - ShooterConstants.FEEDER_VOLTAGE_COMPENSATION - ) + private val wristPIDController : SparkMaxPIDController = wristSparkMax.pidController + //private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + // private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, + //ShooterConstants.FEEDER_GEAR_RATIO, + //ShooterConstants.FEEDER_VOLTAGE_COMPENSATION + //) init{ //reset the motors - rollerSparkMax.restoreFactoryDefaults() - rollerSparkMax.clearFaults() + //rollerSparkMax.restoreFactoryDefaults() + // rollerSparkMax.clearFaults() wristSparkMax.restoreFactoryDefaults() wristSparkMax.clearFaults() - feederSparkMax.restoreFactoryDefaults() - feederSparkMax.clearFaults() + //feederSparkMax.restoreFactoryDefaults() + // feederSparkMax.clearFaults() //voltage and current limits - rollerSparkMax.enableVoltageCompensation(ShooterConstants.ROLLER_VOLTAGE_COMPENSATION.inVolts) - rollerSparkMax.setSmartCurrentLimit(ShooterConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + // 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()) - feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) - feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_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){ - inputs.rollerVelocity = rollerSensor.velocity + /* inputs.rollerVelocity = rollerSensor.velocity inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent @@ -62,7 +64,7 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID // percentOutput * statorCurrent inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius - +*/ inputs.wristPostion = wristSensor.position inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps @@ -73,7 +75,7 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue inputs.wristTemperature = wristSparkMax.motorTemperature.celsius - inputs.feederVelocity = feederSensor.velocity + /* inputs.feederVelocity = feederSensor.velocity inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent @@ -81,8 +83,54 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = // percentOutput * statorCurrent inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue - inputs.feederTemperature = feederSparkMax.motorTemperature.celsius + inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ + } + override fun configWristPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { +//TODO fix this please + wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) + wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) + wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) + } + + override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { + wristPIDController.setReference( + wristSensor.positionToRawUnits( + clamp( + position, + ShooterConstants.WRIST_SOFTLIMIT_DOWNWARDSTURN, + ShooterConstants.WRIST_SOFTLIMIT_UPWARDSTURN + ) + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts + ) + } + + override fun setWristVoltage(voltage: ElectricalPotential) { + wristSparkMax.setVoltage( + clamp( + voltage, + -ShooterConstants.WRIST_VOLTAGE_COMPENSATION , + ShooterConstants.WRIST_VOLTAGE_COMPENSATION + ) + .inVolts + ) } + override fun setWristBrakeMode(brake: Boolean) { + if(brake){ + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) + }else{ + wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast) + } + } + override fun zeroEncoder() { + wristSparkMax.encoder.position = 0.0 + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index a184539c..2aac629e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -34,11 +34,12 @@ sealed interface Request { } sealed interface ShooterRequest : Request { class OpenLoop(wristVoltage : ElectricalPotential, - rollerVoltage: ElectricalPotential, - feederVoltage: ElectricalPotential):ShooterRequest{} + //rollerVoltage: ElectricalPotential, + //feederVoltage: ElectricalPotential + ):ShooterRequest{} class TargetingPosition (val wristPosition : Angle, - val rollerVelocity: AngularVelocity, - val feederVelocity: AngularVelocity + //val flywheelVelocity: AngularVelocity, + //val feederVelocity: AngularVelocity ):ShooterRequest{} class Zero () : ShooterRequest{} From 10ddca588cb629aee64dc0d519ee711405b5ac9a Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sun, 14 Jan 2024 21:43:47 -0500 Subject: [PATCH 38/58] Added flywheel subsystem --- .../robot2023/subsystems/Shooter/Flywheel.kt | 67 ++++++++++++++ .../subsystems/Shooter/FlywheelConstants.kt | 44 ++++++++++ .../subsystems/Shooter/FlywheelIO.kt | 63 ++++++++++++++ .../subsystems/Shooter/FlywheelIOFalcon.kt | 87 +++++++++++++++++++ .../robot2023/subsystems/Shooter/Shooter.kt | 6 +- .../subsystems/Shooter/ShooterIONeo.kt | 1 - 6 files changed, 263 insertions(+), 5 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt new file mode 100644 index 00000000..dd364355 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -0,0 +1,67 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* + +class Flywheel (val io: FlywheelIO) { + var feedforward = SimpleMotorFeedforward() + val inputs = FlywheelIO.FlywheelIOInputs() + private val flywheelkS = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val flywheelkkV = + LoggedTunableValue( + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + ) + private val flywheelkA = + LoggedTunableValue( + "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + ) + + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts + fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { + io.setFlywheelVoltage(appliedVoltage) } + + var lastFlywheelRunTime = 0.0.seconds + private var lastFlywheelVoltage = 0.0.volts + private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) + private var hasNote:Boolean = true + var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + Flywheel.Companion.FlywheelStates.UNINITIALIZED -> { + nextState = Flywheel.Companion.FlywheelStates.ZERO + } + Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelTargetVoltage) + lastFlywheelRunTime = Clock.fpgaTime + } + Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ + if (flywheelTargetVoltage != lastFlywheelVoltage){ + if(hasNote){ + + } + } + } + Flywheel.Companion.FlywheelStates.ZERO ->{ + + } + } + + } + + companion object { + enum class FlywheelStates { + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_VELOCITY, + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt new file mode 100644 index 00000000..64bff2c7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt @@ -0,0 +1,44 @@ +package com.team4099.robot2023.subsystems.Shooter + +import org.team4099.lib.units.derived.volts + +object FlywheelConstants { + val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts + val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts + val ROLLER_GEAR_RATIO = 0.0 + + object PID { + + val AUTO_POS_KP: ProportionalGain> + get() { + if (RobotBase.isReal()) { + return 8.0.meters.perSecond / 1.0.meters + } else { + return 7.0.meters.perSecond / 1.0.meters + } + } + val AUTO_POS_KI: IntegralGain> + get() { + if (RobotBase.isReal()) { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } else { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } + } + + val AUTO_POS_KD: DerivativeGain> + get() { + if (RobotBase.isReal()) { + return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } else { + return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } + + } + } + val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds + val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt new file mode 100644 index 00000000..084077f8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -0,0 +1,63 @@ +package com.team4099.robot2023.subsystems.Shooter + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface FlywheelIO { + + class FlywheelIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + var rollerAppliedVoltage = 0.volts + var rollerStatorCurrent = 0.amps + var rollerSupplyCurrent = 0.amps + var rollerTempreature = 0.celsius + + 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) + } + 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 + } + } + } + fun setFlywheelVoltage (voltage: ElectricalPotential){ + + } + fun setFlywheelBrakeMode (brake: Boolean){ + + } + fun updateInputs(inputs:FlywheelIOInputs){ + + } + fun zeroEncoder(){ + + } + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt new file mode 100644 index 00000000..f66967d0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -0,0 +1,87 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.ctreAngularMechanismSensor + +class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ +private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelSensor = + ctreAngularMechanismSensor( + flywheelFalcon, + FlywheelConstants.ROLLER_GEAR_RATIO, + FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, + + ) + val flywheelStatorCurrentSignal: StatusSignal + val flywheelSupplyCurrentSignal: StatusSignal + val flywheelTempSignal: StatusSignal + init { + flywheelFalcon.configurator.apply(TalonFXConfiguration()) + + flywheelFalcon.clearStickyFaults() + flywheelFalcon.configurator.apply(flywheelConfiguration) +//TODO fix PID + flywheelConfiguration.Slot0.kP = + flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelConfiguration.Slot0.kI = + flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelConfiguration.Slot0.kD = + flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelConfiguration.Slot0.kV = 0.05425 + // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) + flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + flywheelFalcon.configurator.apply(flywheelConfiguration) + + flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent + flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent + flywheelTempSignal = flywheelFalcon.deviceTemp + + MotorChecker.add( + "flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + } + + + + + + override fun setFlywheelBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + + if (brake) { + motorOutputConfig.NeutralMode = NeutralModeValue.Brake + } else { + motorOutputConfig.NeutralMode = NeutralModeValue.Coast + } + flywheelFalcon.configurator.apply(motorOutputConfig) + } + override fun zeroEncoder(){ + //TODO finish zero encoder fun (ask sumone what the encoder for falcon is) + flywheelFalcon + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index f9b1c3d7..8064d58f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -32,7 +32,7 @@ class Shooter (val io: ShooterIO){ "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) )*/ var currentState = ShooterStates.UNINITIALIZED - var flywheelTargetVoltage : ElectricalPotential= 0.0.volts + //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ @@ -56,7 +56,6 @@ class Shooter (val io: ShooterIO){ 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 timeProfileGeneratedAt = 0.0.seconds - //TODO ask aanshi wtf to pass in for init parameters var currentRequest = Request.ShooterRequest.OpenLoop( ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, @@ -77,7 +76,6 @@ fun periodic(){ nextState = ShooterStates.ZERO } ShooterStates.ZERO ->{ -//TODO create zero encoder if we get one } ShooterStates.OPEN_LOOP ->{ @@ -112,7 +110,7 @@ fun periodic(){ } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO implement set wrist pos function + //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 060434f7..9100063d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -13,7 +13,6 @@ import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue -//TODO write a kraken file object ShooterIONeo : ShooterIO{ //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, From 38808e645846c3dfcec87886582932724972b4d5 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Mon, 15 Jan 2024 18:56:09 -0500 Subject: [PATCH 39/58] Started flywheel PID and did feedforward --- .../constants}/FlywheelConstants.kt | 3 +- .../robot2023/subsystems/Shooter/Flywheel.kt | 23 +++++++---- .../subsystems/Shooter/FlywheelIO.kt | 7 ++++ .../subsystems/Shooter/FlywheelIOFalcon.kt | 40 ++++++++++++++++--- .../robot2023/subsystems/Shooter/Shooter.kt | 2 +- .../robot2023/subsystems/Shooter/ShooterIO.kt | 6 +-- .../subsystems/Shooter/ShooterIONeo.kt | 7 ++-- .../subsystems/superstructure/Request.kt | 6 +++ 8 files changed, 72 insertions(+), 22 deletions(-) rename src/main/kotlin/com/team4099/robot2023/{subsystems/Shooter => config/constants}/FlywheelConstants.kt (94%) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt similarity index 94% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt rename to src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 64bff2c7..74f5903f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,5 +1,6 @@ -package com.team4099.robot2023.subsystems.Shooter +package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object FlywheelConstants { 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 dd364355..9573314f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -2,25 +2,30 @@ package com.team4099.robot2023.subsystems.Shooter import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute class Flywheel (val io: FlywheelIO) { - var feedforward = SimpleMotorFeedforward() + val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val flywheelkkV = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian })) + private val flywheelkV = LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond }) ) + val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { @@ -30,7 +35,9 @@ class Flywheel (val io: FlywheelIO) { private var lastFlywheelVoltage = 0.0.volts private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) private var hasNote:Boolean = true + val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -44,9 +51,9 @@ class Flywheel (val io: FlywheelIO) { } Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ - if(hasNote){ + val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - } + io.setFlywheelVoltage(controlEffort) } } Flywheel.Companion.FlywheelStates.ZERO ->{ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt index 084077f8..e0920b36 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.Shooter import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes @@ -47,6 +48,7 @@ interface FlywheelIO { } } } + fun setFlywheelVoltage (voltage: ElectricalPotential){ } @@ -59,5 +61,10 @@ interface FlywheelIO { fun zeroEncoder(){ } + fun configPID(kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){ + + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt index f66967d0..73484e57 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -2,16 +2,26 @@ package com.team4099.robot2023.subsystems.Shooter import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter 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.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ -private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelPIDController = TalonFX + private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelSensor = ctreAngularMechanismSensor( flywheelFalcon, @@ -22,6 +32,15 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() val flywheelStatorCurrentSignal: StatusSignal val flywheelSupplyCurrentSignal: StatusSignal val flywheelTempSignal: StatusSignal + private val kP = + LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val kD = + LoggedTunableValue( + "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -29,11 +48,11 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() flywheelFalcon.configurator.apply(flywheelConfiguration) //TODO fix PID flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelSensor.proportionalVelocityGainToRawUnits(kP) flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelSensor.integralVelocityGainToRawUnits(kI) flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelSensor.derivativeVelocityGainToRawUnits(kD) flywheelConfiguration.Slot0.kV = 0.05425 // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = @@ -65,9 +84,20 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() ) ) } + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + val PIDConfig = Slot0Configs() + PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP) + PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI) + PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD) + PIDConfig.kV = 0.05425 - + flywheelFalcon.configurator.apply(PIDConfig) + } override fun setFlywheelBrakeMode(brake: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 8064d58f..b80b67f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,7 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward + private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) /* private val wristflywheelkP = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 5fb6b2d1..757a55f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -145,9 +145,9 @@ interface ShooterIO { } fun configWristPID( - kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain , + kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){} fun zeroEncoder(){ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 9100063d..4fd485dc 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -85,11 +85,10 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ } override fun configWristPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain ) { -//TODO fix this please wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 2aac629e..c02af68b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier +import com.team4099.robot2023.subsystems.Shooter.Flywheel import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity @@ -44,4 +45,9 @@ sealed interface Request { class Zero () : ShooterRequest{} } + sealed interface FlywheelRequest : Request { + class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} + class TargetingVelocity (flywheelVelocity: AngularVelocity) + class Zero ():FlywheelRequest{} + } } From 219da3f2d025f38bb5975f9447c2f3674c0acb3f Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:12:10 -0500 Subject: [PATCH 40/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../robot2023/config/constants/Constants.kt | 2 +- .../config/constants/FlywheelConstants.kt | 13 +++++- .../robot2023/subsystems/Shooter/Flywheel.kt | 26 ++++++++++-- .../subsystems/Shooter/FlywheelIO.kt | 13 +++--- .../subsystems/Shooter/FlywheelIOFalcon.kt | 41 +++++++++---------- .../robot2023/subsystems/Shooter/Shooter.kt | 4 +- 6 files changed, 63 insertions(+), 36 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 694d144d..31afc6c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -116,7 +116,7 @@ object Constants { } object Shooter { - const val ROLLER_MOTOR_ID = 51 + const val FLYWHEEL_MOTOR_ID = 51 //TODO find wrist motor id const val SHOOTER_WRIST_MOTOR_ID = 999 const val FEEDER_MOTOR_ID = 999 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 74f5903f..8e96e0af 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,7 +1,10 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perMinute object FlywheelConstants { val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts @@ -42,4 +45,12 @@ object FlywheelConstants { val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps + val SHOOTER_FLYWHEEL_KP: ProportionalGain, Volt> = + 0.001.volts / 1.0.rotations.perMinute + val SHOOTER_FLYWHEEL_KI: IntegralGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val SHOOTER_FLYWHEEL_KD: DerivativeGain, Volt> = + 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + + } \ 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 9573314f..960c48fe 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -1,8 +1,12 @@ package com.team4099.robot2023.subsystems.Shooter +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.ControlModeValue import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.Meter @@ -12,19 +16,30 @@ import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perMinute class Flywheel (val io: FlywheelIO) { + val motor = TalonFX(Constants.Shooter.FLYWHEEL_MOTOR_ID) + private val kP = + LoggedTunableValue("Flywheel/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP) + private val kI = + LoggedTunableValue( + "Flywheel/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI) + private val kD = + LoggedTunableValue( + "Flywheel/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD) + val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian })) + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian }) + ) private val flywheelkV = LoggedTunableValue( "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) ) private val flywheelkA = LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond }) + "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perRadianPerSecond }) ) - val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) + val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) var flywheelTargetVoltage: ElectricalPotential = 0.0.volts @@ -37,7 +52,10 @@ class Flywheel (val io: FlywheelIO) { private var hasNote:Boolean = true val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED +init{ + io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) +} fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -52,7 +70,7 @@ class Flywheel (val io: FlywheelIO) { Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - + io.setFlywheelVelocity()//TODO talk to anshi ab a current velocity var io.setFlywheelVoltage(controlEffort) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt index e0920b36..78d9a9b7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -3,14 +3,12 @@ package com.team4099.robot2023.subsystems.Shooter import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.* 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.inCelsius import org.team4099.lib.units.derived.* -import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.perMinute -import org.team4099.lib.units.perSecond interface FlywheelIO { @@ -51,6 +49,9 @@ interface FlywheelIO { fun setFlywheelVoltage (voltage: ElectricalPotential){ + } + fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ + } fun setFlywheelBrakeMode (brake: Boolean){ @@ -61,9 +62,9 @@ interface FlywheelIO { fun zeroEncoder(){ } - fun configPID(kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain ){ + fun configPID(kP: ProportionalGain , Volt>, + kI: IntegralGain , Volt>, + kD: DerivativeGain , Volt>){ } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt index 73484e57..fdd8df4b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -11,16 +11,15 @@ import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.Meter -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.* import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ - private val flywheelPIDController = TalonFX + private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelSensor = ctreAngularMechanismSensor( @@ -29,18 +28,9 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, ) - val flywheelStatorCurrentSignal: StatusSignal - val flywheelSupplyCurrentSignal: StatusSignal - val flywheelTempSignal: StatusSignal - private val kP = - LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val kI = - LoggedTunableValue( - "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) - ) - private val kD = - LoggedTunableValue( - "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) + var flywheelStatorCurrentSignal: StatusSignal + var flywheelSupplyCurrentSignal: StatusSignal + var flywheelTempSignal: StatusSignal init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -48,11 +38,11 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelFalcon.configurator.apply(flywheelConfiguration) //TODO fix PID flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(kP) + flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(kI) + flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(kD) + flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) flywheelConfiguration.Slot0.kV = 0.05425 // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = @@ -74,7 +64,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelTempSignal = flywheelFalcon.deviceTemp MotorChecker.add( - "flywheel", + "Shooter","Flywheel", MotorCollection( mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, @@ -84,7 +74,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ ) ) } - override fun configureDrivePID( + override fun configPID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt> @@ -98,6 +88,13 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ flywheelFalcon.configurator.apply(PIDConfig) } + override fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ + flywheelFalcon.setControl(0, + flywheelSensor.velocityToRawUnits(angularVelocity), + DemandType.ArbitraryFeedForward, + feedforward.inVolts + ) + } override fun setFlywheelBrakeMode(brake: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index b80b67f6..3bd688df 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,7 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) + private var WristFeedforward: SimpleMotorFeedforward() /* private val wristflywheelkP = @@ -109,7 +109,7 @@ fun periodic(){ lastWristPositionTarget = wristPositionTarget } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition(wristProfile.calculate(timeElapsed)) + setWristPosition( WristFeedforward, wristProfile.calculate(timeElapsed)) //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } From 672e61e7bbb14824bc4e70c22c3a0a143f0703ca Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:13:23 -0500 Subject: [PATCH 41/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../team4099/robot2023/config/constants/FlywheelConstants.kt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 8e96e0af..2b1afafe 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,10 +1,14 @@ package com.team4099.robot2023.config.constants +import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.meters 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 FlywheelConstants { val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts From 290e36e1325d04eddd770a4ea3f9f7c4ee2a3ea2 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 16 Jan 2024 15:17:56 -0500 Subject: [PATCH 42/58] Fixed bugs and did more work on PID and feedforward for flywheel --- .../kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt | 1 - 1 file changed, 1 deletion(-) 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 960c48fe..26677ff0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -54,7 +54,6 @@ class Flywheel (val io: FlywheelIO) { var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED init{ io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) - } fun periodic(){ io.updateInputs(inputs) From dc09289d649a353449dd06b6c5a7d8635216633d Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Tue, 9 Jan 2024 19:00:46 -0500 Subject: [PATCH 43/58] Added Feeder IO File --- .../robot2023/subsystems/feeder/FeederIO.kt | 65 ++++++++++++++++++- 1 file changed, 64 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 780fc386..15eec3c3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,3 +1,66 @@ package com.team4099.robot2023.subsystems.feeder -interface FeederIO +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.inCelsius +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +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 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) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { + floorAppliedVoltage = it.volts + } + table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { + floorStatorCurrent = it.amps + } + table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { + floorSupplyCurrent = it.amps + } + table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { + verticalAppliedVoltage = it.volts + } + table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + verticalStatorCurrent = it.amps + } + table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + verticalSupplyCurrent = it.amps + } + table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } + } + + fun updateInputs(inputs: FeederIOInputs) {} + + fun setFloorVoltage(voltage: ElectricalPotential) {} + + fun setVerticalVoltage(voltage: ElectricalPotential) {} + } +} \ No newline at end of file From 8ef2c123c6c45370326125ec7de1d7ef0c73a6b3 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:14:33 -0500 Subject: [PATCH 44/58] create telescoping arm branch --- .../robot2023/subsystems/TelescopingArm/TelescopingArm.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index 53e9e9ed..a1af62e2 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,3 +1,4 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm +class TelescopingArm { +} \ No newline at end of file From 8db051f199f82b33527639a636880c0b6f9d32fb Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:10:42 -0500 Subject: [PATCH 45/58] create ground intake branch --- .../robot2023/subsystems/GroundIntake/GroundIntake.kt | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt new file mode 100644 index 00000000..5fdc7677 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.GroundIntake + +class GroundIntake { +} \ No newline at end of file From 0c2ad521f6a1dd3caea9b16555fcc45b288f0061 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:05:35 -0500 Subject: [PATCH 46/58] create feeder branch --- .../robot2023/subsystems/feeder/FeederIO.kt | 62 ------------------- 1 file changed, 62 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 15eec3c3..41ddbf24 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,66 +1,4 @@ package com.team4099.robot2023.subsystems.feeder -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -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.inCelsius -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -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 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) - } - - override fun fromLog(table: LogTable?) { - table?.getDouble("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { - floorAppliedVoltage = it.volts - } - table?.getDouble("floorStatorCurrent", floorStatorCurrent.inAmperes)?.let { - floorStatorCurrent = it.amps - } - table?.getDouble("floorSupplyCurrent", floorSupplyCurrent.inAmperes)?.let { - floorSupplyCurrent = it.amps - } - table?.getDouble("floorTempCelcius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } - table?.getDouble("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { - verticalAppliedVoltage = it.volts - } - table?.getDouble("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { - verticalStatorCurrent = it.amps - } - table?.getDouble("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { - verticalSupplyCurrent = it.amps - } - table?.getDouble("verticalTempCelcius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } - } - - fun updateInputs(inputs: FeederIOInputs) {} - - fun setFloorVoltage(voltage: ElectricalPotential) {} - - fun setVerticalVoltage(voltage: ElectricalPotential) {} - } } \ No newline at end of file From a0f106793f8d23d3fb4631314639e8f11e814b59 Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Sun, 7 Jan 2024 20:08:24 -0500 Subject: [PATCH 47/58] Create Feeder.kt --- .../kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index f7209b2a..bb51b835 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.feeder import edu.wpi.first.wpilibj2.command.SubsystemBase +class Feeder(val io: FeederIO) : SubsystemBase() { -class Feeder(val io: FeederIO) : SubsystemBase() +} \ No newline at end of file From aa2c301a6f183b3495cf272c615ce567ec32e22a Mon Sep 17 00:00:00 2001 From: Shilab66 <89350258+Shilab66@users.noreply.github.com> Date: Tue, 9 Jan 2024 18:32:02 -0500 Subject: [PATCH 48/58] Revert "create feeder branch" --- .../com/team4099/robot2023/subsystems/feeder/Feeder.kt | 6 ------ .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 4 ---- 2 files changed, 10 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt deleted file mode 100644 index bb51b835..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ /dev/null @@ -1,6 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt deleted file mode 100644 index 41ddbf24..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.feeder - -interface FeederIO { -} \ No newline at end of file From b1c3825ce1a276721f7b88d09b7dd2b8b39a07f5 Mon Sep 17 00:00:00 2001 From: Hanish Rajan <141093134+fyrhanish@users.noreply.github.com> Date: Tue, 16 Jan 2024 20:11:49 -0500 Subject: [PATCH 49/58] Added Feeder class --- .../kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt | 4 ++++ .../com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 4 ++++ 2 files changed, 8 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt new file mode 100644 index 00000000..41603955 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.feeder + +class Feeder { +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt new file mode 100644 index 00000000..41ddbf24 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.feeder + +interface FeederIO { +} \ No newline at end of file From c4a0628879651ba40cff1d36789fc1d5bff5fc2a Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Wed, 17 Jan 2024 18:45:26 -0500 Subject: [PATCH 50/58] Made feeder skeleton --- .../config/constants/FeederConstants.kt | 17 +++++ .../robot2023/subsystems/feeder/Feeder.kt | 72 ++++++++++++++++++- .../robot2023/subsystems/feeder/FeederIO.kt | 59 +++++++++++++++ .../subsystems/superstructure/Request.kt | 21 ++---- 4 files changed, 151 insertions(+), 18 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt new file mode 100644 index 00000000..5c911459 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -0,0 +1,17 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.volts + +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 +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 41603955..82a69f6d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,4 +1,74 @@ package com.team4099.robot2023.subsystems.feeder -class Feeder { +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FeederConstants +import com.team4099.robot2023.subsystems.superstructure.Request +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 + +class Feeder(val io: FeederIO) { + val inputs = FeederIO.FeederIOInputs() + lateinit var flywheelFeedforward: SimpleMotorFeedforward + var currentState = FeederStates.UNINITIALIZED + var flywheelTargetVoltage : ElectricalPotential= 0.0.volts + var feederTargetVoltage : ElectricalPotential = 0.0.volts + + fun setFlywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setFlywheelVoltage(appliedVoltage) + } + + 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() { + io.updateInputs(inputs) + + val nextState = when (currentState) { + FeederStates.UNINITIALIZED -> { + FeederStates.IDLE + } + + FeederStates.IDLE -> { + setFlywheelVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) + fromRequestToState(currentRequest) + } + + FeederStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelTargetVoltage) + fromRequestToState(currentRequest) + } + } + + currentState = nextState + } + + companion object { + enum class FeederStates { + UNINITIALIZED, + IDLE, + OPEN_LOOP + } + + fun fromRequestToState(request: Request.FeederRequest): FeederStates { + return when (request) { + is Request.FeederRequest.Idle -> FeederStates.IDLE + is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP + } + } + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 41ddbf24..9aa8f42a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,4 +1,63 @@ package com.team4099.robot2023.subsystems.feeder interface FeederIO { +<<<<<<< HEAD +======= + 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 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) + } + + override fun fromLog(table: LogTable?) { + table?.get("floorAppliedVoltage", floorAppliedVoltage.inVolts)?.let { + floorAppliedVoltage = 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("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + verticalStatorCurrent = it.amps + } + table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + verticalSupplyCurrent = it.amps + } + 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) {} + + // fun setVerticalVoltage(voltage: ElectricalPotential) {} +>>>>>>> f30fe02 (Made feeder skeleton) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index c02af68b..a13a7847 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,12 +1,11 @@ package com.team4099.robot2023.subsystems.superstructure +import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier -import com.team4099.robot2023.subsystems.Shooter.Flywheel import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity -import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential @@ -33,21 +32,9 @@ sealed interface Request { class ZeroSensors : DrivetrainRequest class Idle : DrivetrainRequest } - sealed interface ShooterRequest : Request { - class OpenLoop(wristVoltage : ElectricalPotential, - //rollerVoltage: ElectricalPotential, - //feederVoltage: ElectricalPotential - ):ShooterRequest{} - class TargetingPosition (val wristPosition : Angle, - //val flywheelVelocity: AngularVelocity, - //val feederVelocity: AngularVelocity - ):ShooterRequest{} - class Zero () : ShooterRequest{} - } - sealed interface FlywheelRequest : Request { - class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} - class TargetingVelocity (flywheelVelocity: AngularVelocity) - class Zero ():FlywheelRequest{} + sealed interface FeederRequest : Request { + class OpenLoop(flywheelVoltage: ElectricalPotential, feederVoltage: ElectricalPotential): FeederRequest {} + class Idle(): FeederRequest {} } } From 1f88265e980dda551bbd6a5eedab9651fcade8bd Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Thu, 18 Jan 2024 20:16:12 -0500 Subject: [PATCH 51/58] Removed flywheel and added PID constants --- .../config/constants/FeederConstants.kt | 27 ++++--- .../robot2023/subsystems/feeder/Feeder.kt | 70 ++++++++++++------- .../robot2023/subsystems/feeder/FeederIO.kt | 53 +++++--------- .../subsystems/superstructure/Request.kt | 2 +- 4 files changed, 85 insertions(+), 67 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 5c911459..362970ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -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, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 82a69f6d..418840d9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -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 - 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) { @@ -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) } } @@ -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 { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 9aa8f42a..2caf0512 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -4,56 +4,41 @@ interface FeederIO { <<<<<<< HEAD ======= 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) {} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index a13a7847..4e14a884 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -34,7 +34,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 {} } } From e67fedc9c20400c3711e5b0d092555721532309b Mon Sep 17 00:00:00 2001 From: 00magikarp <94652654+00magikarp@users.noreply.github.com> Date: Thu, 18 Jan 2024 18:39:07 -0500 Subject: [PATCH 52/58] celsius is spelled wrong :-1: --- .../robot2023/subsystems/feeder/FeederIO.kt | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 2caf0512..cb44de00 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -12,10 +12,21 @@ interface FeederIO { var isSimulated = false override fun toLog(table: LogTable?) { +<<<<<<< HEAD table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) table?.put("feederTempCelcius", feederTemp.inCelsius) +======= + table?.put("floorAppliedVoltage", floorAppliedVoltage.inVolts) + table?.put("floorStatorCurrent", floorStatorCurrent.inAmperes) + table?.put("floorSupplyCurrent", floorSupplyCurrent.inAmperes) + table?.put("floorTempCelsius", floorTemp.inCelsius) + table?.put("verticalAppliedVoltage", verticalAppliedVoltage.inVolts) + table?.put("verticalStatorCurrent", verticalStatorCurrent.inAmperes) + table?.put("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes) + table?.put("verticalTempCelsius", verticalTemp.inCelsius) +>>>>>>> 7184cdc (celsius is spelled wrong :-1:) } override fun fromLog(table: LogTable?) { @@ -30,10 +41,24 @@ interface FeederIO { table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let { feederSupplyCurrent = it.amps } +<<<<<<< HEAD table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { feederTemp = it.celsius } +======= + table?.get("floorTempCelsius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } + table?.get("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { + verticalAppliedVoltage = it.volts + } + table?.get("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { + verticalStatorCurrent = it.amps + } + table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { + verticalSupplyCurrent = it.amps + } + table?.get("verticalTempCelsius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } +>>>>>>> 7184cdc (celsius is spelled wrong :-1:) } } From b0c5157ff8e2003762febe3b48e6004d607b34e2 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:00:45 -0500 Subject: [PATCH 53/58] Completed FeederIO --- .../robot2023/subsystems/feeder/FeederIO.kt | 49 ++++++++----------- 1 file changed, 21 insertions(+), 28 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index cb44de00..146feeb2 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,9 +1,17 @@ package com.team4099.robot2023.subsystems.feeder +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.* +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.inCelsius +import org.team4099.lib.units.derived.* + interface FeederIO { -<<<<<<< HEAD -======= class FeederIOInputs: LoggableInputs { + var feederVelocity = 0.0.rotations.perMinute var feederAppliedVoltage = 0.0.volts var feederStatorCurrent = 0.0.amps var feederSupplyCurrent = 0.0.amps @@ -12,24 +20,18 @@ interface FeederIO { var isSimulated = false override fun toLog(table: LogTable?) { -<<<<<<< HEAD + table?.put("feederVelocity", feederVelocity.inRadiansPerSecond) table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) table?.put("feederTempCelcius", feederTemp.inCelsius) -======= - table?.put("floorAppliedVoltage", floorAppliedVoltage.inVolts) - table?.put("floorStatorCurrent", floorStatorCurrent.inAmperes) - table?.put("floorSupplyCurrent", floorSupplyCurrent.inAmperes) - table?.put("floorTempCelsius", floorTemp.inCelsius) - table?.put("verticalAppliedVoltage", verticalAppliedVoltage.inVolts) - table?.put("verticalStatorCurrent", verticalStatorCurrent.inAmperes) - table?.put("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes) - table?.put("verticalTempCelsius", verticalTemp.inCelsius) ->>>>>>> 7184cdc (celsius is spelled wrong :-1:) } override fun fromLog(table: LogTable?) { + table?.get("feederVelocity", feederVelocity.inRadiansPerSecond)?.let { + feederVelocity = it.radians.perSecond + } + table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let { feederAppliedVoltage = it.volts } @@ -41,24 +43,10 @@ interface FeederIO { table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let { feederSupplyCurrent = it.amps } -<<<<<<< HEAD table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { feederTemp = it.celsius } -======= - table?.get("floorTempCelsius", floorTemp.inCelsius)?.let { floorTemp = it.celsius } - table?.get("verticalAppliedVoltage", verticalAppliedVoltage.inVolts)?.let { - verticalAppliedVoltage = it.volts - } - table?.get("verticalStatorCurrent", verticalStatorCurrent.inAmperes)?.let { - verticalStatorCurrent = it.amps - } - table?.get("verticalSupplyCurrent", verticalSupplyCurrent.inAmperes)?.let { - verticalSupplyCurrent = it.amps - } - table?.get("verticalTempCelsius", verticalTemp.inCelsius)?.let { verticalTemp = it.celsius } ->>>>>>> 7184cdc (celsius is spelled wrong :-1:) } } @@ -66,8 +54,13 @@ interface FeederIO { fun setFeederVoltage(voltage: ElectricalPotential) {} + fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {} + + fun setFeederBrakeMode(brake: Boolean) {} + + fun configPID(kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>) {} + // fun setFloorVoltage(voltage: ElectricalPotential) {} // fun setVerticalVoltage(voltage: ElectricalPotential) {} ->>>>>>> f30fe02 (Made feeder skeleton) } \ No newline at end of file From 0a86d9b49cb4341f2b5c2d279b5129f7862239b5 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:09:51 -0500 Subject: [PATCH 54/58] Completed Feeder and FeederIO --- .../config/constants/FeederConstants.kt | 13 ++- .../robot2023/subsystems/feeder/Feeder.kt | 84 +++++++++++-------- .../subsystems/superstructure/Request.kt | 3 +- 3 files changed, 61 insertions(+), 39 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 362970ed..9bd6e98d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -17,12 +17,19 @@ import org.team4099.lib.units.perSecond object FeederConstants { val FEEDER_INIT_VOLTAGE = 0.0.volts + // TODO: Add value to Feeder target velocity + val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute + // 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, Volt> = 0.001.volts / 1.0.rotations.perMinute - val FEEDER_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val FEEDER_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + val FEEDER_REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_REAL_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + + val FEEDER_SIM_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_SIM_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_SIM_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 418840d9..95604908 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -3,83 +3,96 @@ 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.wpilibj.RobotBase 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.* import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import kotlin.Pair class Feeder(val io: FeederIO): SubsystemBase() { - val inputs = FeederIO.FeederIOInputs() - - var feederTargetVoltage : ElectricalPotential = 0.0.volts - - var lastFeederRunTime = 0.0.seconds - var lastFeederVoltage = 0.0.volts - - var isZeroed = false + val kP = LoggedTunableValue("Feeder/kP", Pair({it.inVoltsPerRotationPerMinute}, {it.volts / 1.0.rotations.perMinute})) + val kI = LoggedTunableValue("Feeder/kI", Pair({it.inVoltsPerRotations}, {it.volts / (1.0.rotations.perMinute * 1.0.seconds)})) + val kD = LoggedTunableValue("Feeder/kD", Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts / 1.0.rotations.perMinute.perSecond})) - 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) + val kS = LoggedTunableValue("Feeder/kS", FeederConstants.FEEDER_KS, Pair({it.inVolts}, {it.volts})) + val kV = LoggedTunableValue("Feeder/kV", FeederConstants.FEEDER_KV, Pair({it.inVoltsPerRotationPerMinute}, {it.volts.perRotation.perMinute})) + val kA = LoggedTunableValue("Feeder/kA", FeederConstants.FEEDER_KA, Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts.perRotation.perMinute.perSecond})) - 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 + val inputs = FeederIO.FeederIOInputs() - var currentState = FeederStates.UNINITIALIZED + var feederFeedForward: SimpleMotorFeedforward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + var feederTargetVoltage: ElectricalPotential = 0.0.volts + var feederTargetVelocity: AngularVelocity = FeederConstants.FEED_NOTE_TARGET_VELOCITY + var lastFeederRunTime = 0.0.seconds + var currentState: FeederStates = 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 + init { + if (RobotBase.isReal()) { + kP.initDefault(FeederConstants.FEEDER_REAL_KP) + kI.initDefault(FeederConstants.FEEDER_REAL_KI) + kD.initDefault(FeederConstants.FEEDER_REAL_KD) + } else { + kP.initDefault(FeederConstants.FEEDER_SIM_KP) + kI.initDefault(FeederConstants.FEEDER_SIM_KI) + kD.initDefault(FeederConstants.FEEDER_SIM_KD) } + } fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) } + fun setFeederVelocity(velocity: AngularVelocity) { + io.setFeederVelocity(velocity, feederFeedForward.calculate(velocity)) + } + override fun periodic() { io.updateInputs(inputs) - val nextState = when (currentState) { + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + + if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged()) { + feederFeedForward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + } + + val nextState: FeederStates = when (currentState) { FeederStates.UNINITIALIZED -> { FeederStates.IDLE } FeederStates.IDLE -> { setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } FeederStates.OPEN_LOOP -> { setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } - } - currentState = nextState + FeederStates.TARGETING_VELOCITY -> { + setFeederVelocity(feederTargetVelocity) + lastFeederRunTime = Clock.fpgaTime + fromRequestToState(currentRequest) + } + } } companion object { enum class FeederStates { UNINITIALIZED, IDLE, - OPEN_LOOP; + OPEN_LOOP, + TARGETING_VELOCITY; fun equivalentToRequest(request: Request.FeederRequest): Boolean { return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE)) @@ -90,6 +103,7 @@ class Feeder(val io: FeederIO): SubsystemBase() { return when (request) { is Request.FeederRequest.Idle -> FeederStates.IDLE is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP + is Request.FeederRequest.TargetingVelocity -> FeederStates.TARGETING_VELOCITY } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 4e14a884..d3682e16 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -34,7 +34,8 @@ sealed interface Request { } sealed interface FeederRequest : Request { - class OpenLoop(val feederVoltage: ElectricalPotential, val flywheelVoltage: ElectricalPotential): FeederRequest {} class Idle(): FeederRequest {} + class OpenLoop(val feederVoltage: ElectricalPotential): FeederRequest {} + class TargetingVelocity(val feederVelocity: AngularVelocity): FeederRequest {} } } From 3be09837df83e0b512dbcf312dd7189ef3f2d6f4 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:33:14 -0500 Subject: [PATCH 55/58] Rebased feeder --- .../kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 146feeb2..88acc163 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -62,5 +62,6 @@ interface FeederIO { // fun setFloorVoltage(voltage: ElectricalPotential) {} + // fun setVerticalVoltage(voltage: ElectricalPotential) {} } \ No newline at end of file From 7128c18c69810e2d9962d42fe144ef6248182eb6 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 20 Jan 2024 19:35:33 -0500 Subject: [PATCH 56/58] Rebased feeder --- .../kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 88acc163..146feeb2 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -62,6 +62,5 @@ interface FeederIO { // fun setFloorVoltage(voltage: ElectricalPotential) {} - // fun setVerticalVoltage(voltage: ElectricalPotential) {} } \ No newline at end of file From d603b9302b4d5a3ac28a865b73aa3219381c3c4f Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Sat, 20 Jan 2024 23:49:13 -0500 Subject: [PATCH 57/58] finish feeder --- .gitignore | 1 + build.gradle | 2 +- simgui-ds.json | 5 + .../team4099/lib/phoenix6/PositionVoltage.kt | 101 +- .../team4099/lib/phoenix6/VelocityVoltage.kt | 98 +- .../com/team4099/robot2023/BuildConstants.kt | 12 +- .../com/team4099/robot2023/RobotContainer.kt | 19 +- .../drivetrain/DriveBrakeModeCommand.kt | 5 +- .../commands/drivetrain/DrivePathCommand.kt | 53 +- .../commands/drivetrain/GoToAngle.kt | 24 +- .../commands/drivetrain/GyroAutoLevel.kt | 26 +- .../drivetrain/OpenLoopReverseCommand.kt | 5 +- .../commands/drivetrain/TeleopDriveCommand.kt | 2 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 2 +- .../team4099/robot2023/config/ControlBoard.kt | 4 + .../robot2023/config/constants/Constants.kt | 6 +- .../config/constants/FeederConstants.kt | 41 +- .../config/constants/FlywheelConstants.kt | 60 - .../config/constants/ShooterConstants.kt | 44 +- .../subsystems/GroundIntake/GroundIntake.kt | 3 +- .../robot2023/subsystems/Shooter/Flywheel.kt | 91 -- .../subsystems/Shooter/FlywheelIO.kt | 71 -- .../subsystems/Shooter/FlywheelIOFalcon.kt | 114 -- .../robot2023/subsystems/Shooter/Shooter.kt | 158 --- .../robot2023/subsystems/Shooter/ShooterIO.kt | 156 --- .../subsystems/Shooter/ShooterIONeo.kt | 134 -- .../TelescopingArm/TelescopingArm.kt | 3 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 1085 +++++++++-------- .../drivetrain/drive/DrivetrainIO.kt | 42 +- .../drivetrain/drive/DrivetrainIOSim.kt | 18 +- .../subsystems/drivetrain/gyro/GyroIO.kt | 90 +- .../subsystems/drivetrain/gyro/GyroIONavx.kt | 126 +- .../drivetrain/servemodule/SwerveModule.kt | 495 ++++---- .../drivetrain/servemodule/SwerveModuleIO.kt | 239 ++-- .../servemodule/SwerveModuleIOSim.kt | 473 +++---- .../robot2023/subsystems/feeder/Feeder.kt | 206 ++-- .../robot2023/subsystems/feeder/FeederIO.kt | 93 +- .../subsystems/feeder/FeederIONeo.kt | 100 ++ .../subsystems/feeder/FeederIOSim.kt | 56 +- .../subsystems/superstructure/Request.kt | 12 +- 40 files changed, 1877 insertions(+), 2398 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt diff --git a/.gitignore b/.gitignore index 8842e0b4..8dcbce64 100644 --- a/.gitignore +++ b/.gitignore @@ -172,3 +172,4 @@ out/ # Simulation GUI and other tools window save file *-window.json + diff --git a/build.gradle b/build.gradle index ba2238c0..17663ff7 100644 --- a/build.gradle +++ b/build.gradle @@ -83,7 +83,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'org.jetbrains.kotlin:kotlin-test-junit5' - implementation 'com.github.team4099:FalconUtils:1.1.28' + implementation 'com.github.team4099:FalconUtils:1.1.29' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 51816cb9..2efb09ec 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 class PositionVoltage( - var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity - var enableFOC: Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot: Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - var limitReverseMotion: Boolean = false, - var velocity: AngularVelocity = 0.0.degrees.perSecond, + private var position: + Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false, + private var velocity: AngularVelocity = 0.0.degrees.perSecond, ) { - val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) - - fun setPosition(new_position: Angle) { - position = new_position - positionVoltagePhoenix6.Position = new_position.inRotations - } - - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - positionVoltagePhoenix6.EnableFOC = new_enableFOC - } - - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } - - fun setSlot(new_slot: Int) { - slot = new_slot - positionVoltagePhoenix6.Slot = new_slot - } - - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } - - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - positionVoltagePhoenix6.LimitForwardMotion = new_limitForward - } - - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + val positionVoltagePhoenix6 = + PositionVoltagePhoenix6( + position.inRotations, + velocity.inRotationsPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) + + fun setPosition(new_position: Angle) { + position = new_position + positionVoltagePhoenix6.Position = new_position.inRotations + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + positionVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + positionVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + positionVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt index 6efd986b..dde287dc 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -1,6 +1,5 @@ package com.team4099.lib.phoenix6 -import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.derived.ElectricalPotential @@ -10,55 +9,68 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerSecond import org.team4099.lib.units.inRotationsPerSecondPerSecond import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 -class VelocityVoltage(var velocity: AngularVelocity, - var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, - var enableFOC:Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot:Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - var limitReverseMotion: Boolean = false){ +class VelocityVoltage( + private var velocity: AngularVelocity, + private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false +) { - val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + val velocityVoltagePhoenix6 = + VelocityVoltagePhoenix6( + velocity.inRotationsPerSecond, + acceleration.inRotationsPerSecondPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) - fun setVelocity(new_velocity: AngularVelocity) { - velocity = new_velocity - velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond - } + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } - fun setAcceleration(new_accel: AngularAcceleration) { - acceleration = new_accel - velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond - } + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - velocityVoltagePhoenix6.EnableFOC = new_enableFOC - } + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } - fun setSlot(new_slot: Int) { - slot = new_slot - velocityVoltagePhoenix6.Slot = new_slot - } + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward - } + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 39f5c23d..60a86e73 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 28 -const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b" -const val GIT_DATE = "2024-01-16T14:49:50Z" -const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-01-16T15:27:50Z" -const val BUILD_UNIX_TIME = 1705436870465L +const val GIT_REVISION = 102 +const val GIT_SHA = "177b056f2e17e0175cc3fd37903303defea08ea0" +const val GIT_DATE = "2024-01-20T20:08:22Z" +const val GIT_BRANCH = "feeder" +const val BUILD_DATE = "2024-01-20T23:40:50Z" +const val BUILD_UNIX_TIME = 1705812050035L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c04891d5..6ebf4b96 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -9,34 +9,31 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.intake.Intake -import com.team4099.robot2023.subsystems.intake.IntakeIONEO -import com.team4099.robot2023.subsystems.intake.IntakeIOSim +import com.team4099.robot2023.subsystems.feeder.Feeder +import com.team4099.robot2023.subsystems.feeder.FeederIONeo +import com.team4099.robot2023.subsystems.feeder.FeederIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase -import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband -import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { private val drivetrain: Drivetrain - private val intake: Intake private val vision: Vision private val limelight: LimelightVision + private val feeder: Feeder init { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {}) - intake = Intake(IntakeIONEO) vision = Vision( // object: CameraIO {} @@ -48,10 +45,10 @@ object RobotContainer { // CameraIONorthstar("backward") ) limelight = LimelightVision(object : LimelightVisionIO {}) + feeder = Feeder(FeederIONeo) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - intake = Intake(IntakeIOSim) vision = Vision( CameraIONorthstar("northstar_1"), @@ -59,6 +56,7 @@ object RobotContainer { CameraIONorthstar("northstar_3"), ) limelight = LimelightVision(object : LimelightVisionIO {}) + feeder = Feeder(FeederIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) @@ -135,6 +133,9 @@ object RobotContainer { // Constants.Universal.Substation.SINGLE_SUBSTATION // ) // ) + + ControlBoard.feederShootTest.whileTrue(feeder.feederOpenLoopIntakeTestCommand()) + ControlBoard.feederIntakeTest.whileTrue(feeder.feederOpenLoopShootTestCommand()) } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 97472978..3eb85b2e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 9c6fea93..fb449709 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,7 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,9 +22,37 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter +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.base.seconds +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.cos +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMeter +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond +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.perMeter +import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.sin +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.inMetersPerSecondPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.inRadiansPerSecondPerSecond +import org.team4099.lib.units.perSecond import java.util.function.Supplier import kotlin.math.PI @@ -216,7 +244,8 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( + drivetrain.currentRequest = + DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -281,16 +310,18 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 30d1a3f2..f48ddc33 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,7 +3,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -11,10 +10,21 @@ import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +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.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +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.radians import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond import kotlin.math.PI +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GoToAngle(val drivetrain: Drivetrain) : Command() { private val thetaPID: ProfiledPIDController> @@ -82,7 +92,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -98,8 +109,9 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index e49f994f..bbfff2f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,16 +4,24 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.units.* +import org.team4099.lib.units.Fraction +import org.team4099.lib.units.Value +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.Second import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +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.radians +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { private val gyroPID: ProfiledPIDController> @@ -92,9 +100,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, gyroFeedback, fieldOriented = true - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop(0.0.radians.perSecond, gyroFeedback, fieldOriented = true) Logger.recordOutput( "AutoLevel/DesiredPitchDegrees", DrivetrainConstants.DOCKING_GYRO_SETPOINT.inDegrees @@ -120,8 +127,9 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index 6f4fa148..257be010 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 18f554e4..6a09a062 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class TeleopDriveCommand( val driver: DriverProfile, diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index d3b6d267..e8510fb3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,9 +1,9 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 2a3446ac..76938ada 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,4 +97,8 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } + + val feederIntakeTest = Trigger { driver.aButton } + val feederShootTest = Trigger { driver.bButton } } + diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 75ef3d16..75c5a2f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -117,7 +117,7 @@ object Constants { object Shooter { const val FLYWHEEL_MOTOR_ID = 51 - //TODO find wrist motor id + // TODO find wrist motor id const val SHOOTER_WRIST_MOTOR_ID = 999 const val FEEDER_MOTOR_ID = 999 } @@ -132,9 +132,7 @@ object Constants { const val REV_ENCODER_PORT = 7 } - object Feeder { - - } + object Feeder object Led { const val LED_CANDLE_ID = 61 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 0113d826..a4d0acdb 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -1,31 +1,34 @@ 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.grams +import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.kilo import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object FeederConstants { - val FEEDER_INIT_VOLTAGE = 0.0.volts - val VOLTAGE_COMPENSATION = 0.0.volts - const val FEEDER_GEAR_RATIO = 0 - const val FEEDER_INERTIA = 0 + val FEEDER_MOTOR_INVERTED = false + val FEEDER_IDLE_VOLTAGE = 0.0.volts + val VOLTAGE_COMPENSATION = 12.0.volts + val FEEDER_CURRENT_LIMIT = 40.0.amps - // TODO: Add value to Feeder target velocity - val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute + val BEAM_BREAK_WAIT_TIME = 0.2.seconds - // 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_GEAR_RATIO = 24.0 / 12.0 + val FEEDER_INERTIA = 0.0017672509.kilo.grams * 1.0.meters.squared - val FEEDER_REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute - val FEEDER_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val FEEDER_REAL_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + var NOTE_VELOCITY_THRESHOLD = 60.degrees.perSecond - val FEEDER_SIM_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute - val FEEDER_SIM_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val FEEDER_SIM_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) -} \ No newline at end of file + var WAIT_BEFORE_DETECT_VELOCITY_DROP = 1.seconds + + + val INTAKE_NOTE_VOLTAGE = 6.volts + val SHOOT_NOTE_VOLTAGE = 11.9.volts + +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt deleted file mode 100644 index 2b1afafe..00000000 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ /dev/null @@ -1,60 +0,0 @@ -package com.team4099.robot2023.config.constants - -import edu.wpi.first.wpilibj.RobotBase -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.Meter -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.meters -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 FlywheelConstants { - val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts - val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts - val ROLLER_GEAR_RATIO = 0.0 - - object PID { - - val AUTO_POS_KP: ProportionalGain> - get() { - if (RobotBase.isReal()) { - return 8.0.meters.perSecond / 1.0.meters - } else { - return 7.0.meters.perSecond / 1.0.meters - } - } - val AUTO_POS_KI: IntegralGain> - get() { - if (RobotBase.isReal()) { - return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) - } else { - return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) - } - } - - val AUTO_POS_KD: DerivativeGain> - get() { - if (RobotBase.isReal()) { - return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond - } else { - return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond - } - - } - } - val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps - val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps - val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds - val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps - - val SHOOTER_FLYWHEEL_KP: ProportionalGain, Volt> = - 0.001.volts / 1.0.rotations.perMinute - val SHOOTER_FLYWHEEL_KI: IntegralGain, Volt> = - 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val SHOOTER_FLYWHEEL_KD: DerivativeGain, Volt> = - 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) - - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt index 5e813dd4..450de710 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ShooterConstants.kt @@ -1,31 +1,27 @@ 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 + // 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 +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt index 5fdc7677..f74b8aa6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.GroundIntake -class GroundIntake { -} \ No newline at end of file +class GroundIntake diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt deleted file mode 100644 index 26677ff0..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ /dev/null @@ -1,91 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.signals.ControlModeValue -import com.team4099.lib.hal.Clock -import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.FlywheelConstants -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.AngularVelocity -import org.team4099.lib.units.base.Meter -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* -import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.perMinute - -class Flywheel (val io: FlywheelIO) { - val motor = TalonFX(Constants.Shooter.FLYWHEEL_MOTOR_ID) - private val kP = - LoggedTunableValue("Flywheel/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP) - private val kI = - LoggedTunableValue( - "Flywheel/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI) - private val kD = - LoggedTunableValue( - "Flywheel/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD) - - - val inputs = FlywheelIO.FlywheelIOInputs() - private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian }) - ) - private val flywheelkV = - LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) - ) - private val flywheelkA = - LoggedTunableValue( - "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perRadianPerSecond }) - ) - val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) - - - var flywheelTargetVoltage: ElectricalPotential = 0.0.volts - fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { - io.setFlywheelVoltage(appliedVoltage) } - - var lastFlywheelRunTime = 0.0.seconds - private var lastFlywheelVoltage = 0.0.volts - private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) - private var hasNote:Boolean = true - val desiredVelocity: AngularVelocity = 1800.rotations.perMinute - var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED -init{ - io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) -} - fun periodic(){ - io.updateInputs(inputs) - var nextState = currentState - when (currentState) { - Flywheel.Companion.FlywheelStates.UNINITIALIZED -> { - nextState = Flywheel.Companion.FlywheelStates.ZERO - } - Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { - setFlywheelVoltage(flywheelTargetVoltage) - lastFlywheelRunTime = Clock.fpgaTime - } - Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ - if (flywheelTargetVoltage != lastFlywheelVoltage){ - val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - io.setFlywheelVelocity()//TODO talk to anshi ab a current velocity var - io.setFlywheelVoltage(controlEffort) - } - } - Flywheel.Companion.FlywheelStates.ZERO ->{ - - } - } - - } - - companion object { - enum class FlywheelStates { - UNINITIALIZED, - ZERO, - OPEN_LOOP, - TARGETING_VELOCITY, - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt deleted file mode 100644 index 78d9a9b7..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ /dev/null @@ -1,71 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.* -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.inCelsius -import org.team4099.lib.units.derived.* - -interface FlywheelIO { - - class FlywheelIOInputs : LoggableInputs { - var rollerVelocity = 0.0.rotations.perMinute - var rollerAppliedVoltage = 0.volts - var rollerStatorCurrent = 0.amps - var rollerSupplyCurrent = 0.amps - var rollerTempreature = 0.celsius - - 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) - } - 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 - } - } - } - - fun setFlywheelVoltage (voltage: ElectricalPotential){ - - } - fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ - - } - fun setFlywheelBrakeMode (brake: Boolean){ - - } - fun updateInputs(inputs:FlywheelIOInputs){ - - } - fun zeroEncoder(){ - - } - fun configPID(kP: ProportionalGain , Volt>, - kI: IntegralGain , Volt>, - kD: DerivativeGain , Volt>){ - - } - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt deleted file mode 100644 index fdd8df4b..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ /dev/null @@ -1,114 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -import com.ctre.phoenix6.StatusSignal -import com.ctre.phoenix6.configs.MotorOutputConfigs -import com.ctre.phoenix6.configs.Slot0Configs -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.hardware.TalonFX -import com.ctre.phoenix6.signals.NeutralModeValue -import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.FlywheelConstants -import com.team4099.robot2023.subsystems.falconspin.Falcon500 -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.AngularVelocity -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.* -import org.team4099.lib.units.ctreAngularMechanismSensor -import org.team4099.lib.units.derived.* - -class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ - - private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val flywheelSensor = - ctreAngularMechanismSensor( - flywheelFalcon, - FlywheelConstants.ROLLER_GEAR_RATIO, - FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, - - ) - var flywheelStatorCurrentSignal: StatusSignal - var flywheelSupplyCurrentSignal: StatusSignal - var flywheelTempSignal: StatusSignal - init { - flywheelFalcon.configurator.apply(TalonFXConfiguration()) - - flywheelFalcon.clearStickyFaults() - flywheelFalcon.configurator.apply(flywheelConfiguration) -//TODO fix PID - flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) - flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) - flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) - flywheelConfiguration.Slot0.kV = 0.05425 - // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) - flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = - FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes - flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold = - FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes - flywheelConfiguration.CurrentLimits.SupplyTimeThreshold = - FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds - flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - flywheelConfiguration.CurrentLimits.StatorCurrentLimit = - FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes - flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - flywheelFalcon.configurator.apply(flywheelConfiguration) - - flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent - flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent - flywheelTempSignal = flywheelFalcon.deviceTemp - - MotorChecker.add( - "Shooter","Flywheel", - MotorCollection( - mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), - FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, - 90.celsius, - FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) - } - override fun configPID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - val PIDConfig = Slot0Configs() - - PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP) - PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI) - PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD) - PIDConfig.kV = 0.05425 - - flywheelFalcon.configurator.apply(PIDConfig) - } - override fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ - flywheelFalcon.setControl(0, - flywheelSensor.velocityToRawUnits(angularVelocity), - DemandType.ArbitraryFeedForward, - feedforward.inVolts - ) - } - - - override fun setFlywheelBrakeMode(brake: Boolean) { - val motorOutputConfig = MotorOutputConfigs() - - if (brake) { - motorOutputConfig.NeutralMode = NeutralModeValue.Brake - } else { - motorOutputConfig.NeutralMode = NeutralModeValue.Coast - } - flywheelFalcon.configurator.apply(motorOutputConfig) - } - override fun zeroEncoder(){ - //TODO finish zero encoder fun (ask sumone what the encoder for falcon is) - flywheelFalcon - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt deleted file mode 100644 index eec37766..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ /dev/null @@ -1,158 +0,0 @@ -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.ShooterIONeo.setWristPosition -import com.team4099.robot2023.subsystems.superstructure.Request -import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.units.Velocity - -import org.team4099.lib.units.base.Meter -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.perSecond - -class Shooter (val io: ShooterIO){ - val inputs = ShooterIO.ShooterIOInputs() - //TODO do feedforward - private val wristkS = - LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) - ) - private val wristlkV = - LoggedTunableValue( - "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) - ) - private val wristkA = - LoggedTunableValue( - "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }) - ) - val flywheelFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) - - - - - private val wristflywheelkP = - LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val wristflywheelkI = - LoggedTunableValue( - "Wrist/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) - ) - private val wristflywheelkD = - LoggedTunableValue( - "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - ) - 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){ - io.setflywheelVoltage(appliedVoltage) - }*/ - 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 - 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 timeProfileGeneratedAt = 0.0.seconds - var currentRequest = Request.ShooterRequest.OpenLoop( - ShooterConstants.WRIST_INIT_VOLTAGE, - //ShooterConstants.ROLLLER_INIT_VOLTAGE, - //ShooterConstants.FEEDER_INIT_VOLTAGE - ) - private var wristProfile = - TrapezoidProfile( - wristConstraints, - TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), - TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) - ) - - fun periodic(){ - io.updateInputs(inputs) - 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 - - /* setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime*/ - if (isZeroed == true ){ - nextState = fromRequestToState(currentRequest) - } - nextState = fromRequestToState(currentRequest) - - } - - 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 - } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO fix this error - Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) - nextState = fromRequestToState(currentRequest) - } - - - - - } - - } - companion object{ - enum class ShooterStates{ - UNINITIALIZED, - ZERO, - OPEN_LOOP, - TARGETING_POSITION, - } - inline fun fromRequestToState(request: Request.ShooterRequest): ShooterStates { - return when (request) { - is Request.ShooterRequest.OpenLoop -> ShooterStates.OPEN_LOOP - is Request.ShooterRequest.TargetingPosition -> ShooterStates.TARGETING_POSITION - is Request.ShooterRequest.Zero -> ShooterStates.ZERO - - } - } - } - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt deleted file mode 100644 index 757a55f6..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ /dev/null @@ -1,156 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -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 { - class ShooterIOInputs : LoggableInputs { - var rollerVelocity = 0.rotations.perMinute - var rollerAppliedVoltage = 0.volts - var rollerStatorCurrent = 0.amps - var rollerSupplyCurrent = 0.amps - var rollerTempreature = 0.celsius - - var wristPostion = 0.degrees - var wristVelocity = 0.radians.perSecond - var wristAppliedVoltage = 0.volts - var wristStatorCurrent = 0.amps - 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 - - 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 { - wristPostion = it.degrees - } - table.get("wristVelocity", wristVelocity.inRadiansPerSecond).let { - wristVelocity = it.radians.perSecond - } - table.get("wristAppliedVoltage", wristAppliedVoltage.inVolts).let { - wristAppliedVoltage = it.volts - } - table.get("wristStatorCurrent", wristStatorCurrent.inAmperes).let { - wristStatorCurrent = it.amps - } - table.get("wristSupplyCurrent", wristSupplyCurrent.inAmperes).let { - wristSupplyCurrent = it.amps - - } - 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 setRollerVoltage (voltage: ElectricalPotential){ - - }*/ - fun setWristVoltage (voltage: ElectricalPotential){ - - } - //fun setFeederVoltage (voltage: ElectricalPotential){ - -// } - fun setWristPosition (position : Angle, feedforward : ElectricalPotential){ - - } - //fun setRollerBrakeMode (brake: Boolean){ - - //} - //fun setFeederBrakeMode (brake: Boolean){ - - // } - fun setWristBrakeMode (brake: Boolean){ - - } - - fun configWristPID( - kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain - ){} - fun zeroEncoder(){ - - } - -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt deleted file mode 100644 index 4fd485dc..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ /dev/null @@ -1,134 +0,0 @@ -package com.team4099.robot2023.subsystems.Shooter - -import com.revrobotics.CANSparkMax -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 org.team4099.lib.units.base.Meter -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 ShooterIONeo : ShooterIO{ - //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, - //ShooterConstants.ROLLER_GEAR_RATIO, - // ShooterConstants.ROLLER_VOLTAGE_COMPENSATION - // ) - -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 - ) - private val wristPIDController : SparkMaxPIDController = wristSparkMax.pidController - //private val feederSparkMax = CANSparkMax(Constants.Shooter.FEEDER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - // private val feederSensor = sparkMaxAngularMechanismSensor( feederSparkMax, - //ShooterConstants.FEEDER_GEAR_RATIO, - //ShooterConstants.FEEDER_VOLTAGE_COMPENSATION - //) - init{ - //reset the motors - //rollerSparkMax.restoreFactoryDefaults() - // rollerSparkMax.clearFaults() - - wristSparkMax.restoreFactoryDefaults() - wristSparkMax.clearFaults() - - //feederSparkMax.restoreFactoryDefaults() - // feederSparkMax.clearFaults() - - //voltage and current limits - // 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()) - - // feederSparkMax.enableVoltageCompensation(ShooterConstants.FEEDER_VOLTAGE_COMPENSATION.inVolts) - // feederSparkMax.setSmartCurrentLimit(ShooterConstants.FEEDER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) - } - - override fun updateInputs (inputs: ShooterIO.ShooterIOInputs){ - /* inputs.rollerVelocity = rollerSensor.velocity - inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput - inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps - // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BatteryVoltage - // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = - // percentOutput * statorCurrent - inputs.rollerSupplyCurrent = inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue - inputs.rollerTempreature = rollerSparkMax.motorTemperature.celsius -*/ - inputs.wristPostion = wristSensor.position - inputs.wristAppliedVoltage = wristSparkMax.busVoltage.volts * wristSparkMax.appliedOutput - inputs.wristStatorCurrent = wristSparkMax.outputCurrent.amps - // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BatteryVoltage - // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = - // percentOutput * statorCurrent - inputs.wristSupplyCurrent = inputs.wristStatorCurrent * wristSparkMax.appliedOutput.absoluteValue - inputs.wristTemperature = wristSparkMax.motorTemperature.celsius - - /* inputs.feederVelocity = feederSensor.velocity - inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput - inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps - // BatteryVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BatteryVoltage - // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = - // percentOutput * statorCurrent - inputs.feederSupplyCurrent = inputs.wristStatorCurrent * feederSparkMax.appliedOutput.absoluteValue - inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ - } - override fun configWristPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) - wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) - wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) - } - - override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { - wristPIDController.setReference( - wristSensor.positionToRawUnits( - clamp( - position, - ShooterConstants.WRIST_SOFTLIMIT_DOWNWARDSTURN, - ShooterConstants.WRIST_SOFTLIMIT_UPWARDSTURN - ) - ), - CANSparkMax.ControlType.kPosition, - 0, - feedforward.inVolts - ) - } - - override fun setWristVoltage(voltage: ElectricalPotential) { - wristSparkMax.setVoltage( - clamp( - voltage, - -ShooterConstants.WRIST_VOLTAGE_COMPENSATION , - ShooterConstants.WRIST_VOLTAGE_COMPENSATION - ) - .inVolts - ) - } - - override fun setWristBrakeMode(brake: Boolean) { - if(brake){ - wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) - }else{ - wristSparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast) - } - } - - override fun zeroEncoder() { - wristSparkMax.encoder.position = 0.0 - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index a1af62e2..53e9e9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm { -} \ No newline at end of file +class TelescopingArm diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 870ddbdc..b94811e9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -5,7 +5,6 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -46,610 +45,616 @@ import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.function.Supplier +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = - Alert( - "Gyro is not connected, field relative driving will be significantly worse.", - Alert.AlertType.ERROR - ) - - val gyroInputs = GyroIO.GyroIOInputs() - val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - - var gyroYawOffset = 0.0.radians - - val closestAlignmentAngle: Angle - get() { - for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { - return angle.degrees - } - } - return 0.0.degrees + val gyroNotConnectedAlert = + Alert( + "Gyro is not connected, field relative driving will be significantly worse.", + Alert.AlertType.ERROR + ) + + val gyroInputs = GyroIO.GyroIOInputs() + val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR + + var gyroYawOffset = 0.0.radians + + val closestAlignmentAngle: Angle + get() { + for (angle in -180..90 step 90) { + if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + return angle.degrees } + } + return 0.0.degrees + } - var canMoveSafely = Supplier { false } - - var elevatorHeightSupplier = Supplier { 0.0.inches } - - init { - - // Wheel speeds - zeroSteering() + var canMoveSafely = Supplier { false } + + var elevatorHeightSupplier = Supplier { 0.0.inches } + + init { + + // Wheel speeds + zeroSteering() + } + + private val frontLeftWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val frontRightWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backLeftWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backRightWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + + val moduleTranslations = + listOf( + frontLeftWheelLocation, + frontRightWheelLocation, + backLeftWheelLocation, + backRightWheelLocation + ) + + val swerveDriveKinematics = + SwerveDriveKinematics( + frontLeftWheelLocation.translation2d, + frontRightWheelLocation.translation2d, + backLeftWheelLocation.translation2d, + backRightWheelLocation.translation2d + ) + + var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + + var swerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + var setPointStates = + mutableListOf( + SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() + ) + + var odometryPose: Pose2d + get() = swerveDrivePoseEstimator.getLatestPose() + set(value) { + swerveDrivePoseEstimator.resetPose(value) + + if (RobotBase.isReal()) {} else { + undriftedPose = odometryPose + swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 + } } - private val frontLeftWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val frontRightWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backLeftWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backRightWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) + var rawGyroAngle = odometryPose.rotation - val moduleTranslations = - listOf( - frontLeftWheelLocation, - frontRightWheelLocation, - backLeftWheelLocation, - backRightWheelLocation - ) + var undriftedPose: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) + set(value) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + value.pose2d + ) + } - val swerveDriveKinematics = - SwerveDriveKinematics( - frontLeftWheelLocation.translation2d, - frontRightWheelLocation.translation2d, - backLeftWheelLocation.translation2d, - backRightWheelLocation.translation2d - ) + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) - var swerveDriveOdometry = - SwerveDriveOdometry( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray() - ) + var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var setPointStates = - mutableListOf( - SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() - ) + var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - set(value) { - swerveDrivePoseEstimator.resetPose(value) + var omegaVelocity = 0.0.radians.perSecond - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + var lastModulePositions = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) - var rawGyroAngle = odometryPose.rotation + var lastGyroYaw = 0.0.radians - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) - set(value) { - swerveDriveOdometry.resetPosition( - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - value.pose2d - ) - } + var velocityTarget = 0.degrees.perSecond - var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + var fieldOrientation = true - var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var omegaVelocity = 0.0.radians.perSecond - - var lastModulePositions = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) - - var lastGyroYaw = 0.0.radians - - var velocityTarget = 0.degrees.perSecond + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + velocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + fieldOrientation = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + else -> {} + } + field = value + } - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + init { + zeroSteering() + } - var fieldOrientation = true + override fun periodic() { + val startTime = Clock.realTimestamp + gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroIO.updateInputs(gyroInputs) - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + swerveModules.forEach { it.periodic() } - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + // TODO: add logic to reduce setpoint based on elevator/arm position + DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond - var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED - var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() - set(value) { - when (value) { - is DrivetrainRequest.OpenLoop -> { - velocityTarget = value.angularVelocity - targetedDriveVector = value.driveVector - fieldOrientation = value.fieldOriented - } - is DrivetrainRequest.ClosedLoop -> { - targetedChassisSpeeds = value.chassisSpeeds - targetedChassisAccels = value.chassisAccels - } - else -> {} - } - field = value - } + Logger.recordOutput( + "Drivetrain/maxSetpointMetersPerSecond", + DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - init { - zeroSteering() + // Update field velocity + val measuredStates = arrayOfNulls(4) + for (i in 0..3) { + measuredStates[i] = + SwerveModuleState( + swerveModules[i].inputs.driveVelocity.inMetersPerSecond, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } + val chassisState: ChassisSpeeds = + ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) + val fieldVelCalculated = + Translation2d( + chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters + ) + .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + + robotVelocity = Pair(chassisState.vx, chassisState.vy) + fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) + + omegaVelocity = chassisState.omega + if (!gyroInputs.gyroConnected) { + gyroInputs.gyroYawRate = omegaVelocity + rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate + gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset } - override fun periodic() { - val startTime = Clock.realTimestamp - gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) - gyroIO.updateInputs(gyroInputs) + // updating odometry every loop cycle + updateOdometry() + + Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) + Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) + + Logger.processInputs("Drivetrain/Gyro", gyroInputs) + Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) + Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) + + Logger.recordOutput( + VisionConstants.POSE_TOPIC_NAME, + doubleArrayOf( + odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians + ) + ) + Logger.recordOutput( + "Odometry/pose3d", + Pose3d( + odometryPose.x, + odometryPose.y, + 1.0.meters, + Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) + ) + .pose3d + ) + Logger.recordOutput( + "Odometry/targetPose", + doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + ) + + Logger.recordOutput( + "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors() + + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + } - swerveModules.forEach { it.periodic() } + currentState = nextState + + // Log the current state + Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + } + + private fun updateOdometry() { + val wheelDeltas = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) + for (i in 0 until 4) { + wheelDeltas[i] = + SwerveModulePosition( + swerveModules[i].inputs.drivePosition.inMeters - + lastModulePositions[i].distanceMeters, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + lastModulePositions[i] = + SwerveModulePosition( + swerveModules[i].inputs.drivePosition.inMeters, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } - // TODO: add logic to reduce setpoint based on elevator/arm position - DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond + var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - Logger.recordOutput( - "Drivetrain/maxSetpointMetersPerSecond", - DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + if (gyroInputs.gyroConnected) { + driveTwist = + edu.wpi.first.math.geometry.Twist2d( + driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians ) + lastGyroYaw = gyroInputs.rawGyroYaw + } - // Update field velocity - val measuredStates = arrayOfNulls(4) - for (i in 0..3) { - measuredStates[i] = - SwerveModuleState( - swerveModules[i].inputs.driveVelocity.inMetersPerSecond, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - } - val chassisState: ChassisSpeeds = - ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) - val fieldVelCalculated = - Translation2d( - chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters - ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig - - robotVelocity = Pair(chassisState.vx, chassisState.vy) - fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) - - omegaVelocity = chassisState.omega - if (!gyroInputs.gyroConnected) { - gyroInputs.gyroYawRate = omegaVelocity - rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate - gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset - } + // reversing the drift to store the ground truth pose + if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { + val undriftedModules = arrayOfNulls(4) + for (i in 0 until 4) { + undriftedModules[i] = + SwerveModulePosition( + ( + swerveModules[i].modulePosition.distanceMeters.meters - + swerveModules[i].inputs.drift + ) + .inMeters, + swerveModules[i].modulePosition.angle + ) + } + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) + + drift = undriftedPose.minus(odometryPose) + + Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) + } - // updating odometry every loop cycle - updateOdometry() - - Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) - Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) - - Logger.processInputs("Drivetrain/Gyro", gyroInputs) - Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) - Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) - - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians)) - Logger.recordOutput( - "Odometry/pose3d", - Pose3d( - odometryPose.x, - odometryPose.y, - 1.0.meters, - Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) - ) - .pose3d + swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + } + + /** + * @param angularVelocity Represents the angular velocity of the chassis + * @param driveVector Pair of linear velocities: First is X vel, second is Y vel + * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) + */ + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + fieldOriented: Boolean = true + ) { + val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 + val allianceFlippedDriveVector = + Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + if (fieldOriented) { + Logger.recordOutput("drivetrain/isFieldOriented", true) + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + odometryPose.rotation ) - Logger.recordOutput( - "Odometry/targetPose", - doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, ) + } - Logger.recordOutput( - "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", - (Clock.realTimestamp - startTime).inMilliseconds + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy + ), + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega ) - var nextState = currentState - - when (currentState) { - DrivetrainState.UNINITIALIZED -> { - // Transitions - nextState = DrivetrainState.ZEROING_SENSORS - } - DrivetrainState.ZEROING_SENSORS -> { - zeroSensors() - - // Transitions - currentRequest = DrivetrainRequest.Idle() - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.OPEN_LOOP -> { - // Outputs - setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.CLOSED_LOOP -> { - // Outputs - setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.IDLE -> { - nextState = fromRequestToState(currentRequest) - } - } + val twistToNextPose: Twist2d = velocityTransform.log() - currentState = nextState - - // Log the current state - Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + desiredChassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) } - private fun updateOdometry() { - val wheelDeltas = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) - for (i in 0 until 4) { - wheelDeltas[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters - - lastModulePositions[i].distanceMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - lastModulePositions[i] = - SwerveModulePosition( - swerveModules[i].inputs.drivePosition.inMeters, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - } + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - // reversing the drift to store the ground truth pose - if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { - val undriftedModules = arrayOfNulls(4) - for (i in 0 until 4) { - undriftedModules[i] = - SwerveModulePosition( - ( - swerveModules[i].modulePosition.distanceMeters.meters - - swerveModules[i].inputs.drift - ) - .inMeters, - swerveModules[i].modulePosition.angle - ) - } - swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) - - drift = undriftedPose.minus(odometryPose) - - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) - } + setPointStates = swerveModuleStates.toMutableList() - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) } - - /** - * @param angularVelocity Represents the angular velocity of the chassis - * @param driveVector Pair of linear velocities: First is X vel, second is Y vel - * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) - */ - fun setOpenLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - fieldOriented: Boolean = true - ) { - val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 - val allianceFlippedDriveVector = - Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) - - val swerveModuleStates: Array - var desiredChassisSpeeds: ChassisSpeeds - - if (fieldOriented) { - Logger.recordOutput("drivetrain/isFieldOriented", true) - desiredChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - odometryPose.rotation - ) - } else { - desiredChassisSpeeds = - ChassisSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - ) - } - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy - ), - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - desiredChassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - } - - swerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + } + + /** + * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular + * and linear acceleration. Calculates both angular and linear velocities and acceleration and + * calls setPositionClosedLoop for each SwerveModule object. + * + * @param angularVelocity The angular velocity of a specified drive + * @param driveVector.first The linear velocity on the X axis + * @param driveVector.second The linear velocity on the Y axis + * @param angularAcceleration The angular acceleration of a specified drive + * @param driveAcceleration.first The linear acceleration on the X axis + * @param driveAcceleration.second The linear acceleration on the Y axis + * @param fieldOriented Defines whether module states are calculated relative to field + */ + fun setClosedLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, + driveAcceleration: Pair = + Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), + fieldOriented: Boolean = true, + ) { + val velSwerveModuleStates: Array? + val accelSwerveModuleStates: Array? + + if (fieldOriented) { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + // This is with respect to the field, meaning all velocity and acceleration vectors are + // adjusted to be relative to the field instead of relative to the robot. + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation + ) + .chassisSpeedsWPILIB ) - setPointStates = swerveModuleStates.toMutableList() - - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) - } - } - - /** - * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular - * and linear acceleration. Calculates both angular and linear velocities and acceleration and - * calls setPositionClosedLoop for each SwerveModule object. - * - * @param angularVelocity The angular velocity of a specified drive - * @param driveVector.first The linear velocity on the X axis - * @param driveVector.second The linear velocity on the Y axis - * @param angularAcceleration The angular acceleration of a specified drive - * @param driveAcceleration.first The linear acceleration on the X axis - * @param driveAcceleration.second The linear acceleration on the Y axis - * @param fieldOriented Defines whether module states are calculated relative to field - */ - fun setClosedLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, - driveAcceleration: Pair = - Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), - fieldOriented: Boolean = true, - ) { - val velSwerveModuleStates: Array? - val accelSwerveModuleStates: Array? - - if (fieldOriented) { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - // This is with respect to the field, meaning all velocity and acceleration vectors are - // adjusted to be relative to the field instead of relative to the robot. - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation - ) - .chassisSpeedsWPILIB - ) - - // Although this isn't perfect, calculating acceleration states using the same math as - // velocity can get us "good enough" accel states to minimize skew - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels.fromFieldRelativeAccels( - driveAcceleration.first, - driveAcceleration.second, - angularAcceleration, - odometryPose.rotation - ) - .chassisAccelsWPILIB - ) - } else { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) - .chassisSpeedsWPILIB - ) - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) - .chassisAccelsWPILIB - ) - } - - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + // Although this isn't perfect, calculating acceleration states using the same math as + // velocity can get us "good enough" accel states to minimize skew + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels.fromFieldRelativeAccels( + driveAcceleration.first, + driveAcceleration.second, + angularAcceleration, + odometryPose.rotation + ) + .chassisAccelsWPILIB + ) + } else { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) + .chassisSpeedsWPILIB + ) + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) + .chassisAccelsWPILIB ) - - setPointStates = velSwerveModuleStates.toMutableList() - - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } } - fun setClosedLoop( - chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, - chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = - edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - ) { - var chassisSpeeds = chassisSpeeds - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vxMetersPerSecond.meters.perSecond, - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vyMetersPerSecond.meters.perSecond - ), - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.omegaRadiansPerSecond.radians.perSecond - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - chassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - .chassisSpeedsWPILIB - } - - val velSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) - val accelSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond - ) - - setPointStates = velSwerveModuleStates.toMutableList() + setPointStates = velSwerveModuleStates.toMutableList() - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } + } + + fun setClosedLoop( + chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, + chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + ) { + var chassisSpeeds = chassisSpeeds + + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vxMetersPerSecond.meters.perSecond, + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vyMetersPerSecond.meters.perSecond + ), + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.omegaRadiansPerSecond.radians.perSecond + ) - fun resetModuleZero() { - swerveModules.forEach { it.resetModuleZero() } - } + val twistToNextPose: Twist2d = velocityTransform.log() - /** Zeros all the sensors on the drivetrain. */ - fun zeroSensors() { - zeroGyroPitch(0.0.degrees) - zeroGyroRoll() - zeroSteering() - zeroDrive() + chassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + .chassisSpeedsWPILIB } - /** - * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. - * - * @param toAngle Zeros the gyro to the value - */ - fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - - if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( - toAngle.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - undriftedPose.pose2d - ) - } + val velSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) - if (!(gyroInputs.gyroConnected)) { - gyroYawOffset = toAngle - rawGyroAngle - } - } + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroPitch(toAngle) - } + setPointStates = velSwerveModuleStates.toMutableList() - fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroRoll(toAngle) + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } - - /** Zeros the steering motors for each swerve module. */ - fun zeroSteering() { - swerveModules.forEach { it.zeroSteering() } + } + + fun resetModuleZero() { + swerveModules.forEach { it.resetModuleZero() } + } + + /** Zeros all the sensors on the drivetrain. */ + fun zeroSensors() { + zeroGyroPitch(0.0.degrees) + zeroGyroRoll() + zeroSteering() + zeroDrive() + } + + /** + * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. + * + * @param toAngle Zeros the gyro to the value + */ + fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + gyroIO.zeroGyroYaw(toAngle) + + swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) + + if (RobotBase.isSimulation()) { + swerveDriveOdometry.resetPosition( + toAngle.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + undriftedPose.pose2d + ) } - /** Zeros the drive motors for each swerve module. */ - private fun zeroDrive() { - swerveModules.forEach { it.zeroDrive() } + if (!(gyroInputs.gyroConnected)) { + gyroYawOffset = toAngle - rawGyroAngle } - - fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + } + + fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroPitch(toAngle) + } + + fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroRoll(toAngle) + } + + /** Zeros the steering motors for each swerve module. */ + fun zeroSteering() { + swerveModules.forEach { it.zeroSteering() } + } + + /** Zeros the drive motors for each swerve module. */ + private fun zeroDrive() { + swerveModules.forEach { it.zeroDrive() } + } + + fun addVisionData(visionData: List) { + swerveDrivePoseEstimator.addVisionData(visionData) + } + + companion object { + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP; + + inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) + ) + } } - companion object { - enum class DrivetrainState { - UNINITIALIZED, - IDLE, - ZEROING_SENSORS, - OPEN_LOOP, - CLOSED_LOOP; - - inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { - return ( - (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || - (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || - (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || - (request is DrivetrainRequest.Idle && this == IDLE) - ) - } - } - - inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { - return when (request) { - is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP - is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP - is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS - is DrivetrainRequest.Idle -> DrivetrainState.IDLE - } - } + inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + } } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt index 8ffd9987..419b53e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt @@ -4,24 +4,24 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO interface DrivetrainIO { - fun getSwerveModules(): List { - return listOf( - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Right Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Right Wheel" - }) - ) - } -} \ No newline at end of file + fun getSwerveModules(): List { + return listOf( + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Right Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Right Wheel" + }) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt index 25e40a71..b77281b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt @@ -4,12 +4,12 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOSim object DrivetrainIOSim : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule(SwerveModuleIOSim("Front Left Wheel")), - SwerveModule(SwerveModuleIOSim("Front Right Wheel")), - SwerveModule(SwerveModuleIOSim("Back Left Wheel")), - SwerveModule(SwerveModuleIOSim("Back Right Wheel")) - ) - } -} \ No newline at end of file + override fun getSwerveModules(): List { + return listOf( + SwerveModule(SwerveModuleIOSim("Front Left Wheel")), + SwerveModule(SwerveModuleIOSim("Front Right Wheel")), + SwerveModule(SwerveModuleIOSim("Back Left Wheel")), + SwerveModule(SwerveModuleIOSim("Back Right Wheel")) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt index 481e3875..89cbd4d9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt @@ -10,52 +10,52 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond interface GyroIO { - class GyroIOInputs : LoggableInputs { - var rawGyroYaw = 0.0.radians - var gyroYaw = 0.0.radians - var gyroPitch = -3.degrees - var gyroRoll = 0.0.radians - var gyroYawRate = 0.0.radians.perSecond - var gyroPitchRate = 0.0.radians.perSecond - var gyroRollRate = 0.0.radians.perSecond - - var odometryYawPositions = arrayOf() - - var gyroConnected = false - - override fun toLog(table: LogTable?) { - table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) - table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) - table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) - table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) - table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) - table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) - table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) - table?.put("gyroConnected", gyroConnected) - } - - override fun fromLog(table: LogTable?) { - table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } - table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } - table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } - table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } - table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { - gyroYawRate = it.degrees.perSecond - } - table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { - gyroPitchRate = it.degrees.perSecond - } - table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { - gyroRollRate = it.degrees.perSecond - } - table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } - } + class GyroIOInputs : LoggableInputs { + var rawGyroYaw = 0.0.radians + var gyroYaw = 0.0.radians + var gyroPitch = -3.degrees + var gyroRoll = 0.0.radians + var gyroYawRate = 0.0.radians.perSecond + var gyroPitchRate = 0.0.radians.perSecond + var gyroRollRate = 0.0.radians.perSecond + + var odometryYawPositions = arrayOf() + + var gyroConnected = false + + override fun toLog(table: LogTable?) { + table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) + table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) + table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) + table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) + table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) + table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) + table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) + table?.put("gyroConnected", gyroConnected) } - fun updateInputs(inputs: GyroIOInputs) {} - fun zeroGyroYaw(toAngle: Angle) {} + override fun fromLog(table: LogTable?) { + table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } + table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } + table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } + table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } + table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { + gyroYawRate = it.degrees.perSecond + } + table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { + gyroPitchRate = it.degrees.perSecond + } + table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { + gyroRollRate = it.degrees.perSecond + } + table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } + } + } + fun updateInputs(inputs: GyroIOInputs) {} + + fun zeroGyroYaw(toAngle: Angle) {} - fun zeroGyroPitch(toAngle: Angle) {} + fun zeroGyroPitch(toAngle: Angle) {} - fun zeroGyroRoll(toAngle: Angle) {} -} \ No newline at end of file + fun zeroGyroRoll(toAngle: Angle) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt index c08c5395..06fd482b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt @@ -11,77 +11,77 @@ import org.team4099.lib.units.perSecond import kotlin.math.IEEErem object GyroIONavx : GyroIO { - private val gyro = AHRS(SerialPort.Port.kMXP) + private val gyro = AHRS(SerialPort.Port.kMXP) - init { - gyro.zeroYaw() - } + init { + gyro.zeroYaw() + } - var gyroYawOffset: Angle = 0.0.degrees - var gyroPitchOffset: Angle = 0.0.degrees - var gyroRollOffset: Angle = 0.0.degrees + var gyroYawOffset: Angle = 0.0.degrees + var gyroPitchOffset: Angle = 0.0.degrees + var gyroRollOffset: Angle = 0.0.degrees - /** The current angle of the drivetrain. */ - val gyroYaw: Angle - get() { - if (gyro.isConnected) { - var rawYaw = gyro.angle + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } + /** The current angle of the drivetrain. */ + val gyroYaw: Angle + get() { + if (gyro.isConnected) { + var rawYaw = gyro.angle + gyroYawOffset.inDegrees + rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate + return rawYaw.IEEErem(360.0).degrees + } else { + return (-1.337).degrees + } + } - val gyroPitch: Angle - get() { - if (gyro.isConnected) { - val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroPitch: Angle + get() { + if (gyro.isConnected) { + val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees + return rawPitch.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroRoll: Angle - get() { - if (gyro.isConnected) { - val rawRoll = gyro.roll + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroRoll: Angle + get() { + if (gyro.isConnected) { + val rawRoll = gyro.roll + gyroRollOffset.inDegrees + return rawRoll.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroYawRate: AngularVelocity - get() { - if (gyro.isConnected) { - return gyro.rate.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } + val gyroYawRate: AngularVelocity + get() { + if (gyro.isConnected) { + return gyro.rate.degrees.perSecond + } else { + return -1.337.degrees.perSecond + } + } - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - inputs.rawGyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyroYaw - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll + override fun updateInputs(inputs: GyroIO.GyroIOInputs) { + inputs.rawGyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyroYaw + inputs.gyroYawRate = gyroYawRate + inputs.gyroPitch = gyroPitch + inputs.gyroRoll = gyroRoll - inputs.gyroConnected = gyro.isConnected - } + inputs.gyroConnected = gyro.isConnected + } - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees - } + override fun zeroGyroYaw(toAngle: Angle) { + gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees + } - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees - } + override fun zeroGyroPitch(toAngle: Angle) { + gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees + } - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees - } -} \ No newline at end of file + override fun zeroGyroRoll(toAngle: Angle) { + gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt index 77060f32..0eb3f017 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -12,265 +12,282 @@ import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.angle +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotation2ds +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.inVoltsPerMeters +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond +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.perMeterPerSecond +import org.team4099.lib.units.derived.perMeterPerSecondPerSecond +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond import kotlin.math.IEEErem import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { - val inputs = SwerveModuleIO.SwerveModuleIOInputs() - - var modulePosition = SwerveModulePosition() - - private var speedSetPoint: LinearVelocity = 0.feet.perSecond - private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond - - private var steeringSetPoint: Angle = 0.degrees - - private var shouldInvert = false - - private val steeringkP = - LoggedTunableValue( - "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) - ) - private val steeringkI = - LoggedTunableValue( - "Drivetrain/moduleSteeringkI", - Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) - ) - private val steeringkD = - LoggedTunableValue( - "Drivetrain/moduleSteeringkD", - Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) - ) - - private val steeringMaxVel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX - ) - private val steeringMaxAccel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX - ) - - private val drivekP = - LoggedTunableValue( - "Drivetrain/moduleDrivekP", - Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) - ) - - private val drivekI = - LoggedTunableValue( - "Drivetrain/moduleDrivekI", - Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) - ) - - private val drivekD = - LoggedTunableValue( - "Drivetrain/moduleDrivekD", - Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) - ) - - init { - if (isReal()) { - steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) - } else { - steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) - } + val inputs = SwerveModuleIO.SwerveModuleIOInputs() + + var modulePosition = SwerveModulePosition() + + private var speedSetPoint: LinearVelocity = 0.feet.perSecond + private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond + + private var steeringSetPoint: Angle = 0.degrees + + private var shouldInvert = false + + private val steeringkP = + LoggedTunableValue( + "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) + ) + private val steeringkI = + LoggedTunableValue( + "Drivetrain/moduleSteeringkI", + Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val steeringkD = + LoggedTunableValue( + "Drivetrain/moduleSteeringkD", + Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + private val steeringMaxVel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX + ) + private val steeringMaxAccel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX + ) + + private val drivekP = + LoggedTunableValue( + "Drivetrain/moduleDrivekP", + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) + ) + + private val drivekI = + LoggedTunableValue( + "Drivetrain/moduleDrivekI", + Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) + ) + + private val drivekD = + LoggedTunableValue( + "Drivetrain/moduleDrivekD", + Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) + ) + + init { + if (isReal()) { + steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) + } else { + steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } + } - fun periodic() { - io.updateInputs(inputs) - - // Updating SwerveModulePosition every loop cycle - modulePosition.distanceMeters = inputs.drivePosition.inMeters - modulePosition.angle = inputs.steeringPosition.inRotation2ds - - if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { - io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) - } - - if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { - io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) - } - - if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { - io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) - } - - Logger.processInputs(io.label, inputs) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveSpeedSetpointMetersPerSecond", - // if (!shouldInvert) speedSetPoint.inMetersPerSecond - // else -speedSetPoint.inMetersPerSecond - // ) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveAccelSetpointMetersPerSecondSq", - // accelerationSetPoint.inMetersPerSecondPerSecond - // ) - // Logger.getInstance() - // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/steeringValueDegreesWithMod", - // inputs.steeringPosition.inDegrees.IEEErem(360.0) - // ) - } + fun periodic() { + io.updateInputs(inputs) - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param steering The angular position desired for the swerve module to be set to - * @param speed The speed desired for the swerve module to reach - * @param acceleration The linear acceleration used to calculate how to reach the desired speed - */ - fun set( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, - optimize: Boolean = true - ) { - if (speed == 0.feet.perSecond) { - io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) - return - } - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - speedSetPoint = - if (shouldInvert) { - speed * -1 - } else { - speed - } - accelerationSetPoint = - if (shouldInvert) { - acceleration * -1 - } else { - acceleration - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - - // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } + // Updating SwerveModulePosition every loop cycle + modulePosition.distanceMeters = inputs.drivePosition.inMeters + modulePosition.angle = inputs.steeringPosition.inRotation2ds - fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - val outputSpeed = - if (shouldInvert) { - speed * -1 - } else { - speed - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - io.setOpenLoop(steeringSetPoint, outputSpeed) + if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { + io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using open loop control. - * - * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - */ - fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { - if (optimize) { - val optimizedState = - SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) - io.setOpenLoop( - optimizedState.angle.angle, - optimizedState - .speedMetersPerSecond - .meters - .perSecond // consider desaturating wheel speeds here if it doesn't work - // from drivetrain - ) - Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) - } else { - io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) - Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) - } + if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { + io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration - * vectors. - * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) - */ - fun setPositionClosedLoop( - desiredVelState: SwerveModuleState, - desiredAccelState: SwerveModuleState, - optimize: Boolean = true - ) { - if (optimize) { - val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) - val optimizedAccelState = - SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) - io.setClosedLoop( - optimizedVelState.angle.angle, - optimizedVelState.speedMetersPerSecond.meters.perSecond, - optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } else { - io.setClosedLoop( - desiredVelState.angle.angle, - desiredVelState.speedMetersPerSecond.meters.perSecond, - desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } + if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { + io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) } - /** Creates event of the current potentiometer value as needs to be manually readjusted. */ - fun resetModuleZero() { - io.resetModuleZero() + Logger.processInputs(io.label, inputs) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveSpeedSetpointMetersPerSecond", + // if (!shouldInvert) speedSetPoint.inMetersPerSecond + // else -speedSetPoint.inMetersPerSecond + // ) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveAccelSetpointMetersPerSecondSq", + // accelerationSetPoint.inMetersPerSecondPerSecond + // ) + // Logger.getInstance() + // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/steeringValueDegreesWithMod", + // inputs.steeringPosition.inDegrees.IEEErem(360.0) + // ) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param steering The angular position desired for the swerve module to be set to + * @param speed The speed desired for the swerve module to reach + * @param acceleration The linear acceleration used to calculate how to reach the desired speed + */ + fun set( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, + optimize: Boolean = true + ) { + if (speed == 0.feet.perSecond) { + io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) + return } + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - /** Zeros the steering motor */ - fun zeroSteering() { - io.zeroSteering() + shouldInvert = (steeringDifference.absoluteValue > (Math.PI / 2).radians) && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - /** Zeros the drive motor */ - fun zeroDrive() { - io.zeroDrive() + speedSetPoint = + if (shouldInvert) { + speed * -1 + } else { + speed + } + accelerationSetPoint = + if (shouldInvert) { + acceleration * -1 + } else { + acceleration + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + + // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + } + + fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians + + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - fun setDriveBrakeMode(brake: Boolean) { - io.setDriveBrakeMode(brake) + val outputSpeed = + if (shouldInvert) { + speed * -1 + } else { + speed + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + io.setOpenLoop(steeringSetPoint, outputSpeed) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using open loop control. + * + * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + */ + fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { + if (optimize) { + val optimizedState = + SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) + io.setOpenLoop( + optimizedState.angle.angle, + optimizedState + .speedMetersPerSecond + .meters + .perSecond // consider desaturating wheel speeds here if it doesn't work + // from drivetrain + ) + Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) + } else { + io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) + Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } - - fun setSteeringBrakeMode(brake: Boolean) { - io.setSteeringBrakeMode(brake) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration + * vectors. + * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) + */ + fun setPositionClosedLoop( + desiredVelState: SwerveModuleState, + desiredAccelState: SwerveModuleState, + optimize: Boolean = true + ) { + if (optimize) { + val optimizedVelState = + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + val optimizedAccelState = + SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) + io.setClosedLoop( + optimizedVelState.angle.angle, + optimizedVelState.speedMetersPerSecond.meters.perSecond, + optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) + } else { + io.setClosedLoop( + desiredVelState.angle.angle, + desiredVelState.speedMetersPerSecond.meters.perSecond, + desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) } -} \ No newline at end of file + } + + /** Creates event of the current potentiometer value as needs to be manually readjusted. */ + fun resetModuleZero() { + io.resetModuleZero() + } + + /** Zeros the steering motor */ + fun zeroSteering() { + io.zeroSteering() + } + + /** Zeros the drive motor */ + fun zeroDrive() { + io.zeroDrive() + } + + fun setDriveBrakeMode(brake: Boolean) { + io.setDriveBrakeMode(brake) + } + + fun setSteeringBrakeMode(brake: Boolean) { + io.setSteeringBrakeMode(brake) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt index f40da16e..bbc82e41 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -2,121 +2,144 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearAcceleration +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter +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.inCelsius +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perSecond interface SwerveModuleIO { - class SwerveModuleIOInputs : LoggableInputs { - var driveAppliedVoltage = 0.0.volts - var steeringAppliedVoltage = 0.0.volts - - var driveStatorCurrent = 0.0.amps - var driveSupplyCurrent = 0.0.amps - var steeringStatorCurrent = 0.0.amps - var steeringSupplyCurrent = 0.0.amps - - var drivePosition = 0.0.meters - var steeringPosition = 0.0.degrees - - var driveVelocity = 0.0.meters.perSecond - var steeringVelocity = 0.0.degrees.perSecond - - var driveTemp = 0.0.celsius - var steeringTemp = 0.0.celsius - - var potentiometerOutputRaw = 0.0 - var potentiometerOutputRadians = 0.0.radians - - var drift = 0.0.meters - - override fun toLog(table: LogTable?) { - table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) - table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) - table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) - table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) - table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) - table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) - table?.put("drivePositionMeters", drivePosition.inMeters) - table?.put("steeringPositionRadians", steeringPosition.inRadians) - table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) - table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - table?.put("driveTempCelcius", driveTemp.inCelsius) - table?.put("steeringTempCelcius", steeringTemp.inCelsius) - table?.put("potentiometerOutputRaw", potentiometerOutputRaw) - table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) - table?.put("driftPositionMeters", drift.inMeters) - } - - override fun fromLog(table: LogTable?) { - table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { - driveAppliedVoltage = it.volts - } - table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { - steeringAppliedVoltage = it.volts - } - table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { - driveStatorCurrent = it.amps - } - table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { - driveSupplyCurrent = it.amps - } - table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { - steeringStatorCurrent = it.amps - } - table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { - steeringSupplyCurrent = it.amps - } - table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { - drivePosition = it.meters - } - table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { - steeringPosition = it.radians - } - table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { - driveVelocity = it.meters.perSecond - } - table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - ?.let { steeringVelocity = it.radians.perSecond } - table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } - table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { - steeringTemp = it.celsius - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { - potentiometerOutputRaw = it - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { - potentiometerOutputRadians = it.radians - } - table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } - } + class SwerveModuleIOInputs : LoggableInputs { + var driveAppliedVoltage = 0.0.volts + var steeringAppliedVoltage = 0.0.volts + + var driveStatorCurrent = 0.0.amps + var driveSupplyCurrent = 0.0.amps + var steeringStatorCurrent = 0.0.amps + var steeringSupplyCurrent = 0.0.amps + + var drivePosition = 0.0.meters + var steeringPosition = 0.0.degrees + + var driveVelocity = 0.0.meters.perSecond + var steeringVelocity = 0.0.degrees.perSecond + + var driveTemp = 0.0.celsius + var steeringTemp = 0.0.celsius + + var potentiometerOutputRaw = 0.0 + var potentiometerOutputRadians = 0.0.radians + + var drift = 0.0.meters + + override fun toLog(table: LogTable?) { + table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) + table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) + table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) + table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) + table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) + table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) + table?.put("drivePositionMeters", drivePosition.inMeters) + table?.put("steeringPositionRadians", steeringPosition.inRadians) + table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) + table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + table?.put("driveTempCelcius", driveTemp.inCelsius) + table?.put("steeringTempCelcius", steeringTemp.inCelsius) + table?.put("potentiometerOutputRaw", potentiometerOutputRaw) + table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) + table?.put("driftPositionMeters", drift.inMeters) } - val label: String + override fun fromLog(table: LogTable?) { + table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { + driveAppliedVoltage = it.volts + } + table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { + steeringAppliedVoltage = it.volts + } + table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { + driveStatorCurrent = it.amps + } + table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { + driveSupplyCurrent = it.amps + } + table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { + steeringStatorCurrent = it.amps + } + table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { + steeringSupplyCurrent = it.amps + } + table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { + drivePosition = it.meters + } + table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { + steeringPosition = it.radians + } + table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { + driveVelocity = it.meters.perSecond + } + table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + ?.let { steeringVelocity = it.radians.perSecond } + table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } + table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { + steeringTemp = it.celsius + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { + potentiometerOutputRaw = it + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { + potentiometerOutputRadians = it.radians + } + table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } + } + } + + val label: String - fun updateInputs(inputs: SwerveModuleIOInputs) {} + fun updateInputs(inputs: SwerveModuleIOInputs) {} - fun setSteeringSetpoint(angle: Angle) {} - fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} - fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} + fun setSteeringSetpoint(angle: Angle) {} + fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} + fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} - fun resetModuleZero() {} - fun zeroSteering() {} - fun zeroDrive() {} + fun resetModuleZero() {} + fun zeroSteering() {} + fun zeroDrive() {} - fun setDriveBrakeMode(brake: Boolean) {} + fun setDriveBrakeMode(brake: Boolean) {} - fun setSteeringBrakeMode(brake: Boolean) {} + fun setSteeringBrakeMode(brake: Boolean) {} - fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) {} - fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} - fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} -} \ No newline at end of file + fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) {} + fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt index 49074abd..5a219453 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt @@ -13,235 +13,262 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.LinearAcceleration +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.Meter +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.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotations +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.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond import kotlin.random.Random class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { - // Use inverses of gear ratios because our standard is <1 is reduction - val driveMotorSim: FlywheelSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - val steerMotorSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - init { - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - } - - var turnRelativePosition = 0.0.radians - var turnAbsolutePosition = - (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to - var driveVelocity = 0.0.meters.perSecond - var drift: Length = 0.0.meters - - private val driveFeedback = - PIDController( - DrivetrainConstants.PID.SIM_DRIVE_KP, - DrivetrainConstants.PID.SIM_DRIVE_KI, - DrivetrainConstants.PID.SIM_DRIVE_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - private val driveFeedForward = - SimpleMotorFeedforward( - DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV - ) - - private val steeringFeedback = - PIDController( - DrivetrainConstants.PID.SIM_STEERING_KP, - DrivetrainConstants.PID.SIM_STEERING_KI, - DrivetrainConstants.PID.SIM_STEERING_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - - init { - steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) - steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + // Use inverses of gear ratios because our standard is <1 is reduction + val driveMotorSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, + DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + val steerMotorSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, + DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + init { + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + } + + var turnRelativePosition = 0.0.radians + var turnAbsolutePosition = + (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to + var driveVelocity = 0.0.meters.perSecond + var drift: Length = 0.0.meters + + private val driveFeedback = + PIDController( + DrivetrainConstants.PID.SIM_DRIVE_KP, + DrivetrainConstants.PID.SIM_DRIVE_KI, + DrivetrainConstants.PID.SIM_DRIVE_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + private val driveFeedForward = + SimpleMotorFeedforward( + DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV + ) + + private val steeringFeedback = + PIDController( + DrivetrainConstants.PID.SIM_STEERING_KP, + DrivetrainConstants.PID.SIM_STEERING_KI, + DrivetrainConstants.PID.SIM_STEERING_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + + init { + steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) + steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + } + + override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { + driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + val angleDifference: Angle = + (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + .radians + turnAbsolutePosition += angleDifference + turnRelativePosition += angleDifference + + // constrains it to 2pi radians + while (turnAbsolutePosition < 0.radians) { + turnAbsolutePosition += (2.0 * Math.PI).radians } - - override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - val angleDifference: Angle = - (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - .radians - turnAbsolutePosition += angleDifference - turnRelativePosition += angleDifference - - // constrains it to 2pi radians - while (turnAbsolutePosition < 0.radians) { - turnAbsolutePosition += (2.0 * Math.PI).radians - } - while (turnAbsolutePosition > (2.0 * Math.PI).radians) { - turnAbsolutePosition -= (2.0 * Math.PI).radians - } - - // s = r * theta -> d/2 * rad/s = m/s - driveVelocity = - (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - - // simming drift - var loopCycleDrift = 0.0.meters - if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { - loopCycleDrift = - (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) - .meters // 0.0005 is just a nice number that ended up working out for drift - } - drift += loopCycleDrift - - // pi * d * rotations = distance travelled - inputs.drivePosition = - inputs.drivePosition + - DrivetrainConstants.WHEEL_DIAMETER * - Math.PI * - ( - driveMotorSim.angularVelocityRadPerSec * - Constants.Universal.LOOP_PERIOD_TIME.inSeconds - ) - .radians - .inRotations + - loopCycleDrift // adding a random amount of drift - inputs.steeringPosition = turnAbsolutePosition - inputs.drift = drift - - inputs.driveVelocity = driveVelocity - inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond - - inputs.driveAppliedVoltage = (-1337).volts - inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps - inputs.driveStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.driveTemp = (-1337).celsius - inputs.steeringTemp = (-1337).celsius - - inputs.steeringAppliedVoltage = (-1337).volts - inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps - inputs.steeringStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.potentiometerOutputRadians = turnAbsolutePosition - inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians - - // Setting a more accurate simulated voltage under load - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage( - inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes - ) - ) + while (turnAbsolutePosition > (2.0 * Math.PI).radians) { + turnAbsolutePosition -= (2.0 * Math.PI).radians } - // helper functions to clamp all inputs and set sim motor voltages properly - private fun setDriveVoltage(volts: ElectricalPotential) { - val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) - } + // s = r * theta -> d/2 * rad/s = m/s + driveVelocity = + (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - private fun setSteeringVoltage(volts: ElectricalPotential) { - val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + // simming drift + var loopCycleDrift = 0.0.meters + if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { + loopCycleDrift = + (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) + .meters // 0.0005 is just a nice number that ended up working out for drift } - - override fun setSteeringSetpoint(angle: Angle) { - val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) - Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) - Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) - Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) - Logger.recordOutput( - "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + drift += loopCycleDrift + + // pi * d * rotations = distance travelled + inputs.drivePosition = + inputs.drivePosition + + DrivetrainConstants.WHEEL_DIAMETER * + Math.PI * + ( + driveMotorSim.angularVelocityRadPerSec * + Constants.Universal.LOOP_PERIOD_TIME.inSeconds ) - setSteeringVoltage(feedback) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) - val feedforward = driveFeedForward.calculate(speed, acceleration) - setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) - - setSteeringSetpoint(steering) - } - - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - setDriveVoltage( - RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Resetting your module's 0 doesn't do anything meaningful in sim :(") - } - - override fun zeroDrive() { - println("Zero drive do anything meaningful in sim") - } - - override fun zeroSteering() { - turnAbsolutePosition = 0.0.radians - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - driveFeedback.setPID(kP, kI, kD) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - steeringFeedback.setPID(kP, kI, kD) - } - - override fun setDriveBrakeMode(brake: Boolean) { - println("Can't set brake mode in simulation") - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - println("Can't configure motion magic in simulation") - } -} \ No newline at end of file + .radians + .inRotations + + loopCycleDrift // adding a random amount of drift + inputs.steeringPosition = turnAbsolutePosition + inputs.drift = drift + + inputs.driveVelocity = driveVelocity + inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond + + inputs.driveAppliedVoltage = (-1337).volts + inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps + inputs.driveStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.driveTemp = (-1337).celsius + inputs.steeringTemp = (-1337).celsius + + inputs.steeringAppliedVoltage = (-1337).volts + inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps + inputs.steeringStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.potentiometerOutputRadians = turnAbsolutePosition + inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians + + // Setting a more accurate simulated voltage under load + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage( + inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes + ) + ) + } + + // helper functions to clamp all inputs and set sim motor voltages properly + private fun setDriveVoltage(volts: ElectricalPotential) { + val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) + } + + private fun setSteeringVoltage(volts: ElectricalPotential) { + val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + } + + override fun setSteeringSetpoint(angle: Angle) { + val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) + Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) + Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) + Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) + Logger.recordOutput( + "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + ) + setSteeringVoltage(feedback) + } + + override fun setClosedLoop( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration + ) { + Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) + val feedforward = driveFeedForward.calculate(speed, acceleration) + setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) + + setSteeringSetpoint(steering) + } + + override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { + setDriveVoltage( + RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) + ) + setSteeringSetpoint(steering) + } + + override fun resetModuleZero() { + println("Resetting your module's 0 doesn't do anything meaningful in sim :(") + } + + override fun zeroDrive() { + println("Zero drive do anything meaningful in sim") + } + + override fun zeroSteering() { + turnAbsolutePosition = 0.0.radians + } + + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + driveFeedback.setPID(kP, kI, kD) + } + + override fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + steeringFeedback.setPID(kP, kI, kD) + } + + override fun setDriveBrakeMode(brake: Boolean) { + println("Can't set brake mode in simulation") + } + + override fun configureSteeringMotionMagic( + maxVel: AngularVelocity, + maxAccel: AngularAcceleration + ) { + println("Can't configure motion magic in simulation") + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 95604908..026f7be6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,110 +1,136 @@ 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.subsystems.superstructure.Request -import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.* +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* -import kotlin.Pair - -class Feeder(val io: FeederIO): SubsystemBase() { - val kP = LoggedTunableValue("Feeder/kP", Pair({it.inVoltsPerRotationPerMinute}, {it.volts / 1.0.rotations.perMinute})) - val kI = LoggedTunableValue("Feeder/kI", Pair({it.inVoltsPerRotations}, {it.volts / (1.0.rotations.perMinute * 1.0.seconds)})) - val kD = LoggedTunableValue("Feeder/kD", Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts / 1.0.rotations.perMinute.perSecond})) - - val kS = LoggedTunableValue("Feeder/kS", FeederConstants.FEEDER_KS, Pair({it.inVolts}, {it.volts})) - val kV = LoggedTunableValue("Feeder/kV", FeederConstants.FEEDER_KV, Pair({it.inVoltsPerRotationPerMinute}, {it.volts.perRotation.perMinute})) - val kA = LoggedTunableValue("Feeder/kA", FeederConstants.FEEDER_KA, Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts.perRotation.perMinute.perSecond})) - - val inputs = FeederIO.FeederIOInputs() - - var feederFeedForward: SimpleMotorFeedforward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) - var feederTargetVoltage: ElectricalPotential = 0.0.volts - var feederTargetVelocity: AngularVelocity = FeederConstants.FEED_NOTE_TARGET_VELOCITY - var lastFeederRunTime = 0.0.seconds - - var currentState: FeederStates = FeederStates.UNINITIALIZED - var currentRequest: Request.FeederRequest = Request.FeederRequest.Idle() - - init { - if (RobotBase.isReal()) { - kP.initDefault(FeederConstants.FEEDER_REAL_KP) - kI.initDefault(FeederConstants.FEEDER_REAL_KI) - kD.initDefault(FeederConstants.FEEDER_REAL_KD) - } else { - kP.initDefault(FeederConstants.FEEDER_SIM_KP) - kI.initDefault(FeederConstants.FEEDER_SIM_KI) - kD.initDefault(FeederConstants.FEEDER_SIM_KD) +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.volts + +class Feeder(val io: FeederIO) : SubsystemBase() { + val inputs = FeederIO.FeederIOInputs() + + var feederTargetVoltage: ElectricalPotential = 0.0.volts + var lastFeederRunTime = 0.0.seconds + + var currentState: FeederStates = FeederStates.UNINITIALIZED + var currentRequest: Request.FeederRequest = + Request.FeederRequest.OpenLoopIntake(FeederConstants.FEEDER_IDLE_VOLTAGE) + set(value) { + when (value) { + is Request.FeederRequest.OpenLoopIntake -> { + feederTargetVoltage = value.feederVoltage + } + is Request.FeederRequest.OpenLoopShoot -> { + feederTargetVoltage = value.feederVoltage + } + else -> {} } + field = value + } + + val noteDetected: Boolean + get() { + return inputs.feederVelocity.absoluteValue <= FeederConstants.NOTE_VELOCITY_THRESHOLD && + inputs.feederStatorCurrent > 10.amps && + inputs.feederAppliedVoltage.sign < 0 && + (Clock.fpgaTime - lastFeederRunTime) >= + FeederConstants.WAIT_BEFORE_DETECT_VELOCITY_DROP || inputs.isSimulated } - fun setFeederVoltage(appliedVoltage: ElectricalPotential){ - io.setFeederVoltage(appliedVoltage) - } + var firstTripBeamBreakTime = Clock.fpgaTime - fun setFeederVelocity(velocity: AngularVelocity) { - io.setFeederVelocity(velocity, feederFeedForward.calculate(velocity)) + var lastBeamState = false + val hasNote: Boolean + get() { + return inputs.beamBroken && + Clock.fpgaTime - firstTripBeamBreakTime > FeederConstants.BEAM_BREAK_WAIT_TIME } - override fun periodic() { - io.updateInputs(inputs) + override fun periodic() { + io.updateInputs(inputs) - if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { - io.configPID(kP.get(), kI.get(), kD.get()) - } + Logger.processInputs("Feeder", inputs) - if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged()) { - feederFeedForward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) - } - - val nextState: FeederStates = when (currentState) { - FeederStates.UNINITIALIZED -> { - FeederStates.IDLE - } - - FeederStates.IDLE -> { - setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) - lastFeederRunTime = Clock.fpgaTime - fromRequestToState(currentRequest) - } - - FeederStates.OPEN_LOOP -> { - setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime - fromRequestToState(currentRequest) - } - - FeederStates.TARGETING_VELOCITY -> { - setFeederVelocity(feederTargetVelocity) - lastFeederRunTime = Clock.fpgaTime - fromRequestToState(currentRequest) - } - } + if (!lastBeamState && inputs.beamBroken) { + firstTripBeamBreakTime = Clock.fpgaTime } - companion object { - enum class FeederStates { - UNINITIALIZED, - IDLE, - OPEN_LOOP, - TARGETING_VELOCITY; - - fun equivalentToRequest(request: Request.FeederRequest): Boolean { - return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE)) - } + lastBeamState = inputs.beamBroken + + var nextState = currentState + when (currentState) { + FeederStates.UNINITIALIZED -> { + nextState = FeederStates.IDLE + } + FeederStates.IDLE -> { + setFeederVoltage(FeederConstants.FEEDER_IDLE_VOLTAGE) + nextState = fromRequestToState(currentRequest) + } + FeederStates.OPEN_LOOP_INTAKE -> { + + if (!hasNote) { + setFeederVoltage(feederTargetVoltage) + nextState = fromRequestToState(currentRequest) + } else { + nextState = FeederStates.IDLE } - - fun fromRequestToState(request: Request.FeederRequest): FeederStates { - return when (request) { - is Request.FeederRequest.Idle -> FeederStates.IDLE - is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP - is Request.FeederRequest.TargetingVelocity -> FeederStates.TARGETING_VELOCITY - } + } + FeederStates.OPEN_LOOP_SHOOT -> { + if (hasNote) { + setFeederVoltage(feederTargetVoltage) + nextState = fromRequestToState(currentRequest) + } else { + nextState = FeederStates.IDLE } + } + } + currentState = nextState + } + + fun setFeederVoltage(appliedVoltage: ElectricalPotential) { + io.setFeederVoltage(appliedVoltage) + } + + fun feederOpenLoopIntakeTestCommand(): Command { + return runOnce({ + currentRequest = Request.FeederRequest.OpenLoopIntake(FeederConstants.INTAKE_NOTE_VOLTAGE) + }) + } + + fun feederOpenLoopShootTestCommand(): Command { + return runOnce({ + currentRequest = Request.FeederRequest.OpenLoopShoot(FeederConstants.SHOOT_NOTE_VOLTAGE) + }) + } + + + companion object { + enum class FeederStates { + UNINITIALIZED, + IDLE, + OPEN_LOOP_INTAKE, + OPEN_LOOP_SHOOT; + + fun equivalentToRequest(request: Request.FeederRequest): Boolean { + return ( + (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_SHOOT) || + (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_INTAKE) || + (request is Request.FeederRequest.Idle && this == IDLE) + ) + } + } + + fun fromRequestToState(request: Request.FeederRequest): FeederStates { + return when (request) { + is Request.FeederRequest.Idle -> FeederStates.IDLE + is Request.FeederRequest.OpenLoopIntake -> FeederStates.OPEN_LOOP_INTAKE + is Request.FeederRequest.OpenLoopShoot -> FeederStates.OPEN_LOOP_SHOOT + } } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 8de21e7c..9464277b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,67 +1,64 @@ package com.team4099.robot2023.subsystems.feeder -import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.* 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.inCelsius -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond interface FeederIO { - class FeederIOInputs: LoggableInputs { - var feederVelocity = 0.0.rotations.perMinute - 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("feederVelocity", feederVelocity.inRadiansPerSecond) - table?.put("feederAppliedVoltage", feederAppliedVoltage.inVolts) - table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) - table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) - table?.put("feederTempCelcius", feederTemp.inCelsius) - } + class FeederIOInputs : LoggableInputs { + var feederVelocity = 0.0.rotations.perMinute + var feederAppliedVoltage = 0.0.volts + var feederStatorCurrent = 0.0.amps + var feederSupplyCurrent = 0.0.amps + var feederTemp = 0.0.celsius + + var beamBroken = false + + var isSimulated = false + + override fun toLog(table: LogTable?) { + table?.put("feederVelocity", feederVelocity.inRadiansPerSecond) + 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("feederVelocity", feederVelocity.inRadiansPerSecond)?.let { - feederVelocity = it.radians.perSecond - } + override fun fromLog(table: LogTable?) { + table?.get("feederVelocity", feederVelocity.inRadiansPerSecond)?.let { + feederVelocity = it.radians.perSecond + } - table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let { - feederAppliedVoltage = it.volts - } + table?.get("feederAppliedVoltage", feederAppliedVoltage.inVolts)?.let { + feederAppliedVoltage = it.volts + } - table?.get("feederStatorCurrent", feederStatorCurrent.inAmperes)?.let { - feederStatorCurrent = it.amps - } + table?.get("feederStatorCurrent", feederStatorCurrent.inAmperes)?.let { + feederStatorCurrent = it.amps + } - table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let { - feederSupplyCurrent = it.amps - } + table?.get("feederSupplyCurrent", feederSupplyCurrent.inAmperes)?.let { + feederSupplyCurrent = it.amps + } - table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { - feederTemp = it.celsius - } - } + table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { feederTemp = it.celsius } } + } - fun updateInputs(inputs: FeederIOInputs) {} - - fun setFeederVoltage(voltage: ElectricalPotential) {} - - fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {} - - fun setFeederBrakeMode(brake: Boolean) {} - - fun configPID(kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>) {} + fun updateInputs(inputs: FeederIOInputs) {} - // fun setFloorVoltage(voltage: ElectricalPotential) {} + fun setFeederVoltage(voltage: ElectricalPotential) {} - // fun setVerticalVoltage(voltage: ElectricalPotential) {} -} \ No newline at end of file + fun setFeederBrakeMode(brake: Boolean) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt new file mode 100644 index 00000000..380d625b --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt @@ -0,0 +1,100 @@ +package com.team4099.robot2023.subsystems.feeder + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FeederConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import org.littletonrobotics.junction.Logger +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.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.absoluteValue + +object FeederIONeo : FeederIO { + private val feederSparkMax = + CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val feederSensor = + sparkMaxAngularMechanismSensor( + feederSparkMax, FeederConstants.FEEDER_GEAR_RATIO, FeederConstants.VOLTAGE_COMPENSATION + ) + + init { + feederSparkMax.restoreFactoryDefaults() + feederSparkMax.clearFaults() + + feederSparkMax.enableVoltageCompensation(FeederConstants.VOLTAGE_COMPENSATION.inVolts) + feederSparkMax.setSmartCurrentLimit(FeederConstants.FEEDER_CURRENT_LIMIT.inAmperes.toInt()) + feederSparkMax.inverted = FeederConstants.FEEDER_MOTOR_INVERTED + + feederSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + + feederSparkMax.openLoopRampRate = 0.0 + feederSparkMax.burnFlash() + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf(Neo(feederSparkMax, "Roller Motor")), + FeederConstants.FEEDER_CURRENT_LIMIT, + 70.celsius, + FeederConstants.FEEDER_CURRENT_LIMIT - 0.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: FeederIO.FeederIOInputs) { + inputs.feederVelocity = feederSensor.velocity + inputs.feederAppliedVoltage = feederSparkMax.busVoltage.volts * feederSparkMax.appliedOutput + inputs.feederStatorCurrent = feederSparkMax.outputCurrent.amps + + // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BusVoltage + // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.feederSupplyCurrent = + inputs.feederStatorCurrent * feederSparkMax.appliedOutput.absoluteValue + inputs.feederTemp = feederSparkMax.motorTemperature.celsius + + Logger.recordOutput( + "Intake/rollerMotorOvercurrentFault", + feederSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) + ) + Logger.recordOutput("Intake/busVoltage", feederSparkMax.busVoltage) + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setFeederVoltage(voltage: ElectricalPotential) { + feederSparkMax.setVoltage( + clamp(voltage, -FeederConstants.VOLTAGE_COMPENSATION, FeederConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } + + /** + * Sets the roller motor brake mode + * + * @param brake if it brakes + */ + override fun setFeederBrakeMode(brake: Boolean) { + if (brake) { + feederSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + feederSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt index 1fa3064e..b06a4d70 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt @@ -1,53 +1,45 @@ package com.team4099.robot2023.subsystems.feeder -import edu.wpi.first.wpilibj.simulation.FlywheelSim import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FeederConstants import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.FlywheelSim import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inKilogramsMeterSquared import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perMinute -class FeederIOSim : FeederIO { - private val feederSim: FlywheelSim = FlywheelSim( - DCMotor.getNEO(1), - FeederConstants.FEEDER_GEAR_RATIO, - FeederConstants.FEEDER_INERTIA +object FeederIOSim : FeederIO { + private val feederSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + FeederConstants.FEEDER_GEAR_RATIO, + FeederConstants.FEEDER_INERTIA.inKilogramsMeterSquared ) - override fun updateInputs(inputs: FeederIO.FeederIOInputs) { - feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - - inputs.feederVelocity = feederSim.getAngularVelocityRPM().rotations.perMinute - inputs.feederAppliedVoltage = appliedVoltage - inputs.feederSupplyCurrent = 0.amps - inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps - inputs.feederTemp = 0.0.celsius - } + var appliedVoltage = 0.volts - override fun setRollerVoltage(voltage: ElectricalPotential) { - appliedVoltage = voltage - feederSim.setInputVoltage( - clamp( - voltage, - -FeederConstants.VOLTAGE_COMPENSATION, - FeederConstants.VOLTAGE_COMPENSATION - ) - .inVolts - ) - } + override fun updateInputs(inputs: FeederIO.FeederIOInputs) { + feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - override fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { + inputs.feederVelocity = feederSim.getAngularVelocityRPM().rotations.perMinute + inputs.feederAppliedVoltage = appliedVoltage + inputs.feederSupplyCurrent = 0.amps + inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps + inputs.feederTemp = 0.0.celsius + } - } - - override fun configPID(kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>) { - - } + override fun setFeederVoltage(voltage: ElectricalPotential) { + appliedVoltage = voltage + feederSim.setInputVoltage( + clamp(voltage, -FeederConstants.VOLTAGE_COMPENSATION, FeederConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index d3682e16..fc1afc4e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,15 +1,9 @@ package com.team4099.robot2023.subsystems.superstructure -import com.team4099.robot2023.config.constants.FeederConstants -import com.team4099.robot2023.config.constants.GamePiece -import com.team4099.robot2023.config.constants.NodeTier import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity -import org.team4099.lib.units.base.Length -import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.perSecond // typealias GroundIntakeRequest = SuperStructureState.GroundIntakeStructure.GroundIntakeRequest // typealias GroundIntakeState = SuperStructureState.GroundIntakeStructure.GroundIntakeState @@ -34,8 +28,8 @@ sealed interface Request { } sealed interface FeederRequest : Request { - class Idle(): FeederRequest {} - class OpenLoop(val feederVoltage: ElectricalPotential): FeederRequest {} - class TargetingVelocity(val feederVelocity: AngularVelocity): FeederRequest {} + class Idle() : FeederRequest + class OpenLoopIntake(val feederVoltage: ElectricalPotential) : FeederRequest + class OpenLoopShoot(val feederVoltage: ElectricalPotential) : FeederRequest } } From ffd79239856844cf4b852153da0e5774e24647a9 Mon Sep 17 00:00:00 2001 From: 00magikarp <94652654+00magikarp@users.noreply.github.com> Date: Tue, 23 Jan 2024 18:50:56 -0500 Subject: [PATCH 58/58] completed saraansh's peer review --- .../robot2023/subsystems/feeder/Feeder.kt | 22 +++++-------------- .../robot2023/subsystems/feeder/FeederIO.kt | 2 ++ .../subsystems/feeder/FeederIONeo.kt | 8 +------ .../subsystems/superstructure/Request.kt | 1 - 4 files changed, 8 insertions(+), 25 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 026f7be6..3fe709cb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -65,28 +65,19 @@ class Feeder(val io: FeederIO) : SubsystemBase() { var nextState = currentState when (currentState) { FeederStates.UNINITIALIZED -> { - nextState = FeederStates.IDLE - } - FeederStates.IDLE -> { - setFeederVoltage(FeederConstants.FEEDER_IDLE_VOLTAGE) - nextState = fromRequestToState(currentRequest) + nextState = FeederStates.OPEN_LOOP_INTAKE } FeederStates.OPEN_LOOP_INTAKE -> { - if (!hasNote) { setFeederVoltage(feederTargetVoltage) - nextState = fromRequestToState(currentRequest) - } else { - nextState = FeederStates.IDLE - } + } + nextState = fromRequestToState(currentRequest) } FeederStates.OPEN_LOOP_SHOOT -> { if (hasNote) { setFeederVoltage(feederTargetVoltage) - nextState = fromRequestToState(currentRequest) - } else { - nextState = FeederStates.IDLE } + nextState = fromRequestToState(currentRequest) } } currentState = nextState @@ -112,22 +103,19 @@ class Feeder(val io: FeederIO) : SubsystemBase() { companion object { enum class FeederStates { UNINITIALIZED, - IDLE, OPEN_LOOP_INTAKE, OPEN_LOOP_SHOOT; fun equivalentToRequest(request: Request.FeederRequest): Boolean { return ( (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_SHOOT) || - (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_INTAKE) || - (request is Request.FeederRequest.Idle && this == IDLE) + (request is Request.FeederRequest.OpenLoopIntake && this == OPEN_LOOP_INTAKE) ) } } fun fromRequestToState(request: Request.FeederRequest): FeederStates { return when (request) { - is Request.FeederRequest.Idle -> FeederStates.IDLE is Request.FeederRequest.OpenLoopIntake -> FeederStates.OPEN_LOOP_INTAKE is Request.FeederRequest.OpenLoopShoot -> FeederStates.OPEN_LOOP_SHOOT } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 9464277b..00fafbbf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -33,6 +33,7 @@ interface FeederIO { table?.put("feederStatorCurrent", feederStatorCurrent.inAmperes) table?.put("feederSupplyCurrent", feederSupplyCurrent.inAmperes) table?.put("feederTempCelcius", feederTemp.inCelsius) + table?.put("feederBeamBroken", beamBroken) } override fun fromLog(table: LogTable?) { @@ -53,6 +54,7 @@ interface FeederIO { } table?.get("feederTempCelcius", feederTemp.inCelsius)?.let { feederTemp = it.celsius } + table?.get("feederBeamBroken", beamBroken)?.let { beamBroken = it } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt index 380d625b..80c437a0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIONeo.kt @@ -37,7 +37,6 @@ object FeederIONeo : FeederIO { feederSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - feederSparkMax.openLoopRampRate = 0.0 feederSparkMax.burnFlash() MotorChecker.add( @@ -47,7 +46,7 @@ object FeederIONeo : FeederIO { mutableListOf(Neo(feederSparkMax, "Roller Motor")), FeederConstants.FEEDER_CURRENT_LIMIT, 70.celsius, - FeederConstants.FEEDER_CURRENT_LIMIT - 0.amps, + 30.amps, 90.celsius ), ) @@ -66,11 +65,6 @@ object FeederIONeo : FeederIO { inputs.feederStatorCurrent * feederSparkMax.appliedOutput.absoluteValue inputs.feederTemp = feederSparkMax.motorTemperature.celsius - Logger.recordOutput( - "Intake/rollerMotorOvercurrentFault", - feederSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) - ) - Logger.recordOutput("Intake/busVoltage", feederSparkMax.busVoltage) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index fc1afc4e..6c8b29bd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -28,7 +28,6 @@ sealed interface Request { } sealed interface FeederRequest : Request { - class Idle() : FeederRequest class OpenLoopIntake(val feederVoltage: ElectricalPotential) : FeederRequest class OpenLoopShoot(val feederVoltage: ElectricalPotential) : FeederRequest }