From af060f29e117bcb356a8028ace24474cd3e9c484 Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sun, 28 Jan 2024 19:36:14 -0500 Subject: [PATCH] pid variable adjustments --- .../kotlin/com/team4099/robot2023/BuildConstants.kt | 12 ++++++------ .../robot2023/config/constants/FlywheelConstants.kt | 6 +++--- .../robot2023/subsystems/flywheel/Flywheel.kt | 2 +- .../robot2023/subsystems/flywheel/FlywheelIO.kt | 13 +++++++------ .../robot2023/subsystems/flywheel/FlywheelIOSim.kt | 6 ++---- 5 files changed, 19 insertions(+), 20 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 460421f5..eb54ec4e 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -4,12 +4,12 @@ package com.team4099.robot2023 * Automatically generated file containing build version information. */ const val MAVEN_GROUP = "" -const val MAVEN_NAME = "cresendo" +const val MAVEN_NAME = "Crescendo-2024v2" const val VERSION = "unspecified" -const val GIT_REVISION = 101 -const val GIT_SHA = "a599b3e0917de8004033315a58f3dad671b072e5" -const val GIT_DATE = "2024-01-28T18:05:00Z" +const val GIT_REVISION = 102 +const val GIT_SHA = "604a3c944cf21d691d7f2d854357417009e237cd" +const val GIT_DATE = "2024-01-28T19:00:59Z" const val GIT_BRANCH = "shooter" -const val BUILD_DATE = "2024-01-28T18:58:53Z" -const val BUILD_UNIX_TIME = 1706486333778L +const val BUILD_DATE = "2024-01-28T19:34:57Z" +const val BUILD_UNIX_TIME = 1706488497518L const val DIRTY = 1 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 3b63543b..f9843aac 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -39,21 +39,21 @@ object FlywheelConstants { val FLYWHEEL_TOLERANCE = 50.0.rotations.perMinute object PID { val RIGHT_REAL_KP: ProportionalGain, Volt> = - 0.001.volts / 1.0.rotations.perMinute + 0.0.volts / 1.0.rotations.perMinute val RIGHT_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) val RIGHT_REAL_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute.perSecond) val RIGHT_SIM_KP: ProportionalGain, Volt> = - 0.1.volts / 1.0.rotations.perMinute + 0.0.volts / 1.0.rotations.perMinute val RIGHT_SIM_KI: IntegralGain, Volt> = 0.0.volts / (1.0.radians.perSecond * 1.0.seconds) val RIGHT_SIM_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute.perSecond) val LEFT_REAL_KP: ProportionalGain, Volt> = - 0.001.volts / 1.0.rotations.perMinute + 0.1.volts / 1.0.rotations.perMinute val LEFT_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) val LEFT_REAL_KD: DerivativeGain, Volt> = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index 06e528fa..ffbbf867 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -267,7 +267,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() { } fun flywheelOpenLoopCommand(): Command { - return runOnce({ currentRequest = Request.FlywheelRequest.OpenLoop(10.volts) }) + return runOnce({ currentRequest = Request.FlywheelRequest.OpenLoop(12.volts) }) } fun flywheelResetCommand(): Command { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt index 2ccfec95..02be6abc 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -21,6 +21,7 @@ import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond @@ -45,7 +46,7 @@ interface FlywheelIO { var isSimulated = false override fun toLog(table: LogTable) { - table.put("flywheelRightVelocityRPM", rightFlywheelVelocity.inRadiansPerSecond) + table.put("flywheelRightVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute) table.put("flywheelRightAppliedVoltage", rightFlywheelAppliedVoltage.inVolts) table.put("flywheelRightStatorCurrent", rightFlywheelStatorCurrent.inAmperes) table.put("flywheelRightSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes) @@ -53,7 +54,7 @@ interface FlywheelIO { table.put("rightFlywheelDutyCycle", rightFlywheelDutyCycle.inVolts) table.put("rightFlywheelTorque", rightFlywheelTorque.inNewtons) - table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond) + table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRotationsPerMinute) table.put("flywheelLeftAppliedVoltage", leftFlywheelAppliedVoltage.inVolts) table.put("flywheelLeftStatorCurrent", leftFlywheelStatorCurrent.inAmperes) table.put("flywheelLeftSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes) @@ -64,8 +65,8 @@ interface FlywheelIO { override fun fromLog(table: LogTable) { // Flywheel logs - table.get("rightFlywheelVelocityRPM", rightFlywheelVelocity.inRadiansPerSecond).let { - rightFlywheelVelocity = it.radians.perSecond + table.get("rightFlywheelVelocityRPM", rightFlywheelVelocity.inRotationsPerMinute).let { + rightFlywheelVelocity = it.rotations.perMinute } table.get("rightFlywheelAppliedVoltage", rightFlywheelAppliedVoltage.inVolts).let { rightFlywheelAppliedVoltage = it.volts @@ -87,8 +88,8 @@ interface FlywheelIO { } // Left motor - table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond).let { - leftFlywheelVelocity = it.radians.perSecond + table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRotationsPerMinute).let { + leftFlywheelVelocity = it.rotations.perMinute } table.get("leftFlywheelAppliedVoltage", leftFlywheelAppliedVoltage.inVolts).let { leftFlywheelAppliedVoltage = it.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt index d61c6fbd..4272e29d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt @@ -20,14 +20,14 @@ import org.team4099.lib.units.perMinute object FlywheelIOSim : FlywheelIO { private val flywheelRightSim: FlywheelSim = FlywheelSim( - DCMotor.getKrakenX60Foc(1), + DCMotor.getFalcon500(2), FlywheelConstants.LEFT_GEAR_RATIO, FlywheelConstants.INERTIA.inKilogramsMeterSquared ) private val flywheelLeftSim: FlywheelSim = FlywheelSim( - DCMotor.getKrakenX60Foc(1), + DCMotor.getFalcon500(2), FlywheelConstants.RIGHT_GEAR_RATIO, FlywheelConstants.INERTIA.inKilogramsMeterSquared ) @@ -53,14 +53,12 @@ object FlywheelIOSim : FlywheelIO { flywheelLeftSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) flywheelRightSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - inputs.leftFlywheelVelocity = flywheelLeftSim.getAngularVelocityRPM().rotations.perMinute inputs.leftFlywheelVelocity = flywheelLeftSim.getAngularVelocityRPM().rotations.perMinute inputs.leftFlywheelAppliedVoltage = appliedLeftVoltage inputs.leftFlywheelSupplyCurrent = 0.amps inputs.leftFlywheelStatorCurrent = flywheelLeftSim.currentDrawAmps.amps inputs.leftFlywheelTemperature = 0.0.celsius - inputs.rightFlywheelVelocity = flywheelRightSim.getAngularVelocityRPM().rotations.perMinute inputs.rightFlywheelVelocity = flywheelRightSim.getAngularVelocityRPM().rotations.perMinute inputs.rightFlywheelAppliedVoltage = appliedRightVoltage inputs.rightFlywheelSupplyCurrent = 0.amps