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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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/31] 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