From c1ecbcf7516d54d009cbeac304fc9d1a52185aae Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 01:34:39 -0400 Subject: [PATCH 1/9] working battery voltage status bar --- .../com/team4099/robot2023/RobotContainer.kt | 6 +- .../config/constants/IntakeConstants.kt | 7 +- .../config/constants/LEDConstants.kt | 13 ++- .../config/constants/WristConstants.kt | 2 +- .../subsystems/elevator/ElevatorIO.kt | 2 +- .../subsystems/intake/IntakeIOFalconNEO.kt | 17 ++-- .../robot2023/subsystems/led/LedIO.kt | 3 + .../robot2023/subsystems/led/LedIOCandle.kt | 96 ++++++++++++++++++- .../team4099/robot2023/subsystems/led/Leds.kt | 25 ++--- .../superstructure/Superstructure.kt | 2 - .../robot2023/subsystems/wrist/WristIO.kt | 2 +- 11 files changed, 142 insertions(+), 33 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9c96230a..eabd800f 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -15,6 +15,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator +import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder @@ -34,6 +35,7 @@ import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIO import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.subsystems.wrist.Wrist +import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.subsystems.wrist.WristIOTalon import com.team4099.robot2023.util.driver.Ryan @@ -80,9 +82,9 @@ object RobotContainer { limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(ElevatorIONEO) + elevator = Elevator(object : ElevatorIO {}) flywheel = Flywheel(FlywheelIOTalon) - wrist = Wrist(WristIOTalon) + wrist = Wrist(object : WristIO {}) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 0682b554..47b13607 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -4,6 +4,7 @@ import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts @@ -17,7 +18,11 @@ object IntakeConstants { val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees) // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 80.0.amps + val ROLLER_CURRENT_LIMIT = 80.amps + val ROLLER_SUPPLY_CURRENT_LIMIT = 120.0.amps + val ROLLER_STATOR_CURRENT_LIMIT = 200.0.amps + val ROLLER_CURRENT_TIME_THRESHOLD = 1.5.seconds + val ROLLER_SUPPLY_TRIGGER_THRESHOLD = 65.amps const val ROLLER_MOTOR_INVERTED = true const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index 4d135228..45eafef3 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -7,23 +7,26 @@ import org.team4099.lib.units.derived.volts object LEDConstants { val INTAKE_CURRENT_THRESHOLD = 15.amps val OUTAKE_CURRENT_THRESHOLD = 20.amps + val BATTERY_FULL_THRESHOLD = 12.5.volts val LED_COUNT = 50 - val BATTERY_WARNING_THRESHOLD = 12.3.volts - - enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) { + enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int, var attachedValue: Any? = null) { // Gold NO_NOTE(null, 0, 0, 0), NOTHING(null, 0, 0, 0), RED(null, 255, 0, 0), BLUE(null, 0, 0, 255), + GREEN(null, 0, 255, 0), + PURPLE(null, 160, 32, 240), + MAGENTA(null, 255, 0, 255), // Blue HAS_NOTE(null, 0, 0, 255), - // Red - LOW_BATTERY(null, 255, 105, 0), + // Yellow + BATTERY_DISPLAY(null, 255, 105, 0), + WHITE(null, 255, 255, 255), // Green diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 8a193c77..37e35967 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -31,7 +31,7 @@ object WristConstants { val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 1.06488 / 1.0 val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO = - 5.0 / 1.0 * 4.0 / 1.0 * 3.0 / 1.0 * 46.0 / 42.0 * 90.0 / 33.0 * 1.0 / 1.06488 + 5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / 1.06488 val VOLTAGE_COMPENSATION = 12.0.volts val ABSOLUTE_ENCODER_OFFSET = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index a5e20665..28013a0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -37,7 +37,7 @@ interface ElevatorIO { var leaderTempCelcius = 0.0.celsius var followerTempCelcius = 0.0.celsius - var isSimulating = false + var isSimulating = true override fun toLog(table: LogTable) { table?.put("elevatorPositionInches", elevatorPosition.inInches) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt index 50c171c3..acc1e27c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt @@ -20,6 +20,7 @@ import com.team4099.robot2023.subsystems.falconspin.Neo import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts @@ -56,11 +57,13 @@ object IntakeIOFalconNEO : IntakeIO { var rollerFalconConfiguration = TalonFXConfiguration() rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes - rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit = - IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit = + IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes +// rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds +// rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true - rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = false + rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive rollerFalconConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast @@ -76,7 +79,7 @@ object IntakeIOFalconNEO : IntakeIO { "Roller", MotorCollection( mutableListOf(Falcon500(rollerFalcon, "Roller Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius @@ -87,7 +90,7 @@ object IntakeIOFalconNEO : IntakeIO { centerWheelSparkMax.clearFaults() centerWheelSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + // centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) centerWheelSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast @@ -99,7 +102,7 @@ object IntakeIOFalconNEO : IntakeIO { "Center Wheel", MotorCollection( mutableListOf(Neo(centerWheelSparkMax, "Center Wheel Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, + IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT, 60.celsius, 50.amps, 120.celsius diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt index 26ac3c09..0ede25f5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt @@ -3,8 +3,11 @@ package com.team4099.robot2023.subsystems.led import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.derived.ElectricalPotential interface LedIO { + var batteryVoltage: ElectricalPotential + class LedIOInputs : LoggableInputs { var ledState = LEDConstants.CandleState.NO_NOTE.toString() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index e14e188b..21dbc4a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -4,11 +4,17 @@ import com.ctre.phoenix.led.CANdle import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.LEDConstants import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts object LedIOCandle : LedIO { private val ledController = CANdle(Constants.LED.LED_CANDLE_ID) private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE + private var runs = 0 + private var reverseLEDS = false + override var batteryVoltage: ElectricalPotential = 12.0.volts override fun updateInputs(inputs: LedIO.LedIOInputs) { inputs.ledState = lastState.name @@ -20,8 +26,96 @@ object LedIOCandle : LedIO { setCANdleState(newState) } + private fun wave( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + lengthOfWave: Int = 10, + lengthOfGradient: Double = 5.0, + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, runs, lengthOfWave) + + // Set outer colors + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, runs - lengthOfGradient.toInt()) + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, runs + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - runs - lengthOfWave) + + // Set gradient colors + if (lengthOfGradient > 0.0) { + for ((step, ledIdx) in ((runs - lengthOfGradient.toInt())..runs).withIndex()) { + ledController.setLEDs( + (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(), + (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(), + (defaultState.b + (state.b - defaultState.b) * ((step + 1) / lengthOfGradient)).toInt(), + 0, + ledIdx, + 1 + ) + } + + for ((step, ledIdx) in (runs..(runs + lengthOfGradient).toInt()).withIndex()) { + ledController.setLEDs( + (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), + (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), + (defaultState.b + (state.b - defaultState.b) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), + 0, + ledIdx, + 1 + ) + } + } + + if (runs >= LEDConstants.LED_COUNT - lengthOfWave) { + reverseLEDS = true + } + else if (runs < lengthOfWave){ + reverseLEDS = false + } + + runs += if (!reverseLEDS) 1 else -1 + } + + private fun gradient( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + lengthOfSolidEnds: Int = 10 + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds) + ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds) + + for ((step, ledIdx) in (lengthOfSolidEnds..(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) { + ledController.setLEDs( + (state.r + (otherState.r - state.r) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + (state.g + (otherState.g - state.g) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + (state.b + (otherState.b - state.b) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + 0, + ledIdx, + 1 + ) + } + } + + private fun progressBar( + state: LEDConstants.CandleState, + defaultState: LEDConstants.CandleState, + percent: Double + ) { + ledController.setLEDs(state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt()) + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT) + } + private fun setCANdleState(state: LEDConstants.CandleState) { - if (state.animation == null) { + if (state == LEDConstants.CandleState.BATTERY_DISPLAY) { + progressBar( + LEDConstants.CandleState.BATTERY_DISPLAY, + LEDConstants.CandleState.NOTHING, + (batteryVoltage.inVolts - 11.5) / (LEDConstants.BATTERY_FULL_THRESHOLD.inVolts - 11.5) + ) + } + else if (state == LEDConstants.CandleState.BLUE) { + ledController.clearAnimation(0) + val otherState = LEDConstants.CandleState.MAGENTA + gradient(state, otherState) + } + else if (state.animation == null) { ledController.clearAnimation(0) ledController.setLEDs(state.r, state.g, state.b) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index 7dd0f7b9..ea14466b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,9 +1,13 @@ package com.team4099.robot2023.subsystems.led +import com.team4099.robot2023.RobotContainer import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation +import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.derived.volts +import java.sql.Driver class Leds(val io: LedIO) { var inputs = LedIO.LedIOInputs() @@ -11,7 +15,6 @@ class Leds(val io: LedIO) { var hasNote = false var subsystemsAtPosition = false var isIdle = true - var batteryIsLow = false var state = LEDConstants.CandleState.NO_NOTE set(value) { @@ -21,17 +24,15 @@ class Leds(val io: LedIO) { fun periodic() { io.updateInputs(inputs) - if (batteryIsLow && DriverStation.isDisabled()) { - state = LEDConstants.CandleState.LOW_BATTERY - } else if (DriverStation.isDisabled()) { - if (DriverStation.getAlliance().isPresent) { - if (FMSData.isBlue) { - state = LEDConstants.CandleState.BLUE - } else { - state = LEDConstants.CandleState.RED - } - } else { - state = LEDConstants.CandleState.NOTHING + if (DriverStation.getAlliance().isEmpty) { + io.batteryVoltage = RobotController.getBatteryVoltage().volts + state = LEDConstants.CandleState.BATTERY_DISPLAY + } + else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { + if (FMSData.isBlue) { + state = LEDConstants.CandleState.BLUE + } else { + state = LEDConstants.CandleState.RED } } else if (hasNote) { if (subsystemsAtPosition && !isIdle) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index 0b60eca0..e7635db8 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -143,8 +143,6 @@ class Superstructure( leds.isIdle = currentState == SuperstructureStates.IDLE leds.subsystemsAtPosition = wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition - leds.batteryIsLow = - RobotController.getBatteryVoltage() < LEDConstants.BATTERY_WARNING_THRESHOLD.inVolts val ledLoopStartTime = Clock.realTimestamp leds.periodic() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt index d8f031a0..4607448c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -37,7 +37,7 @@ interface WristIO { var wristAcceleration = 0.0.degrees.perSecond.perSecond - var isSimulated = false + var isSimulated = true override fun toLog(table: LogTable) { table.put("wristPosition", wristPosition.inDegrees) From 2047e846c07324726fae9c98fd960059e80dade3 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 02:09:35 -0400 Subject: [PATCH 2/9] more led stuff --- .../com/team4099/utils/LimelightHelpers.java | 228 +++++++++++++++++- .../com/team4099/robot2023/RobotContainer.kt | 3 +- .../robot2023/config/constants/Constants.kt | 4 +- .../config/constants/VisionConstants.kt | 2 +- .../robot2023/subsystems/led/LedIOCandle.kt | 50 ++-- .../subsystems/limelight/LimelightVision.kt | 1 + 6 files changed, 262 insertions(+), 26 deletions(-) diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java index 0cb199a3..9ef9ec52 100644 --- a/src/main/java/com/team4099/utils/LimelightHelpers.java +++ b/src/main/java/com/team4099/utils/LimelightHelpers.java @@ -1,4 +1,5 @@ -package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023) +package com.team4099.utils;//com.team4099.utils.LimelightHelpers +//LimelightHelpers v1.5.0 (March 27, 2024) import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -301,6 +302,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -361,8 +374,59 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -383,7 +447,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -395,7 +459,7 @@ private static Pose3d toPose3D(double[] inData){ private static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -403,6 +467,86 @@ private static Pose2d toPose2D(double[] inData){ return new Pose2d(tran2d, r2d); } + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -542,8 +686,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -602,6 +746,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -616,6 +782,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) @@ -641,6 +827,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. @@ -694,6 +885,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -763,7 +976,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); @@ -775,5 +988,4 @@ public static LimelightResults getLatestResults(String limelightName) { return results; } -} - +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index eabd800f..a2426c5c 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -29,6 +29,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision @@ -79,7 +80,7 @@ object RobotContainer { drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2")) - limelight = LimelightVision(object : LimelightVisionIO {}) + limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) elevator = Elevator(object : ElevatorIO {}) 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 41089e90..c547889c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -54,8 +54,8 @@ object Constants { object Tuning { - const val TUNING_MODE = false - const val DEBUGING_MODE = false + const val TUNING_MODE = true + const val DEBUGING_MODE = true const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index 2903f1aa..babc80e8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -77,7 +77,7 @@ object VisionConstants { } object Limelight { - val LIMELIGHT_NAME = "limelight-zapdos" + val LIMELIGHT_NAME = "limelight-owl" val HORIZONTAL_FOV = 59.6.degrees val VERITCAL_FOV = 45.7.degrees val HIGH_TAPE_HEIGHT = 43.875.inches + 1.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index 21dbc4a5..9dd2aad4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -12,8 +12,10 @@ object LedIOCandle : LedIO { private val ledController = CANdle(Constants.LED.LED_CANDLE_ID) private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE - private var runs = 0 + private var waveRuns = 0 + private var loopCycles = 0 private var reverseLEDS = false + private var finishedFade = false override var batteryVoltage: ElectricalPotential = 12.0.volts override fun updateInputs(inputs: LedIO.LedIOInputs) { @@ -32,15 +34,15 @@ object LedIOCandle : LedIO { lengthOfWave: Int = 10, lengthOfGradient: Double = 5.0, ) { - ledController.setLEDs(state.r, state.g, state.b, 0, runs, lengthOfWave) + ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave) // Set outer colors - ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, runs - lengthOfGradient.toInt()) - ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, runs + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - runs - lengthOfWave) + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt()) + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, waveRuns + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - waveRuns - lengthOfWave) // Set gradient colors if (lengthOfGradient > 0.0) { - for ((step, ledIdx) in ((runs - lengthOfGradient.toInt())..runs).withIndex()) { + for ((step, ledIdx) in ((waveRuns - lengthOfGradient.toInt())..waveRuns).withIndex()) { ledController.setLEDs( (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(), (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(), @@ -51,7 +53,7 @@ object LedIOCandle : LedIO { ) } - for ((step, ledIdx) in (runs..(runs + lengthOfGradient).toInt()).withIndex()) { + for ((step, ledIdx) in (waveRuns..(waveRuns + lengthOfGradient).toInt()).withIndex()) { ledController.setLEDs( (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), @@ -63,14 +65,14 @@ object LedIOCandle : LedIO { } } - if (runs >= LEDConstants.LED_COUNT - lengthOfWave) { + if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) { reverseLEDS = true } - else if (runs < lengthOfWave){ + else if (waveRuns < lengthOfWave){ reverseLEDS = false } - runs += if (!reverseLEDS) 1 else -1 + waveRuns += if (!reverseLEDS) 1 else -1 } private fun gradient( @@ -81,11 +83,11 @@ object LedIOCandle : LedIO { ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds) ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds) - for ((step, ledIdx) in (lengthOfSolidEnds..(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) { + for ((step, ledIdx) in (lengthOfSolidEnds until(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) { ledController.setLEDs( - (state.r + (otherState.r - state.r) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), - (state.g + (otherState.g - state.g) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), - (state.b + (otherState.b - state.b) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + (state.r + (otherState.r - state.r) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + (state.g + (otherState.g - state.g) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + (state.b + (otherState.b - state.b) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), 0, ledIdx, 1 @@ -102,6 +104,26 @@ object LedIOCandle : LedIO { ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT) } + private fun fadeBetweenColors( + state: LEDConstants.CandleState, + otherState: LEDConstants.CandleState, + loopCyclesToConverge: Int = 5 + ) { + for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT).withIndex()) { + val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() + val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() + val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() + ledController.setLEDs( + calculatedR, + calculatedB, + calculatedG, + 0, + ledIdx, + 1 + ) + } + } + private fun setCANdleState(state: LEDConstants.CandleState) { if (state == LEDConstants.CandleState.BATTERY_DISPLAY) { progressBar( @@ -113,7 +135,7 @@ object LedIOCandle : LedIO { else if (state == LEDConstants.CandleState.BLUE) { ledController.clearAnimation(0) val otherState = LEDConstants.CandleState.MAGENTA - gradient(state, otherState) + fadeBetweenColors(state, otherState) } else if (state.animation == null) { ledController.clearAnimation(0) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 00d19361..495d6841 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -63,6 +63,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { init {} override fun periodic() { + io.setPipeline(0) val startTime = Clock.realTimestamp io.updateInputs(inputs) Logger.processInputs("LimelightVision", inputs) From d9f698dc9959a76b57a9ac35dbe0c5d4b6140030 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 03:12:04 -0400 Subject: [PATCH 3/9] more led stuff --- .../com/team4099/robot2023/RobotContainer.kt | 1 + .../config/constants/LEDConstants.kt | 3 +- .../robot2023/subsystems/led/LedIOCandle.kt | 32 ++++++++++++++++--- .../subsystems/limelight/LimelightVision.kt | 4 ++- 4 files changed, 33 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index a2426c5c..9ccc73d4 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -164,6 +164,7 @@ object RobotContainer { } fun mapTeleopControls() { + limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain)) ControlBoard.intake.whileTrue( diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index 45eafef3..1d64cc73 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -15,9 +15,10 @@ object LEDConstants { NO_NOTE(null, 0, 0, 0), NOTHING(null, 0, 0, 0), RED(null, 255, 0, 0), + LIGHT_RED(null, 255, 67, 36), BLUE(null, 0, 0, 255), + PURPLE(null, 67, 36, 255), GREEN(null, 0, 255, 0), - PURPLE(null, 160, 32, 240), MAGENTA(null, 255, 0, 255), // Blue diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index 9dd2aad4..07973082 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -7,6 +7,7 @@ import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts +import kotlin.math.absoluteValue object LedIOCandle : LedIO { @@ -107,21 +108,39 @@ object LedIOCandle : LedIO { private fun fadeBetweenColors( state: LEDConstants.CandleState, otherState: LEDConstants.CandleState, - loopCyclesToConverge: Int = 5 + loopCyclesToConverge: Int = 10 ) { - for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT).withIndex()) { + val stepUpInLoopCycles = 2 + var reachedStartColor = true + var reachedEndColor = true + + for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles).withIndex()) { val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() + + reachedStartColor = (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b) + reachedEndColor = ((calculatedR - otherState.r).absoluteValue < 10 && (calculatedG - otherState.g).absoluteValue < 10 && (calculatedB - otherState.b).absoluteValue < 10) + ledController.setLEDs( calculatedR, - calculatedB, calculatedG, + calculatedB, 0, ledIdx, 1 ) } + + loopCycles += if (finishedFade) -1 else 1 + + if (reachedStartColor) { + finishedFade = false + } + + if (reachedEndColor) { // All LEDs are at the end color + finishedFade = true + } } private fun setCANdleState(state: LEDConstants.CandleState) { @@ -134,8 +153,11 @@ object LedIOCandle : LedIO { } else if (state == LEDConstants.CandleState.BLUE) { ledController.clearAnimation(0) - val otherState = LEDConstants.CandleState.MAGENTA - fadeBetweenColors(state, otherState) + fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2) + } + else if (state == LEDConstants.CandleState.RED) { + ledController.clearAnimation(0) + fadeBetweenColors(state, LEDConstants.CandleState.LIGHT_RED, loopCyclesToConverge = 2) } else if (state.animation == null) { ledController.clearAnimation(0) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt index 495d6841..4ca49ca3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt @@ -63,7 +63,9 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() { init {} override fun periodic() { - io.setPipeline(0) + visibleGamePieces.clear() + targetGamePieceTx = null + val startTime = Clock.realTimestamp io.updateInputs(inputs) Logger.processInputs("LimelightVision", inputs) From 2ed8d8d2f5d2953eb38639d34b1738bbee6e814c Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 20:12:35 -0400 Subject: [PATCH 4/9] Wrist tuning and vision constants --- .../com/team4099/utils/LimelightHelpers.java | 18 +- .../com/team4099/robot2023/RobotContainer.kt | 6 +- .../robot2023/auto/mode/TestAutoPath.kt | 14 +- .../ThreeNoteCenterlineFromAmpAutoPath.kt | 2 +- .../robot2023/config/constants/Constants.kt | 2 +- .../config/constants/DrivetrainConstants.kt | 8 +- .../config/constants/LEDConstants.kt | 3 +- .../config/constants/WristConstants.kt | 5 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 5 + .../subsystems/intake/IntakeIOFalconNEO.kt | 7 +- .../robot2023/subsystems/led/LedIOCandle.kt | 170 +++++++++++------- .../team4099/robot2023/subsystems/led/Leds.kt | 7 +- .../superstructure/Superstructure.kt | 3 - .../robot2023/subsystems/vision/Vision.kt | 8 +- .../subsystems/wrist/WristIOTalon.kt | 25 ++- 15 files changed, 162 insertions(+), 121 deletions(-) diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java index 9ef9ec52..1431ef51 100644 --- a/src/main/java/com/team4099/utils/LimelightHelpers.java +++ b/src/main/java/com/team4099/utils/LimelightHelpers.java @@ -514,18 +514,18 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr private static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { - System.out.println("No PoseEstimate available."); + // System.out.println("No PoseEstimate available."); return; } - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.println(); +// System.out.printf("Pose Estimate Information:%n"); +// System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); +// System.out.printf("Latency: %.3f ms%n", pose.latency); +// System.out.printf("Tag Count: %d%n", pose.tagCount); +// System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); +// System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); +// System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); +// System.out.println(); if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { System.out.println("No RawFiducials data available."); diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9ccc73d4..9a61c508 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -15,7 +15,6 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.elevator.Elevator -import com.team4099.robot2023.subsystems.elevator.ElevatorIO import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.feeder.Feeder @@ -36,7 +35,6 @@ import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIO import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.subsystems.wrist.Wrist -import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.subsystems.wrist.WristIOTalon import com.team4099.robot2023.util.driver.Ryan @@ -83,9 +81,9 @@ object RobotContainer { limelight = LimelightVision(LimelightVisionIOReal) intake = Intake(IntakeIOFalconNEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(object : ElevatorIO {}) + elevator = Elevator(ElevatorIONEO) flywheel = Flywheel(FlywheelIOTalon) - wrist = Wrist(object : WristIO {}) + wrist = Wrist(WristIOTalon) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 9f5e456b..3005997f 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -27,21 +27,21 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 2.meters, startingPose.y + 0.02.meters) + Translation2d(startingPose.x + 1.5.meters, startingPose.y + 0.02.meters) .translation2d, null, - (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds + (startingPose.rotation + 15.degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 4.meters, startingPose.y).translation2d, + Translation2d(startingPose.x + 3.meters, startingPose.y).translation2d, null, - (startingPose.rotation + 45.degrees).inRotation2ds + (startingPose.rotation + 30.degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 2.meters, startingPose.y - 0.02.meters) + Translation2d(startingPose.x + 1.5.meters, startingPose.y - 0.02.meters) .translation2d, null, - (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds + (startingPose.rotation + 15.degrees).inRotation2ds ), FieldWaypoint( Translation2d(startingPose.x, startingPose.y).translation2d, @@ -50,7 +50,7 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur ) ) }, - useLowerTolerance = false + useLowerTolerance = true ) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt index 18f5fc59..19a1435a 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteCenterlineFromAmpAutoPath.kt @@ -23,7 +23,7 @@ class ThreeNoteCenterlineFromAmpAutoPath( addRequirements(drivetrain) addCommands( superstructure.prepSpeakerLowCommand(), - superstructure.scoreCommand(), + superstructure.scoreCommand().withTimeout(0.75), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( drivetrain, 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 c547889c..7151475d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -55,7 +55,7 @@ object Constants { object Tuning { const val TUNING_MODE = true - const val DEBUGING_MODE = true + const val DEBUGING_MODE = false const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 41ec3a85..de83610e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -33,7 +33,7 @@ object DrivetrainConstants { const val FOC_ENABLED = true const val MINIMIZE_SKEW = false - const val TELEOP_TURNING_SPEED_PERCENT = 0.75 + const val TELEOP_TURNING_SPEED_PERCENT = 1 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 @@ -61,7 +61,7 @@ object DrivetrainConstants { val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 4.0.meters.perSecond // 4 + val MAX_AUTO_VEL = 3.5.meters.perSecond // 4 val MAX_AUTO_ACCEL = 3.25.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 @@ -134,10 +134,10 @@ object DrivetrainConstants { (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians) + val AUTO_THETA_PID_KP = (0.radians.perSecond / 1.radians) val AUTO_THETA_PID_KI = (0.0.radians.perSecond / (1.radians * 1.seconds)) val AUTO_THETA_PID_KD = - (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val TELEOP_ALIGN_PID_KP = 3.6.degrees.perSecond / 1.degrees val TELEOP_ALIGN_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt index 1d64cc73..301245df 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt @@ -10,12 +10,13 @@ object LEDConstants { val BATTERY_FULL_THRESHOLD = 12.5.volts val LED_COUNT = 50 - enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int, var attachedValue: Any? = null) { + enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) { // Gold NO_NOTE(null, 0, 0, 0), NOTHING(null, 0, 0, 0), RED(null, 255, 0, 0), LIGHT_RED(null, 255, 67, 36), + ORANGE(null, 255, 105, 0), BLUE(null, 0, 0, 255), PURPLE(null, 67, 36, 255), GREEN(null, 0, 255, 0), diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 37e35967..d1785393 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -39,7 +39,8 @@ object WristConstants { 97.72227856659904.degrees - 35.degrees + 1.3.degrees - 0.33.degrees - 1.degrees - - 0.5.degrees + 0.5.degrees - + 1.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches val WRIST_INERTIA = 0.7181257183.kilo.grams * 1.0.meters.squared @@ -58,7 +59,7 @@ object WristConstants { val WRIST_ZERO_SIM_OFFSET = 27.5.degrees val MAX_WRIST_VELOCITY = 300.degrees.perSecond - val MAX_WRIST_ACCELERATION = 1000.degrees.perSecond.perSecond + val MAX_WRIST_ACCELERATION = 1500.degrees.perSecond.perSecond val HARDSTOP_OFFSET = 47.degrees object PID { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 80d8b905..e71767a0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -47,6 +47,7 @@ import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inRotation2ds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.concurrent.locks.Lock @@ -470,6 +471,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB ) } + Logger.recordOutput( + "Drivetrain/omegaDegreesPerSecond", desiredChassisSpeeds.omega.inDegreesPerSecond + ) + if (DrivetrainConstants.MINIMIZE_SKEW) { val velocityTransform = Transform2d( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt index acc1e27c..3bf87ce7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt @@ -20,7 +20,6 @@ import com.team4099.robot2023.subsystems.falconspin.Neo import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts @@ -60,8 +59,10 @@ object IntakeIOFalconNEO : IntakeIO { IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit = IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes -// rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds -// rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes + // rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = + // IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds + // rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = + // IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt index 07973082..fe13d6fc 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt @@ -32,44 +32,24 @@ object LedIOCandle : LedIO { private fun wave( state: LEDConstants.CandleState, defaultState: LEDConstants.CandleState, - lengthOfWave: Int = 10, - lengthOfGradient: Double = 5.0, + lengthOfWave: Int = 10 ) { ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave) // Set outer colors - ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt()) - ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, waveRuns + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - waveRuns - lengthOfWave) - - // Set gradient colors - if (lengthOfGradient > 0.0) { - for ((step, ledIdx) in ((waveRuns - lengthOfGradient.toInt())..waveRuns).withIndex()) { - ledController.setLEDs( - (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(), - (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(), - (defaultState.b + (state.b - defaultState.b) * ((step + 1) / lengthOfGradient)).toInt(), - 0, - ledIdx, - 1 - ) - } - - for ((step, ledIdx) in (waveRuns..(waveRuns + lengthOfGradient).toInt()).withIndex()) { - ledController.setLEDs( - (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), - (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), - (defaultState.b + (state.b - defaultState.b) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(), - 0, - ledIdx, - 1 - ) - } - } + ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + waveRuns + lengthOfWave, + LEDConstants.LED_COUNT - waveRuns - lengthOfWave + ) if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) { reverseLEDS = true - } - else if (waveRuns < lengthOfWave){ + } else if (waveRuns < lengthOfWave) { reverseLEDS = false } @@ -82,13 +62,38 @@ object LedIOCandle : LedIO { lengthOfSolidEnds: Int = 10 ) { ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds) - ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds) - - for ((step, ledIdx) in (lengthOfSolidEnds until(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) { + ledController.setLEDs( + otherState.r, + otherState.g, + otherState.b, + 0, + LEDConstants.LED_COUNT - lengthOfSolidEnds, + lengthOfSolidEnds + ) + + for ( + (step, ledIdx) in + (lengthOfSolidEnds until (LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex() + ) { ledController.setLEDs( - (state.r + (otherState.r - state.r) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), - (state.g + (otherState.g - state.g) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), - (state.b + (otherState.b - state.b) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(), + ( + state.r + + (otherState.r - state.r) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.g + + (otherState.g - state.g) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), + ( + state.b + + (otherState.b - state.b) * + ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0)) + ) + .toInt(), 0, ledIdx, 1 @@ -101,8 +106,17 @@ object LedIOCandle : LedIO { defaultState: LEDConstants.CandleState, percent: Double ) { - ledController.setLEDs(state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt()) - ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT) + ledController.setLEDs( + state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt() + ) + ledController.setLEDs( + defaultState.r, + defaultState.g, + defaultState.b, + 0, + (percent * LEDConstants.LED_COUNT).toInt(), + LEDConstants.LED_COUNT + ) } private fun fadeBetweenColors( @@ -114,22 +128,55 @@ object LedIOCandle : LedIO { var reachedStartColor = true var reachedEndColor = true - for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles).withIndex()) { - val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() - val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() - val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt() - - reachedStartColor = (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b) - reachedEndColor = ((calculatedR - otherState.r).absoluteValue < 10 && (calculatedG - otherState.g).absoluteValue < 10 && (calculatedB - otherState.b).absoluteValue < 10) - - ledController.setLEDs( - calculatedR, - calculatedG, - calculatedB, - 0, - ledIdx, - 1 - ) + for ( + (ledIdx, numberOfLoopCycles) in + ( + loopCyclesToConverge..loopCyclesToConverge + + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles + ) + .withIndex() + ) { + val calculatedR = + ( + state.r + + (otherState.r - state.r) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedG = + ( + state.g + + (otherState.g - state.g) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + val calculatedB = + ( + state.b + + (otherState.b - state.b) * + ( + if (loopCycles > numberOfLoopCycles) 1.0 + else (loopCycles / numberOfLoopCycles.toDouble()) + ) + ) + .toInt() + + reachedStartColor = + (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b) + reachedEndColor = + ( + (calculatedR - otherState.r).absoluteValue < 5 && + (calculatedG - otherState.g).absoluteValue < 5 && + (calculatedB - otherState.b).absoluteValue < 5 + ) + + ledController.setLEDs(calculatedR, calculatedG, calculatedB, 0, ledIdx, 1) } loopCycles += if (finishedFade) -1 else 1 @@ -150,16 +197,13 @@ object LedIOCandle : LedIO { LEDConstants.CandleState.NOTHING, (batteryVoltage.inVolts - 11.5) / (LEDConstants.BATTERY_FULL_THRESHOLD.inVolts - 11.5) ) - } - else if (state == LEDConstants.CandleState.BLUE) { + } else if (state == LEDConstants.CandleState.BLUE) { ledController.clearAnimation(0) - fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2) - } - else if (state == LEDConstants.CandleState.RED) { + fadeBetweenColors(LEDConstants.CandleState.MAGENTA, state, loopCyclesToConverge = 2) + } else if (state == LEDConstants.CandleState.RED) { ledController.clearAnimation(0) - fadeBetweenColors(state, LEDConstants.CandleState.LIGHT_RED, loopCyclesToConverge = 2) - } - else if (state.animation == null) { + fadeBetweenColors(LEDConstants.CandleState.ORANGE, state, loopCyclesToConverge = 2) + } else if (state.animation == null) { ledController.clearAnimation(0) ledController.setLEDs(state.r, state.g, state.b) } else { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt index ea14466b..8510087a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt @@ -1,13 +1,11 @@ package com.team4099.robot2023.subsystems.led -import com.team4099.robot2023.RobotContainer import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.util.FMSData import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.volts -import java.sql.Driver class Leds(val io: LedIO) { var inputs = LedIO.LedIOInputs() @@ -27,11 +25,10 @@ class Leds(val io: LedIO) { if (DriverStation.getAlliance().isEmpty) { io.batteryVoltage = RobotController.getBatteryVoltage().volts state = LEDConstants.CandleState.BATTERY_DISPLAY - } - else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { + } else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) { if (FMSData.isBlue) { state = LEDConstants.CandleState.BLUE - } else { + } else { state = LEDConstants.CandleState.RED } } else if (hasNote) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt index e7635db8..d4c5236d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt @@ -4,7 +4,6 @@ import com.team4099.lib.hal.Clock import com.team4099.robot2023.config.constants.FeederConstants import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.FlywheelConstants -import com.team4099.robot2023.config.constants.LEDConstants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.elevator.Elevator @@ -18,7 +17,6 @@ import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.util.NoteSimulation import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger @@ -35,7 +33,6 @@ import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerMinute diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index b98b9ea8..6a7d8de9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -97,8 +97,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { var robotDistanceToTarget: Length? = null var tagTargets = inputs[instance].cameraTargets - println(tagTargets.size) - val cornerData = mutableListOf() for (tag in tagTargets) { @@ -111,10 +109,10 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { PhotonUtils.calculateDistanceToTargetMeters( cameraPoses[instance].translation.z.inMeters, 57.125.inches.inMeters, - 23.25.degrees.inRadians, + 21.136.degrees.inRadians, tag.pitch.degrees.inRadians ) - .meters + .meters + 4.inches Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/robotDistanceToTarget", @@ -129,7 +127,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { ) robotTSpeaker = - Translation3d(cameraTspeaker2d.x + 4.inches, cameraTspeaker2d.y, 0.meters) + Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) val timestampedTrigVisionUpdate = TimestampedTrigVisionUpdate( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt index 814d739a..a462f4a7 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -28,8 +28,6 @@ import com.team4099.robot2023.subsystems.falconspin.MotorCollection import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.DerivativeGain @@ -148,16 +146,16 @@ object WristIOTalon : WristIO { wristConfiguration.Slot2.kD = wristSensor.derivativePositionGainToRawUnits(WristConstants.PID.SECOND_STAGE_KD) - wristConfiguration.CurrentLimits.SupplyCurrentLimit = - WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes - wristConfiguration.CurrentLimits.SupplyCurrentThreshold = - WristConstants.WRIST_THRESHOLD_CURRENT_LIMIT.inAmperes - wristConfiguration.CurrentLimits.SupplyTimeThreshold = - WristConstants.WRIST_TRIGGER_THRESHOLD_TIME.inSeconds - wristConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - wristConfiguration.CurrentLimits.StatorCurrentLimit = - WristConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes - wristConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + // wristConfiguration.CurrentLimits.SupplyCurrentLimit = + // WristConstants.WRIST_SUPPLY_CURRENT_LIMIT.inAmperes + // wristConfiguration.CurrentLimits.SupplyCurrentThreshold = + // WristConstants.WRIST_THRESHOLD_CURRENT_LIMIT.inAmperes + // wristConfiguration.CurrentLimits.SupplyTimeThreshold = + // WristConstants.WRIST_TRIGGER_THRESHOLD_TIME.inSeconds + // wristConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + // wristConfiguration.CurrentLimits.StatorCurrentLimit = + // WristConstants.WRIST_STATOR_CURRENT_LIMIT.inAmperes + // wristConfiguration.CurrentLimits.StatorCurrentLimitEnable = false wristConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake wristConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive @@ -244,7 +242,8 @@ object WristIOTalon : WristIO { var slot = if (travelingUp) 0 else 1 if (curError.absoluteValue <= secondStagePosErrSwitchThreshold.get() && - curVel.absoluteValue <= secondStageVelocitySwitchThreshold.get() + curVel.absoluteValue <= secondStageVelocitySwitchThreshold.get() && + curVel.sign < 0 ) { slot = 2 } From ea6abe3a483a88a819eea77414dc1ff34cb56c30 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 20:49:57 -0400 Subject: [PATCH 5/9] theta stuff --- .../kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt | 6 +++--- .../robot2023/config/constants/DrivetrainConstants.kt | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index 3005997f..f28d232d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -30,18 +30,18 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur Translation2d(startingPose.x + 1.5.meters, startingPose.y + 0.02.meters) .translation2d, null, - (startingPose.rotation + 15.degrees).inRotation2ds + (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds ), FieldWaypoint( Translation2d(startingPose.x + 3.meters, startingPose.y).translation2d, null, - (startingPose.rotation + 30.degrees).inRotation2ds + (startingPose.rotation + 45.degrees).inRotation2ds ), FieldWaypoint( Translation2d(startingPose.x + 1.5.meters, startingPose.y - 0.02.meters) .translation2d, null, - (startingPose.rotation + 15.degrees).inRotation2ds + (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds ), FieldWaypoint( Translation2d(startingPose.x, startingPose.y).translation2d, diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index de83610e..a10eb0db 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -144,10 +144,10 @@ object DrivetrainConstants { val TELEOP_ALIGN_PID_KD = (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond - val SIM_AUTO_THETA_PID_KP = 10.degrees.perSecond / 1.degrees + val SIM_AUTO_THETA_PID_KP = 0.degrees.perSecond / 1.degrees val SIM_AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val SIM_AUTO_THETA_PID_KD = - (1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val MAX_AUTO_ANGULAR_VEL = 270.0.degrees.perSecond val MAX_AUTO_ANGULAR_ACCEL = 600.0.degrees.perSecond.perSecond From 2c1e9e2ed4405648623f79cdc6ccb49840ffd6d9 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 21:33:19 -0400 Subject: [PATCH 6/9] dt current limits changed between auto and teleop --- .../kotlin/com/team4099/robot2023/Robot.kt | 2 ++ .../com/team4099/robot2023/RobotContainer.kt | 23 ++++++++++++++ .../config/constants/DrivetrainConstants.kt | 9 ++++-- .../drivetrain/swervemodule/SwerveModule.kt | 5 ++++ .../drivetrain/swervemodule/SwerveModuleIO.kt | 2 ++ .../swervemodule/SwerveModuleIOSim.kt | 9 ++++++ .../swervemodule/SwerveModuleIOTalon.kt | 30 +++++++++++-------- 7 files changed, 65 insertions(+), 15 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index 0066d232..cebdd0b7 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -151,6 +151,7 @@ object Robot : LoggedRobot() { } override fun autonomousInit() { + RobotContainer.setDriveCurrentLimits(isInAutonomous = true) val autonCommandWithWait = WaitCommand(0.001).andThen(autonomousCommand) autonCommandWithWait?.schedule() } @@ -216,6 +217,7 @@ object Robot : LoggedRobot() { RobotContainer.zeroSensors(isInAutonomous = false) RobotContainer.mapTeleopControls() RobotContainer.getAutonomousCommand().cancel() + RobotContainer.setDriveCurrentLimits(isInAutonomous = false) RobotContainer.setDriveBrakeMode() RobotContainer.setSteeringCoastMode() if (Constants.Tuning.TUNING_MODE) { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 9a61c508..7e130360 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -9,6 +9,7 @@ import com.team4099.robot2023.commands.drivetrain.TargetSpeakerCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim @@ -43,6 +44,7 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import org.team4099.lib.smoothDeadband +import org.team4099.lib.units.base.Current import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -361,5 +363,26 @@ object RobotContainer { fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain) + fun setDriveCurrentLimits(isInAutonomous: Boolean) { + if (isInAutonomous) { + drivetrain.swerveModules.forEach { + it.setCurrentLimits( + DrivetrainConstants.AUTO_DRIVE_STATOR_CURRENT_LIMIT, + DrivetrainConstants.AUTO_DRIVE_SUPPLY_CURRENT_LIMIT, + DrivetrainConstants.AUTO_DRIVE_THRESHOLD_CURRENT_LIMIT + ) + } + } + else { + drivetrain.swerveModules.forEach { + it.setCurrentLimits( + DrivetrainConstants.TELEOP_DRIVE_STATOR_CURRENT_LIMIT, + DrivetrainConstants.TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT, + DrivetrainConstants.TELEOP_DRIVE_THRESHOLD_CURRENT_LIMIT + ) + } + } + } + fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index a10eb0db..6dded80e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -76,11 +76,14 @@ object DrivetrainConstants { val ALLOWED_STEERING_ANGLE_ERROR = 1.degrees val STEERING_SUPPLY_CURRENT_LIMIT = 20.0.amps - val DRIVE_SUPPLY_CURRENT_LIMIT = 70.0.amps - val DRIVE_THRESHOLD_CURRENT_LIMIT = 70.0.amps + val AUTO_DRIVE_SUPPLY_CURRENT_LIMIT = 90.0.amps + val AUTO_DRIVE_THRESHOLD_CURRENT_LIMIT = 90.0.amps + val TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT = 70.0.amps + val TELEOP_DRIVE_THRESHOLD_CURRENT_LIMIT = 70.0.amps val DRIVE_TRIGGER_THRESHOLD_TIME = 0.1.seconds - val DRIVE_STATOR_CURRENT_LIMIT = 80.0.amps + val AUTO_DRIVE_STATOR_CURRENT_LIMIT = 100.amps + val TELEOP_DRIVE_STATOR_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index d60a078a..b9a79d1a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.RobotBase.isReal import org.littletonrobotics.junction.Logger import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.base.Current import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters @@ -380,4 +381,8 @@ class SwerveModule(val io: SwerveModuleIO) { fun runCharacterization(input: ElectricalPotential) { io.runCharacterization(input) } + + fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) { + io.setCurrentLimits(statorCurrentLimit, supplyCurrentLimit, thresholdCurrentLimit) + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index b7cfcbb2..56253ee4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -9,6 +9,7 @@ import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Current import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -161,4 +162,5 @@ interface SwerveModuleIO { kD: DerivativeGain ) {} fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} + fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 19ac3d21..9e11aaeb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -21,6 +21,7 @@ import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Current import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -283,6 +284,14 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { println("Can't configure motion magic in simulation") } + override fun setCurrentLimits( + statorCurrentLimit: Current, + supplyCurrentLimit: Current, + thresholdCurrentLimit: Current + ) { + println("Can't set current limits in simulation") + } + override fun runCharacterization(input: ElectricalPotential) { val appliedVolts = MathUtil.clamp(input.inVolts, -12.0, 12.0) driveMotorSim.setInputVoltage(appliedVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 320d5602..43f03a6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -28,6 +28,8 @@ import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Ampere +import org.team4099.lib.units.base.Current import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius @@ -140,16 +142,6 @@ class SwerveModuleIOTalon( driveConfiguration.Slot0.kA = DrivetrainConstants.PID.DRIVE_KA.inVoltsPerMetersPerSecondPerSecond // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast driveFalcon.configurator.apply(driveConfiguration) @@ -173,9 +165,9 @@ class SwerveModuleIOTalon( "Drive", MotorCollection( mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, + DrivetrainConstants.TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT, 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, + DrivetrainConstants.TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, 110.celsius ) ) @@ -358,6 +350,20 @@ class SwerveModuleIOTalon( steeringPositionQueue.clear() } + override fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) { + driveConfiguration.CurrentLimits.SupplyCurrentLimit = + supplyCurrentLimit.inAmperes + driveConfiguration.CurrentLimits.SupplyCurrentThreshold = + thresholdCurrentLimit.inAmperes + driveConfiguration.CurrentLimits.SupplyTimeThreshold = + DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds + driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + driveConfiguration.CurrentLimits.StatorCurrentLimit = + statorCurrentLimit.inAmperes + driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune + driveFalcon.configurator.apply(driveConfiguration) + } + override fun configureDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, From 10b1fb635faa51c91f360e887fadbc811444d3e0 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 1 Apr 2024 21:36:16 -0400 Subject: [PATCH 7/9] sptoless apply --- .../com/team4099/robot2023/RobotContainer.kt | 4 +--- .../drivetrain/swervemodule/SwerveModule.kt | 6 +++++- .../drivetrain/swervemodule/SwerveModuleIO.kt | 6 +++++- .../swervemodule/SwerveModuleIOTalon.kt | 16 ++++++++-------- .../robot2023/subsystems/vision/Vision.kt | 15 +++++++-------- 5 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 7e130360..1a3e2b7b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -44,7 +44,6 @@ import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Commands.runOnce import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import org.team4099.lib.smoothDeadband -import org.team4099.lib.units.base.Current import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -372,8 +371,7 @@ object RobotContainer { DrivetrainConstants.AUTO_DRIVE_THRESHOLD_CURRENT_LIMIT ) } - } - else { + } else { drivetrain.swerveModules.forEach { it.setCurrentLimits( DrivetrainConstants.TELEOP_DRIVE_STATOR_CURRENT_LIMIT, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index b9a79d1a..345e9741 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -382,7 +382,11 @@ class SwerveModule(val io: SwerveModuleIO) { io.runCharacterization(input) } - fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) { + fun setCurrentLimits( + statorCurrentLimit: Current, + supplyCurrentLimit: Current, + thresholdCurrentLimit: Current + ) { io.setCurrentLimits(statorCurrentLimit, supplyCurrentLimit, thresholdCurrentLimit) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index 56253ee4..54757740 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -162,5 +162,9 @@ interface SwerveModuleIO { kD: DerivativeGain ) {} fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} - fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) + fun setCurrentLimits( + statorCurrentLimit: Current, + supplyCurrentLimit: Current, + thresholdCurrentLimit: Current + ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 43f03a6f..135093f5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -28,7 +28,6 @@ import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.Ampere import org.team4099.lib.units.base.Current import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.amps @@ -350,16 +349,17 @@ class SwerveModuleIOTalon( steeringPositionQueue.clear() } - override fun setCurrentLimits(statorCurrentLimit: Current, supplyCurrentLimit: Current, thresholdCurrentLimit: Current) { - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - supplyCurrentLimit.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - thresholdCurrentLimit.inAmperes + override fun setCurrentLimits( + statorCurrentLimit: Current, + supplyCurrentLimit: Current, + thresholdCurrentLimit: Current + ) { + driveConfiguration.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit.inAmperes + driveConfiguration.CurrentLimits.SupplyCurrentThreshold = thresholdCurrentLimit.inAmperes driveConfiguration.CurrentLimits.SupplyTimeThreshold = DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - statorCurrentLimit.inAmperes + driveConfiguration.CurrentLimits.StatorCurrentLimit = statorCurrentLimit.inAmperes driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune driveFalcon.configurator.apply(driveConfiguration) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 6a7d8de9..3e764d3b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -107,12 +107,12 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { robotDistanceToTarget = PhotonUtils.calculateDistanceToTargetMeters( - cameraPoses[instance].translation.z.inMeters, - 57.125.inches.inMeters, - 21.136.degrees.inRadians, - tag.pitch.degrees.inRadians - ) - .meters + 4.inches + cameraPoses[instance].translation.z.inMeters, + 57.125.inches.inMeters, + 21.136.degrees.inRadians, + tag.pitch.degrees.inRadians + ) + .meters + 4.inches Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/robotDistanceToTarget", @@ -126,8 +126,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { ) ) - robotTSpeaker = - Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) + robotTSpeaker = Translation3d(cameraTspeaker2d.x, cameraTspeaker2d.y, 0.meters) val timestampedTrigVisionUpdate = TimestampedTrigVisionUpdate( From 8ba9615719499a1d1bf620974e6e03f3fef68fc8 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 2 Apr 2024 09:21:47 -0400 Subject: [PATCH 8/9] failed auto tuning --- .../com/team4099/robot2023/config/constants/Constants.kt | 2 +- .../robot2023/config/constants/DrivetrainConstants.kt | 4 ++-- 2 files changed, 3 insertions(+), 3 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 7151475d..41089e90 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -54,7 +54,7 @@ object Constants { object Tuning { - const val TUNING_MODE = true + const val TUNING_MODE = false const val DEBUGING_MODE = false const val SIMULATE_DRIFT = false const val DRIFT_CONSTANT = 0.001 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 6dded80e..66bdac51 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -137,10 +137,10 @@ object DrivetrainConstants { (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = (0.radians.perSecond / 1.radians) + val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians) val AUTO_THETA_PID_KI = (0.0.radians.perSecond / (1.radians * 1.seconds)) val AUTO_THETA_PID_KD = - (0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond + (0.2.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond val TELEOP_ALIGN_PID_KP = 3.6.degrees.perSecond / 1.degrees val TELEOP_ALIGN_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) From 9e3230bbcbc0e24beae270d9fdb6e78aa47cca01 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 2 Apr 2024 09:57:21 -0400 Subject: [PATCH 9/9] remove current limits --- .../kotlin/com/team4099/robot2023/Robot.kt | 2 -- .../com/team4099/robot2023/RobotContainer.kt | 20 ---------------- .../robot2023/auto/mode/TestAutoPath.kt | 12 +++++----- .../drivetrain/swervemodule/SwerveModule.kt | 8 ------- .../drivetrain/swervemodule/SwerveModuleIO.kt | 5 ---- .../swervemodule/SwerveModuleIOSim.kt | 8 ------- .../swervemodule/SwerveModuleIOTalon.kt | 24 +++++++------------ 7 files changed, 15 insertions(+), 64 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/Robot.kt b/src/main/kotlin/com/team4099/robot2023/Robot.kt index cebdd0b7..0066d232 100644 --- a/src/main/kotlin/com/team4099/robot2023/Robot.kt +++ b/src/main/kotlin/com/team4099/robot2023/Robot.kt @@ -151,7 +151,6 @@ object Robot : LoggedRobot() { } override fun autonomousInit() { - RobotContainer.setDriveCurrentLimits(isInAutonomous = true) val autonCommandWithWait = WaitCommand(0.001).andThen(autonomousCommand) autonCommandWithWait?.schedule() } @@ -217,7 +216,6 @@ object Robot : LoggedRobot() { RobotContainer.zeroSensors(isInAutonomous = false) RobotContainer.mapTeleopControls() RobotContainer.getAutonomousCommand().cancel() - RobotContainer.setDriveCurrentLimits(isInAutonomous = false) RobotContainer.setDriveBrakeMode() RobotContainer.setSteeringCoastMode() if (Constants.Tuning.TUNING_MODE) { diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 1a3e2b7b..20072913 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -362,25 +362,5 @@ object RobotContainer { fun getAutonomousLoadingCommand() = AutonomousSelector.getLoadingCommand(drivetrain) - fun setDriveCurrentLimits(isInAutonomous: Boolean) { - if (isInAutonomous) { - drivetrain.swerveModules.forEach { - it.setCurrentLimits( - DrivetrainConstants.AUTO_DRIVE_STATOR_CURRENT_LIMIT, - DrivetrainConstants.AUTO_DRIVE_SUPPLY_CURRENT_LIMIT, - DrivetrainConstants.AUTO_DRIVE_THRESHOLD_CURRENT_LIMIT - ) - } - } else { - drivetrain.swerveModules.forEach { - it.setCurrentLimits( - DrivetrainConstants.TELEOP_DRIVE_STATOR_CURRENT_LIMIT, - DrivetrainConstants.TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT, - DrivetrainConstants.TELEOP_DRIVE_THRESHOLD_CURRENT_LIMIT - ) - } - } - } - fun mapTunableCommands() {} } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt index f28d232d..546c806d 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt @@ -27,21 +27,21 @@ class TestAutoPath(val drivetrain: Drivetrain, val superstructure: Superstructur startingPose.rotation.inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 1.5.meters, startingPose.y + 0.02.meters) + Translation2d(startingPose.x + 2.meters, startingPose.y + 0.02.meters) .translation2d, null, - (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds + (startingPose.rotation + (45 / 4.0).degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 3.meters, startingPose.y).translation2d, + Translation2d(startingPose.x + 4.meters, startingPose.y).translation2d, null, - (startingPose.rotation + 45.degrees).inRotation2ds + (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds ), FieldWaypoint( - Translation2d(startingPose.x + 1.5.meters, startingPose.y - 0.02.meters) + Translation2d(startingPose.x + 2.meters, startingPose.y - 0.02.meters) .translation2d, null, - (startingPose.rotation + (45 / 2.0).degrees).inRotation2ds + (startingPose.rotation + (45 / 4.0).degrees).inRotation2ds ), FieldWaypoint( Translation2d(startingPose.x, startingPose.y).translation2d, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index 345e9741..c5d44b2c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -381,12 +381,4 @@ class SwerveModule(val io: SwerveModuleIO) { fun runCharacterization(input: ElectricalPotential) { io.runCharacterization(input) } - - fun setCurrentLimits( - statorCurrentLimit: Current, - supplyCurrentLimit: Current, - thresholdCurrentLimit: Current - ) { - io.setCurrentLimits(statorCurrentLimit, supplyCurrentLimit, thresholdCurrentLimit) - } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index 54757740..ecc7e329 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -162,9 +162,4 @@ interface SwerveModuleIO { kD: DerivativeGain ) {} fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} - fun setCurrentLimits( - statorCurrentLimit: Current, - supplyCurrentLimit: Current, - thresholdCurrentLimit: Current - ) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 9e11aaeb..18134abb 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -284,14 +284,6 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { println("Can't configure motion magic in simulation") } - override fun setCurrentLimits( - statorCurrentLimit: Current, - supplyCurrentLimit: Current, - thresholdCurrentLimit: Current - ) { - println("Can't set current limits in simulation") - } - override fun runCharacterization(input: ElectricalPotential) { val appliedVolts = MathUtil.clamp(input.inVolts, -12.0, 12.0) driveMotorSim.setInputVoltage(appliedVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 135093f5..c90f0c0e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -142,6 +142,15 @@ class SwerveModuleIOTalon( DrivetrainConstants.PID.DRIVE_KA.inVoltsPerMetersPerSecondPerSecond // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) + driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.TELEOP_DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.SupplyCurrentThreshold = DrivetrainConstants.TELEOP_DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.SupplyTimeThreshold = + DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds + driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + driveConfiguration.CurrentLimits.StatorCurrentLimit = DrivetrainConstants.TELEOP_DRIVE_STATOR_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune + driveFalcon.configurator.apply(driveConfiguration) + driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast driveFalcon.configurator.apply(driveConfiguration) @@ -349,21 +358,6 @@ class SwerveModuleIOTalon( steeringPositionQueue.clear() } - override fun setCurrentLimits( - statorCurrentLimit: Current, - supplyCurrentLimit: Current, - thresholdCurrentLimit: Current - ) { - driveConfiguration.CurrentLimits.SupplyCurrentLimit = supplyCurrentLimit.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = thresholdCurrentLimit.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = statorCurrentLimit.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = true // TODO tune - driveFalcon.configurator.apply(driveConfiguration) - } - override fun configureDrivePID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>,