diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 964ffad7..863ca494 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 86 -const val GIT_SHA = "c4213047cf865f51a94aec532ae4b5e5ab6465da" -const val GIT_DATE = "2024-01-21T04:31:29Z" -const val GIT_BRANCH = "shooter" -const val BUILD_DATE = "2024-01-21T04:32:08Z" -const val BUILD_UNIX_TIME = 1705829528858L +const val GIT_REVISION = 88 +const val GIT_SHA = "b165996f5456e4e0ce9ce385825d05becb33dc7b" +const val GIT_DATE = "2024-01-21T04:36:39Z" +const val GIT_BRANCH = "HEAD" +const val BUILD_DATE = "2024-01-22T19:59:20Z" +const val BUILD_UNIX_TIME = 1705971560513L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 6e18d841..6a59d500 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -68,6 +68,7 @@ object RobotContainer { vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) limelight.poseSupplier = { drivetrain.odometryPose } + } fun mapDefaultCommands() { 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 678ae65e..905ed2ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -37,6 +37,8 @@ object FlywheelConstants { val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps + val FLYWHEEL_VELOCITY_TOLERANCE = 50.0.rotations.perMinute + object PID { val REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute val REAL_KI: IntegralGain, 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 ab8fe106..c66a0720 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -3,11 +3,15 @@ package com.team4099.robot2023.subsystems.flywheel import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.Wrist.Companion.fromRequestToState import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SubsystemBase +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.seconds @@ -23,7 +27,8 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond -class Flywheel(val io: FlywheelIO) { +class Flywheel(val io: FlywheelIO):SubsystemBase() { + //right flywheel is the only motor private val kP = LoggedTunableValue( "Flywheel/kP", @@ -71,6 +76,14 @@ class Flywheel(val io: FlywheelIO) { var flywheelTargetVelocity: AngularVelocity = 0.rotations.perMinute + val isAtTargetedPosition: Boolean + get() = + ( + currentState == Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY && + (inputs.rightFlywheelVelocity - flywheelTargetVelocity).absoluteValue <= + FlywheelConstants.FLYWHEEL_VELOCITY_TOLERANCE + ) + var currentState = Companion.FlywheelStates.UNINITIALIZED var currentRequest: Request.FlywheelRequest = Request.FlywheelRequest.OpenLoop(0.0.volts) @@ -105,7 +118,7 @@ class Flywheel(val io: FlywheelIO) { FlywheelConstants.PID.FLYWHEEL_KA ) } - fun periodic() { + override fun periodic() { io.updateInputs(inputs) if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { io.configPID(kP.get(), kI.get(), kD.get()) @@ -115,6 +128,13 @@ class Flywheel(val io: FlywheelIO) { flywheelFeedForward = SimpleMotorFeedforward(flywheelkS.get(), flywheelkV.get(), flywheelkA.get()) } + Logger.processInputs("Flywheel", inputs) + + Logger.recordOutput("Flywheel/currentState", currentState.name) + + Logger.recordOutput("Flywheel/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("Flywheel/isAtTargetedVelocity", isAtTargetedPosition) var nextState = currentState when (currentState) { 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 dd857653..c0f5773d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -45,11 +45,7 @@ interface FlywheelIO { table.put("flywheelRightSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes) table.put("flywheelRightTemperature", rightFlywheelTemperature.inCelsius) - table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond) - table.put("flywheelLeftAppliedVoltage", leftFlywheelAppliedVoltage.inVolts) - table.put("flywheelLeftStatorCurrent", leftFlywheelStatorCurrent.inAmperes) - table.put("flywheelLeftSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes) - table.put("flywheelLeftTemperature", leftFlywheelTemperature.inCelsius) + } override fun fromLog(table: LogTable) { @@ -69,22 +65,6 @@ interface FlywheelIO { table.get("rightFlywheelTempreature", rightFlywheelTemperature.inCelsius).let { rightFlywheelTemperature = it.celsius } - - // LeFt motor - table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond).let { - leftFlywheelVelocity = it.radians.perSecond - } - table.get("leftFlywheelAppliedVoltage", leftFlywheelAppliedVoltage.inVolts).let { - leftFlywheelAppliedVoltage = it.volts - } - table.get("leftFlywheelStatorCurrent", leftFlywheelStatorCurrent.inAmperes).let { - leftFlywheelStatorCurrent = it.amps - } - table.get("leftFlywheelSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes).let { - leftFlywheelSupplyCurrent = it.amps - } - table.get("leftFlywheelTempreature", leftFlywheelTemperature.inCelsius).let { - leftFlywheelTemperature = it.celsius } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 2c0e7a7b..24c7849c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ArmFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -32,7 +33,7 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond -class Wrist(val io: WristIO) { +class Wrist(val io: WristIO):SubsystemBase() { val inputs = WristIO.WristIOInputs() private val wristkS = @@ -136,7 +137,7 @@ class Wrist(val io: WristIO) { ) } - fun periodic() { + override fun periodic() { io.updateInputs(inputs) if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { io.configPID(wristkP.get(), wristkI.get(), wristkD.get())