From a6eeeaa8d29444e5b6b7db13176b13d3219a4801 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 16:48:57 +0200 Subject: [PATCH 001/253] Rebase from dev --- src/main/kotlin/frc/robot/BuildConstants.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 src/main/kotlin/frc/robot/BuildConstants.java diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java new file mode 100644 index 000000000..f1081e045 --- /dev/null +++ b/src/main/kotlin/frc/robot/BuildConstants.java @@ -0,0 +1,19 @@ +package frc.robot; + +/** + * Automatically generated file containing build version information. + */ +public final class BuildConstants { + public static final String MAVEN_GROUP = ""; + public static final String MAVEN_NAME = "Alt-Swerve"; + public static final String VERSION = "unspecified"; + public static final int GIT_REVISION = 114; + public static final String GIT_SHA = "d47594e8d8d8aa278bb659bda8a08f688a3f5ce5"; + public static final String GIT_DATE = "2025-01-15 01:04 IST"; + public static final String GIT_BRANCH = "feature/climber/subsystem-io"; + public static final String BUILD_DATE = "2025-01-15 16:48 IST"; + public static final long BUILD_UNIX_TIME = 1736952481060L; + public static final int DIRTY = 1; + + private BuildConstants(){} +} From e6b4d2063ea5fa86a8447e3f3ba769a8a0de1c8d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 16:49:07 +0200 Subject: [PATCH 002/253] Create Climber io --- .../frc/robot/subsystems/climber/ClimberIO.kt | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt new file mode 100644 index 000000000..f4e55c4a5 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -0,0 +1,21 @@ +package frc.robot.subsystems.climber + +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Voltage +import org.team9432.annotation.Logged + +interface ClimberIO { + var inputs:LoggedInputClimber + fun setLatchPosition() {} + fun setPower() {} + fun setAngle() {} + + @Logged + open class InputClimber { + var appliedVoltage: Voltage = Units.Volts.zero() + var angle: Angle = Units.Degree.zero() + var latchPosition: Distance = Units.Centimeter.zero() + } +} From f3208588f969c66781c5d02922a55bc9f89f8d21 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 17:16:17 +0200 Subject: [PATCH 003/253] Create climb constants --- .../robot/subsystems/climber/ClimberConstants.kt | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt new file mode 100644 index 000000000..cfe11e906 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -0,0 +1,13 @@ +package frc.robot.subsystems.climber + +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance + +const val UNFOLD_POWER = 1 +const val FOLD_POWER = 1 +val OPEN_LATCH_POSITION:Distance = Units.Centimeter.of(0.8) +val CLOSE_LATCH_POSITION:Distance = Units.Centimeter.of(0.2) +val UNFOLDED_ANGLE:Angle = Units.Degree.of(60.0) +val FOLDED_ANGLE:Angle = Units.Degree.of(30.0) +const val gearRation = 1.0 \ No newline at end of file From ef43de3b32b75355f88b5ffba17c20d8ae54c598 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 16:56:08 +0200 Subject: [PATCH 004/253] Create Climber io sim --- .../frc/robot/subsystems/climber/ClimberIOSim.kt | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt new file mode 100644 index 000000000..d58a8f924 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -0,0 +1,16 @@ +package frc.robot.subsystems.climber + +import frc.robot.lib.motors.TalonFXSim + +class ClimberIOSim:ClimberIO { + override var inputs: LoggedInputClimber = LoggedInputClimber() + + override fun setLatchPosition() { + } + + override fun setPower() { + } + + override fun setAngle() { + } +} \ No newline at end of file From 051de2c422203acbc89936654c68f233f13c3a78 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 17:55:50 +0200 Subject: [PATCH 005/253] Add moment of inertia to constants --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index cfe11e906..9cd0599c5 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -3,6 +3,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 @@ -10,4 +11,5 @@ val OPEN_LATCH_POSITION:Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION:Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE:Angle = Units.Degree.of(60.0) val FOLDED_ANGLE:Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 \ No newline at end of file +const val gearRation = 1.0 +val momentOfInertia:MomentOfInertia = Units.KilogramSquareMeters.of(0.0) \ No newline at end of file From 70a5b120b7260457e134ca1aa8cca964b349367e Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 17:56:44 +0200 Subject: [PATCH 006/253] Add vars motor dutyCycle and positionControler --- .../frc/robot/subsystems/climber/ClimberIOSim.kt | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index d58a8f924..3b986175a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -1,16 +1,26 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionVoltage +import edu.wpi.first.units.Units import frc.robot.lib.motors.TalonFXSim +import frc.robot.lib.motors.TalonType -class ClimberIOSim:ClimberIO { +class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() + var motor = TalonFXSim(2, gearRation, momentOfInertia.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + var dutyCycle = DutyCycleOut(0.0) + var positionControler =PositionVoltage(0.0) override fun setLatchPosition() { + } - override fun setPower() { + override fun setPower(power:Double) { + motor.setControl(dutyCycle.withOutput(power)) } override fun setAngle() { + } } \ No newline at end of file From 8a047055ff7162ef2b3f9f2bcd3dc3af17803386 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 17:58:34 +0200 Subject: [PATCH 007/253] Add vars to pass in those functions --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index f4e55c4a5..4bdffa72d 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -8,9 +8,9 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs:LoggedInputClimber - fun setLatchPosition() {} - fun setPower() {} - fun setAngle() {} + fun setLatchPosition(position:Distance) {} + fun setPower(power:Double) {} + fun setAngle(angle: Angle) {} @Logged open class InputClimber { From 8b0138186c778d468a88c9bd1a9d09f87cc00680 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 18:00:08 +0200 Subject: [PATCH 008/253] rebased the wrong branch --- .../robot/subsystems/climber/ClimberIOSim.kt | 26 ------------------- 1 file changed, 26 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt deleted file mode 100644 index 3b986175a..000000000 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.subsystems.climber - -import com.ctre.phoenix6.controls.DutyCycleOut -import com.ctre.phoenix6.controls.PositionVoltage -import edu.wpi.first.units.Units -import frc.robot.lib.motors.TalonFXSim -import frc.robot.lib.motors.TalonType - -class ClimberIOSim : ClimberIO { - override var inputs: LoggedInputClimber = LoggedInputClimber() - var motor = TalonFXSim(2, gearRation, momentOfInertia.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) - var dutyCycle = DutyCycleOut(0.0) - var positionControler =PositionVoltage(0.0) - - override fun setLatchPosition() { - - } - - override fun setPower(power:Double) { - motor.setControl(dutyCycle.withOutput(power)) - } - - override fun setAngle() { - - } -} \ No newline at end of file From 45fde1b3637694481fbb3aa6fb1eb6aa8128844b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 18:08:43 +0200 Subject: [PATCH 009/253] Forgot to add updateInput --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 4bdffa72d..00b4b1fff 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -11,6 +11,7 @@ interface ClimberIO { fun setLatchPosition(position:Distance) {} fun setPower(power:Double) {} fun setAngle(angle: Angle) {} + fun updateInput(){} @Logged open class InputClimber { From ec3e27c5a57785ac58fee230526701336ff810a5 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:08:28 +0200 Subject: [PATCH 010/253] Create climber --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Climber.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt new file mode 100644 index 000000000..63b4475df --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -0,0 +1,6 @@ +package frc.robot.subsystems.climber + +import edu.wpi.first.wpilibj2.command.SubsystemBase + +class Climber private constructor(private val ClimberIO):SubsystemBase{ +} \ No newline at end of file From b1a5da6f02a1eaf9e28a1885718cf1c12d9dffbf Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:09:11 +0200 Subject: [PATCH 011/253] Add initialize ,closeLatch , openLatch --- .../frc/robot/subsystems/climber/Climber.kt | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 63b4475df..672b39d07 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,6 +1,31 @@ package frc.robot.subsystems.climber +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase -class Climber private constructor(private val ClimberIO):SubsystemBase{ +class Climber private constructor(private val io:ClimberIO):SubsystemBase(){ + var inputs = io.inputs + + companion object { + @Volatile + private var instance: Climber? = null + + fun initialize(io: ClimberIO) { + synchronized(this) { + if (instance == null) { + instance = Climber(io) + } + } + } + + fun getInstance(): Climber { + return instance ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) + } + } + fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) + fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) + } \ No newline at end of file From d41f0e50fba6afe2a13cc983eeedc847954e5641 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:15:16 +0200 Subject: [PATCH 012/253] Add lock and unlock for climber io --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 00b4b1fff..488a66d42 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -11,6 +11,8 @@ interface ClimberIO { fun setLatchPosition(position:Distance) {} fun setPower(power:Double) {} fun setAngle(angle: Angle) {} + fun lock(){} + fun unlock(){} fun updateInput(){} @Logged From 6f8d3992a02986962520daa70b386d29e72f165e Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:03:13 +0200 Subject: [PATCH 013/253] Add tolerance to constants --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 9cd0599c5..f6f16c911 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -12,4 +12,6 @@ val CLOSE_LATCH_POSITION:Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE:Angle = Units.Degree.of(60.0) val FOLDED_ANGLE:Angle = Units.Degree.of(30.0) const val gearRation = 1.0 -val momentOfInertia:MomentOfInertia = Units.KilogramSquareMeters.of(0.0) \ No newline at end of file +val momentOfInertia:MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var distanceThreshold: Distance = Units.Centimeter.of(0.4) +var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From 1a104398e5ddfed8ff198c98c1dd2e40848897ff Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:03:57 +0200 Subject: [PATCH 014/253] Add sensorDistance to logged inputs --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 488a66d42..c13b6d3b4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -20,5 +20,6 @@ interface ClimberIO { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() var latchPosition: Distance = Units.Centimeter.zero() + var sensorDistance:Distance = Units.Centimeter.zero() } } From 7d38c7f632b091a7827f5cca4a63d6f74ef8026b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:04:03 +0200 Subject: [PATCH 015/253] Add triggers --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 672b39d07..1d1eecb97 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -3,9 +3,16 @@ package frc.robot.subsystems.climber import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase +import edu.wpi.first.wpilibj2.command.button.Trigger +import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io:ClimberIO):SubsystemBase(){ var inputs = io.inputs + @AutoLogOutput + private var isTouching = Trigger{inputs.sensorDistance.lt(distanceThreshold)} + private val hasClimbed = Trigger{inputs.angle.lt(FOLDED_ANGLE)} + private var isLatchClosed = Trigger {inputs.latchPosition.lt(latchTolerance)} + private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { @Volatile From c3aa6511e3f31abf7b9597a4ba517346e251413a Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:19:36 +0200 Subject: [PATCH 016/253] Add ports class --- src/main/kotlin/frc/robot/subsystems/Port.kt | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt new file mode 100644 index 000000000..794799f6b --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -0,0 +1,9 @@ +package frc.robot.subsystems + +object Port { + object Climber { + val mainClimberMotor = 0 + val auxClimberMotor = 1 + val ClimberSensor = 2 + } +} \ No newline at end of file From b2524ceb738e9f56b1fc179c78d1562e2d10ea9d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:22:06 +0200 Subject: [PATCH 017/253] Reformat code --- .../kotlin/frc/robot/subsystems/AUTO-SPEC.md | 43 +++++-- .../frc/robot/subsystems/climber/Climber.kt | 14 ++- .../subsystems/climber/ClimberConstants.kt | 10 +- .../frc/robot/subsystems/climber/ClimberIO.kt | 14 +-- .../frc/robot/subsystems/drive/Drive.java | 107 ++++++++++++------ .../robot/subsystems/drive/DriveCommands.java | 14 ++- .../frc/robot/subsystems/drive/GyroIO.java | 10 +- .../robot/subsystems/drive/GyroIONavX.java | 5 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 52 ++++++--- .../frc/robot/subsystems/drive/ModuleIO.java | 41 ++++--- .../robot/subsystems/drive/ModuleIOSim.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 3 +- .../drive/PhoenixOdometryThread.java | 13 ++- .../subsystems/drive/TunerConstants.java | 72 ++++++------ .../frc/robot/subsystems/vision/Vision.java | 2 + .../subsystems/vision/VisionConstants.java | 9 +- .../frc/robot/subsystems/vision/VisionIO.java | 17 ++- .../subsystems/vision/VisionIOLimelight.java | 17 ++- .../vision/VisionIOPhotonVision.java | 8 +- .../vision/VisionIOPhotonVisionSim.java | 8 +- 21 files changed, 308 insertions(+), 162 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/AUTO-SPEC.md b/src/main/kotlin/frc/robot/subsystems/AUTO-SPEC.md index 6893b629a..1a0877461 100644 --- a/src/main/kotlin/frc/robot/subsystems/AUTO-SPEC.md +++ b/src/main/kotlin/frc/robot/subsystems/AUTO-SPEC.md @@ -1,38 +1,61 @@ # Autonomous Specifications + ## Linked waypoints + - `Lower position`: Lower starting position - `Intake line start` - `Intake line end` + ## Paths + - `Intaking zone to high position` - - Drive from the intaking zone, pick up a cone, wait for a second, and drive close to the second alliance's loading zone. - ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/6c310a67-afb8-44f6-a652-35aaf70c5238) - - Special constraints (if relevant) - - Near loading zone: decrease speed so the red dragon won't notice us. - ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/82f75dd9-9c13-4dbc-913d-741f211f3972) + - Drive from the intaking zone, pick up a cone, wait for a second, and drive close to the second alliance's loading + zone. + ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/6c310a67-afb8-44f6-a652-35aaf70c5238) + - Special constraints (if relevant) + - Near loading zone: decrease speed so the red dragon won't notice us. + ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/82f75dd9-9c13-4dbc-913d-741f211f3972) - Another one... + ## Autos + - `Intaking zone to high position while shooting` - - Uses `Intaking zone to high position` path - - Shoots after passing the second waypoint, near the Charge Station - ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/5e5d017e-a853-4621-8156-03d409aa8542) + - Uses `Intaking zone to high position` path + - Shoots after passing the second waypoint, near the Charge Station + ![image](https://github.com/Galaxia5987/Robot-template/assets/31829093/5e5d017e-a853-4621-8156-03d409aa8542) - Another one... # Choosers -> Could be as simple as "Using PathPlanner's built-in chooser" (BTW use [this](https://www.chiefdelphi.com/t/pathplanner-2024-beta/442364/149?u=dan)) or more complex based on the game, like what's been done [here](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2023-build-thread/420691/179#autos-the-questionnaire-2): + +> Could be as simple as "Using PathPlanner's built-in chooser" (BTW +> use [this](https://www.chiefdelphi.com/t/pathplanner-2024-beta/442364/149?u=dan)) or more complex based on the game, +> like what's been +> +done [here](https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2023-build-thread/420691/179#autos-the-questionnaire-2): + ## Side chooser + Choose which side the robot's on: + - Left - Center - Right + ## Game piece chooser + Choose which game piece type the robot is preloaded with: + - Cube - Cone + ## State machine + > Mermaid state graph which shows which auto to run based on the chooser goes here. # Dashboard -> Include here fields that will be sent over NetworkTables and shown to the drivers during the autonomous period of the match. + +> Include here fields that will be sent over NetworkTables and shown to the drivers during the autonomous period of the +> match. + - Robot location: `Robot/Odometry` - Time left until teleop starts: `Robot/TimeLeftUntilTeleop` diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 1d1eecb97..282aef646 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -6,12 +6,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import org.littletonrobotics.junction.AutoLogOutput -class Climber private constructor(private val io:ClimberIO):SubsystemBase(){ +class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs + @AutoLogOutput - private var isTouching = Trigger{inputs.sensorDistance.lt(distanceThreshold)} - private val hasClimbed = Trigger{inputs.angle.lt(FOLDED_ANGLE)} - private var isLatchClosed = Trigger {inputs.latchPosition.lt(latchTolerance)} + private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { @@ -32,7 +33,8 @@ class Climber private constructor(private val io:ClimberIO):SubsystemBase(){ ) } } - fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) - fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) + + fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index f6f16c911..c9cb76fb9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -7,11 +7,11 @@ import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION:Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION:Distance = Units.Centimeter.of(0.2) -val UNFOLDED_ANGLE:Angle = Units.Degree.of(60.0) -val FOLDED_ANGLE:Angle = Units.Degree.of(30.0) +val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) +val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) +val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val gearRation = 1.0 -val momentOfInertia:MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var distanceThreshold: Distance = Units.Centimeter.of(0.4) var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index c13b6d3b4..0917e96fa 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -7,19 +7,19 @@ import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged interface ClimberIO { - var inputs:LoggedInputClimber - fun setLatchPosition(position:Distance) {} - fun setPower(power:Double) {} + var inputs: LoggedInputClimber + fun setLatchPosition(position: Distance) {} + fun setPower(power: Double) {} fun setAngle(angle: Angle) {} - fun lock(){} - fun unlock(){} - fun updateInput(){} + fun lock() {} + fun unlock() {} + fun updateInput() {} @Logged open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() var latchPosition: Distance = Units.Centimeter.zero() - var sensorDistance:Distance = Units.Centimeter.zero() + var sensorDistance: Distance = Units.Centimeter.zero() } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index a0e84f1b9..8fea22e17 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java @@ -52,9 +52,11 @@ import frc.robot.ConstantsKt; import frc.robot.Mode; import frc.robot.lib.LocalADStarAK; + import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -106,16 +108,17 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - @AutoLogOutput private Rotation2d desiredHeading; + @AutoLogOutput + private Rotation2d desiredHeading; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition[]{ + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( @@ -218,8 +221,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); } // Update odometry @@ -285,21 +288,27 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** Runs the drive in a straight line with the specified drive output. */ + /** + * Runs the drive in a straight line with the specified drive output. + */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runCharacterization(output); } } - /** NOTE: DO NOT USE WITH TorqueCurrentFOC */ + /** + * NOTE: DO NOT USE WITH TorqueCurrentFOC + */ public void runTurnCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runTurnCharacterization(output); } } - /** Stops the drive. */ + /** + * Stops the drive. + */ public void stop() { runVelocity(new ChassisSpeeds()); } @@ -317,14 +326,18 @@ public void stopWithX() { stop(); } - /** Returns a command to run a quasistatic test in the specified direction. */ + /** + * Returns a command to run a quasistatic test in the specified direction. + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) .andThen(sysId.quasistatic(direction)); } - /** Returns a command to run a dynamic test in the specified direction. */ + /** + * Returns a command to run a dynamic test in the specified direction. + */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) @@ -348,14 +361,18 @@ public Command runAllTurnSysID() { turnSysIdDynamic(SysIdRoutine.Direction.kReverse)); } - /** Returns a command to run a dynamic test in the specified direction on the turn modules. */ + /** + * Returns a command to run a dynamic test in the specified direction on the turn modules. + */ public Command turnSysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runTurnCharacterization(0.0)) .withTimeout(1.0) .andThen(turnSysId.dynamic(direction)); } - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + /** + * Returns the module states (turn angles and drive velocities) for all of the modules. + */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; @@ -365,7 +382,9 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + /** + * Returns the module positions (turn angles and drive positions) for all of the modules. + */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -374,13 +393,17 @@ private SwerveModulePosition[] getModulePositions() { return states; } - /** Returns the measured chassis speeds of the robot. */ + /** + * Returns the measured chassis speeds of the robot. + */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } - /** Returns the position of each module in radians. */ + /** + * Returns the position of each module in radians. + */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; for (int i = 0; i < 4; i++) { @@ -389,7 +412,9 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ + /** + * Returns the average velocity of the modules in rotations/sec (Phoenix native units). + */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -398,13 +423,17 @@ public double getFFCharacterizationVelocity() { return output; } - /** Returns the current odometry pose. */ + /** + * Returns the current odometry pose. + */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } - /** Returns the current gyro rotation. */ + /** + * Returns the current gyro rotation. + */ public Rotation2d getRotation() { return gyroInputs.yawPosition; } @@ -428,7 +457,9 @@ public Command setDesiredHeading(Rotation2d rotation) { return Commands.runOnce(() -> desiredHeading = rotation); } - /** Resets the current odometry pose. */ + /** + * Resets the current odometry pose. + */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -438,7 +469,9 @@ public void resetGyro() { desiredHeading = new Rotation2d(); } - /** Adds a new timestamped vision measurement. */ + /** + * Adds a new timestamped vision measurement. + */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -447,26 +480,32 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** Returns the maximum linear speed in meters per sec. */ + /** + * Returns the maximum linear speed in meters per sec. + */ public double getMaxLinearSpeedMetersPerSec() { return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } - /** Returns the maximum angular speed in radians per sec. */ + /** + * Returns the maximum angular speed in radians per sec. + */ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } - /** Returns an array of module translations. */ + /** + * Returns an array of module translations. + */ public static Translation2d[] getModuleTranslations() { - return new Translation2d[] { - new Translation2d( - TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), - new Translation2d( - TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), - new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - new Translation2d( - TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) + return new Translation2d[]{ + new Translation2d( + TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d( + TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d( + TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java index 3d59b9b4a..65ef7f8aa 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; + import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -46,7 +47,8 @@ public class DriveCommands { private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 private static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.4); - private DriveCommands() {} + private DriveCommands() { + } private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { // Apply deadband @@ -148,7 +150,7 @@ public static Command joystickDriveAtAngle( speeds, isFlipped ? drive.getRotation() - .plus(new Rotation2d(Math.PI)) + .plus(new Rotation2d(Math.PI)) : drive.getRotation())); }, drive) @@ -224,7 +226,9 @@ public static Command feedforwardCharacterization(Drive drive) { })); } - /** Measures the robot's wheel radius by spinning in a circle. */ + /** + * Measures the robot's wheel radius by spinning in a circle. + */ public static Command wheelRadiusCharacterization(Drive drive) { SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); @@ -302,8 +306,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { + formatter.format(wheelRadius) + " meters, " + formatter.format( - Units.metersToInches( - wheelRadius)) + Units.metersToInches( + wheelRadius)) + " inches"); }))); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java index 1ea03e137..a87dce8e4 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java @@ -22,11 +22,13 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public double yawVelocityRadPerSec = 0.0; - public double[] odometryYawTimestamps = new double[] {}; - public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; + public double[] odometryYawTimestamps = new double[]{}; + public Rotation2d[] odometryYawPositions = new Rotation2d[]{}; } - public default void zeroGyro() {} + public default void zeroGyro() { + } - public default void updateInputs(GyroIOInputs inputs) {} + public default void updateInputs(GyroIOInputs inputs) { + } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java index 3e2c5e9ae..3caa3b63f 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java @@ -17,9 +17,12 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; + import java.util.Queue; -/** IO implementation for NavX. */ +/** + * IO implementation for NavX. + */ public class GyroIONavX implements GyroIO { private final AHRS navX = new AHRS(NavXComType.kUSB1, (byte) Drive.ODOMETRY_FREQUENCY); private final Queue yawPositionQueue; diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java index f56f370e1..40d5007c8 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -22,9 +22,12 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; + import java.util.Queue; -/** IO implementation for Pigeon 2. */ +/** + * IO implementation for Pigeon 2. + */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2( diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Module.java b/src/main/kotlin/frc/robot/subsystems/drive/Module.java index 06f91457e..538ff2992 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Module.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Module.java @@ -29,13 +29,13 @@ public class Module { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{}; public Module( ModuleIO io, @@ -78,7 +78,9 @@ public void periodic() { turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); } - /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ + /** + * Runs the module with the specified setpoint state. Mutates the state to optimize it. + */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint state.optimize(getAngle()); @@ -89,7 +91,9 @@ public void runSetpoint(SwerveModuleState state) { io.setTurnPosition(state.angle); } - /** Runs the module with the specified output while controlling to zero degrees. */ + /** + * Runs the module with the specified output while controlling to zero degrees. + */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); io.setTurnPosition(new Rotation2d()); @@ -100,53 +104,73 @@ public void runTurnCharacterization(double output) { io.setTurnOpenLoop(output); } - /** Disables all outputs to motors. */ + /** + * Disables all outputs to motors. + */ public void stop() { io.setDriveOpenLoop(0.0); io.setTurnOpenLoop(0.0); } - /** Returns the current turn angle of the module. */ + /** + * Returns the current turn angle of the module. + */ public Rotation2d getAngle() { return inputs.turnPosition; } - /** Returns the current drive position of the module in meters. */ + /** + * Returns the current drive position of the module in meters. + */ public double getPositionMeters() { return inputs.drivePositionRad * constants.WheelRadius; } - /** Returns the current drive velocity of the module in meters per second. */ + /** + * Returns the current drive velocity of the module in meters per second. + */ public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * constants.WheelRadius; } - /** Returns the module position (turn angle and drive position). */ + /** + * Returns the module position (turn angle and drive position). + */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** Returns the module state (turn angle and drive velocity). */ + /** + * Returns the module state (turn angle and drive velocity). + */ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** Returns the module positions received this cycle. */ + /** + * Returns the module positions received this cycle. + */ public SwerveModulePosition[] getOdometryPositions() { return odometryPositions; } - /** Returns the timestamps of the samples received this cycle. */ + /** + * Returns the timestamps of the samples received this cycle. + */ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** Returns the module position in radians. */ + /** + * Returns the module position in radians. + */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; } - /** Returns the module velocity in rotations/sec (Phoenix native units). */ + /** + * Returns the module velocity in rotations/sec (Phoenix native units). + */ public double getFFCharacterizationVelocity() { return Units.radiansToRotations(inputs.driveVelocityRadPerSec); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java index 8a202cd27..649922d28 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java @@ -34,23 +34,38 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double[] odometryTimestamps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + public double[] odometryTimestamps = new double[]{}; + public double[] odometryDrivePositionsRad = new double[]{}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; } - /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + /** + * Updates the set of loggable inputs. + */ + public default void updateInputs(ModuleIOInputs inputs) { + } - /** Run the drive motor at the specified open loop value. */ - public default void setDriveOpenLoop(double output) {} + /** + * Run the drive motor at the specified open loop value. + */ + public default void setDriveOpenLoop(double output) { + } - /** Run the turn motor at the specified open loop value. */ - public default void setTurnOpenLoop(double output) {} + /** + * Run the turn motor at the specified open loop value. + */ + public default void setTurnOpenLoop(double output) { + } - /** Run the drive motor at the specified velocity. */ - public default void setDriveVelocity(double velocityRadPerSec) {} + /** + * Run the drive motor at the specified velocity. + */ + public default void setDriveVelocity(double velocityRadPerSec) { + } - /** Run the turn motor to the specified rotation. */ - public default void setTurnPosition(Rotation2d rotation) {} + /** + * Run the turn motor to the specified rotation. + */ + public default void setTurnPosition(Rotation2d rotation) { + } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java index 1e478376b..a3e2f5f60 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java @@ -116,9 +116,9 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; + inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[]{inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[]{inputs.turnPosition}; } @Override diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java index c904649fc..5c56a83ab 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -37,6 +37,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; + import java.util.Queue; /** @@ -47,7 +48,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; // Hardware objects diff --git a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 8958af762..00a2f7069 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; + import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -66,7 +67,9 @@ public void start() { } } - /** Registers a Phoenix signal to be read from the thread. */ + /** + * Registers a Phoenix signal to be read from the thread. + */ public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -84,7 +87,9 @@ public Queue registerSignal(StatusSignal signal) { return queue; } - /** Registers a generic signal to be read from the thread. */ + /** + * Registers a generic signal to be read from the thread. + */ public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -99,7 +104,9 @@ public Queue registerSignal(DoubleSupplier signal) { return queue; } - /** Returns a new queue that returns timestamp values for each sample. */ + /** + * Returns a new queue that returns timestamp values for each sample. + */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); diff --git a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java index 00988ee62..efd67b134 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java @@ -37,16 +37,16 @@ public class TunerConstants { public static SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants(); public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight; public static void init() { @@ -126,8 +126,8 @@ public static void init() { double[] offsets; if (ConstantsKt.getROBORIO_SERIAL_NUMBER().equals(ALT_ROBORIO_SERIAL)) { offsets = - new double[] { - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 + new double[]{ + 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 }; steerGains = @@ -233,11 +233,11 @@ public static void init() { kBackRightYPos = Meters.of(-0.24); } else { offsets = - new double[] { - -2.9329712664373457, - -1.4818254410975293, - 0.11351457830353745, - 2.0586022173425302 + new double[]{ + -2.9329712664373457, + -1.4818254410975293, + 0.11351457830353745, + 2.0586022173425302 }; steerGains = @@ -350,32 +350,32 @@ public static void init() { .withPigeon2Configs(pigeonConfigs); SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, - TalonFXConfiguration, - CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); FrontLeft = ConstantCreator.createModuleConstants( diff --git a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java index bb33178bb..df73830c6 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java @@ -26,8 +26,10 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; + import java.util.LinkedList; import java.util.List; + import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java index 4a4780b71..7fa113858 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java @@ -17,6 +17,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; + import java.util.HashMap; import java.util.Map; @@ -69,10 +70,10 @@ public class VisionConstants { // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = - new double[] { - 1.0, // OV1 - 1.0, // OV2 - 1.0 // OV3 + new double[]{ + 1.0, // OV1 + 1.0, // OV2 + 1.0 // OV3 }; // Multipliers to apply for MegaTag 2 observations diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java index f2ed010a1..6119adfd2 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java @@ -27,17 +27,23 @@ public static class VisionIOInputs { public int[] tagIds = new int[0]; } - /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + /** + * Represents the angle to a simple target, not used for pose estimation. + */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) { + } - /** Represents a robot pose sample used for pose estimation. */ + /** + * Represents a robot pose sample used for pose estimation. + */ public static record PoseObservation( double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance, - PoseObservationType type) {} + PoseObservationType type) { + } public static enum PoseObservationType { MEGATAG_1, @@ -45,5 +51,6 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) {} + public default void updateInputs(VisionIOInputs inputs) { + } } diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java index f40f10141..31fbfe682 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -22,13 +22,16 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; + import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; import java.util.function.Supplier; -/** IO implementation for real Limelight hardware. */ +/** + * IO implementation for real Limelight hardware. + */ public class VisionIOLimelight implements VisionIO { private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -42,7 +45,7 @@ public class VisionIOLimelight implements VisionIO { /** * Creates a new VisionIOLimelight. * - * @param name The configured name of the Limelight. + * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ public VisionIOLimelight(String name, Supplier rotationSupplier) { @@ -53,9 +56,9 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); megatag1Subscriber = - table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[]{}); megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); } @Override @@ -72,7 +75,7 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 orientationPublisher.accept( - new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + new double[]{rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); NetworkTableInstance.getDefault() .flush(); // Increases network traffic but recommended by Limelight @@ -145,7 +148,9 @@ public void updateInputs(VisionIOInputs inputs) { } } - /** Parses the 3D pose from a Limelight botpose array. */ + /** + * Parses the 3D pose from a Limelight botpose array. + */ private static Pose3d parsePose(double[] rawLLArray) { return new Pose3d( rawLLArray[0], diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java index c037b84f9..d36c26eb0 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -16,13 +16,17 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; + import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; + import org.photonvision.PhotonCamera; -/** IO implementation for real PhotonVision hardware. */ +/** + * IO implementation for real PhotonVision hardware. + */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; @@ -30,7 +34,7 @@ public class VisionIOPhotonVision implements VisionIO { /** * Creates a new VisionIOPhotonVision. * - * @param name The configured name of the camera. + * @param name The configured name of the camera. * @param rotationSupplier The 3D position of the camera relative to the robot. */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index a645bd1a1..2049e1498 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -17,12 +17,16 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; + import java.util.function.Supplier; + import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -/** IO implementation for physics sim using PhotonVision simulator. */ +/** + * IO implementation for physics sim using PhotonVision simulator. + */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { private static VisionSystemSim visionSim; @@ -32,7 +36,7 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { /** * Creates a new VisionIOPhotonVisionSim. * - * @param name The name of the camera. + * @param name The name of the camera. * @param poseSupplier Supplier for the robot pose to use in simulation. */ public VisionIOPhotonVisionSim( From b8bf68b6c6ed5cc834f87b85a5511b897a238f93 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:25:47 +0200 Subject: [PATCH 018/253] remove unnecessary Climb from the names of the vals, and add const to them --- src/main/kotlin/frc/robot/subsystems/Port.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 794799f6b..7d1f84884 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -2,8 +2,8 @@ package frc.robot.subsystems object Port { object Climber { - val mainClimberMotor = 0 - val auxClimberMotor = 1 - val ClimberSensor = 2 + const val mainMotor = 0 + const val auxMotor = 1 + const val Sensor = 2 } } \ No newline at end of file From 9673208178b3742f1d587d6f1b0a673b20322c26 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:10:30 +0200 Subject: [PATCH 019/253] Create Climber io real --- .../robot/subsystems/climber/ClimberIOReal.kt | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt new file mode 100644 index 000000000..eeda08aac --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -0,0 +1,32 @@ +package frc.robot.subsystems.climber + +import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance + +class ClimberIOReal:ClimberIO { + override var inputs: LoggedInputClimber = LoggedInputClimber() +// var mainMotor = TalonFX() + override fun setLatchPosition(position: Distance) { + + } + + override fun setPower(power: Double) { + } + + override fun setAngle(angle: Angle) { + + } + + override fun lock() { + + } + + override fun unlock() { + super.unlock() + } + + override fun updateInput() { + super.updateInput() + } +} \ No newline at end of file From c332702f44e75d2c0ce7c2a3087187425e22f60b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 17:58:34 +0200 Subject: [PATCH 020/253] Add vars to pass in those functions --- .../robot/subsystems/climber/ClimberIOSim.kt | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt new file mode 100644 index 000000000..989d6ece6 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -0,0 +1,29 @@ +package frc.robot.subsystems.climber + +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionVoltage +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance +import frc.robot.lib.motors.TalonFXSim +import frc.robot.lib.motors.TalonType + +class ClimberIOSim : ClimberIO { + override var inputs: LoggedInputClimber = LoggedInputClimber() + var motor = TalonFXSim(2, gearRation, momentOfInertia.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + var dutyCycle = DutyCycleOut(0.0) + var positionControler =PositionVoltage(0.0) + + override fun setLatchPosition(position: Distance) { + + } + + override fun setPower(power:Double) { + motor.setControl(dutyCycle.withOutput(power)) + } + + override fun setAngle(angle: Angle) { + motor.setControl(positionControler.withPosition(angle)) + } + +} \ No newline at end of file From 3a136f5234bcf791246038e8f49e137f1eb2904d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 18:12:59 +0200 Subject: [PATCH 021/253] Add updateInput to ioSim --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 989d6ece6..6528390cb 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -5,6 +5,7 @@ import com.ctre.phoenix6.controls.PositionVoltage import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj.Timer import frc.robot.lib.motors.TalonFXSim import frc.robot.lib.motors.TalonType @@ -26,4 +27,9 @@ class ClimberIOSim : ClimberIO { motor.setControl(positionControler.withPosition(angle)) } + override fun updateInput() { + motor.update(Timer.getFPGATimestamp()) + inputs.angle = Units.Rotations.of(motor.position) + inputs.appliedVoltage = Units.Volt.of(motor.appliedVoltage) + } } \ No newline at end of file From 4451b6ea0ed192cc8cb75a563ec2ed3d376c41c4 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:23:55 +0200 Subject: [PATCH 022/253] Add lock unlock fold and unfold --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 282aef646..833500f48 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,7 @@ package frc.robot.subsystems.climber +import edu.wpi.first.networktables.DoubleEntry +import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -33,8 +35,13 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { ) } } - - fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) + fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) + fun lock():Command = Commands.runOnce({io.lock()}) + fun unlock():Command = Commands.runOnce({io.unlock()}) + fun unfold(){} //TODO + fun fold(){} //TODO + private fun setAngle(angle:Angle):Command = Commands.runOnce({io.setAngle(angle)}) + private fun setPower(power:Double):Command = Commands.runOnce({io.setPower(power)}) } \ No newline at end of file From 5b18dabd52fc9221150a6488486f0d129dd538e1 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:24:02 +0200 Subject: [PATCH 023/253] Add todo --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 6528390cb..88d22acf1 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -19,6 +19,14 @@ class ClimberIOSim : ClimberIO { } + override fun lock() { + //TODO + } + + override fun unlock() { + //TODO + } + override fun setPower(power:Double) { motor.setControl(dutyCycle.withOutput(power)) } From 77f976f5ada5dfc257e49699ed6403f2bafe044c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:40:09 +0200 Subject: [PATCH 024/253] Add pid to constants --- .../robot/subsystems/climber/ClimberConstants.kt | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c9cb76fb9..e80b28c93 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -4,6 +4,8 @@ import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia +import frc.robot.lib.Gains +import frc.robot.lib.selectGainsBasedOnMode const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 @@ -14,4 +16,13 @@ val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val gearRation = 1.0 val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +var latchTolerance: Distance = Units.Centimeter.of(0.4) +var Gains = selectGainsBasedOnMode( + Gains( + 0.0, + 0.0, + ), Gains( + 0.0, + 0.0, + ) +) From 7ff113fea033ba6fe79cca111f83810a606b2a5c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:54:03 +0200 Subject: [PATCH 025/253] Changed constants to upper case letters --- .../frc/robot/subsystems/climber/Climber.kt | 4 +-- .../subsystems/climber/ClimberConstants.kt | 26 ++++++++++++++++--- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 282aef646..85df2c070 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -10,9 +10,9 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } + private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e80b28c93..d7a18be42 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -1,5 +1,8 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance @@ -13,10 +16,25 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 -val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) +const val GEAR_RATIO = 1.0 +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) +var MOTOR_CONFIG = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.Clockwise_Positive + } + CurrentLimits.apply { + StatorCurrentLimitEnable = false + SupplyCurrentLimitEnable = false + } + Slot0.apply { + kD = Gains.kD + kP = Gains.kP + kI = Gains.kI + } +} var Gains = selectGainsBasedOnMode( Gains( 0.0, From ec9a2876e126a57ae39e4ac7f8a762579d78391f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:54:18 +0200 Subject: [PATCH 026/253] Add pid and config to motors --- .../frc/robot/subsystems/climber/ClimberIOReal.kt | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index eeda08aac..ffad9196b 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,12 +1,22 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import frc.robot.subsystems.Port class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() -// var mainMotor = TalonFX() + var mainMotor = TalonFX(Port.Climber.mainMotor) + var auxMotor = TalonFX(Port.Climber.auxMotor) + + init { + auxMotor.setControl(StrictFollower(mainMotor.deviceID)) + listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } + } + override fun setLatchPosition(position: Distance) { } From 5b462273a85fd13a27e6e520ce8240e0bb7ac24c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:55:02 +0200 Subject: [PATCH 027/253] Changed constants to upper case letters --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c9cb76fb9..47ece7d39 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,7 +11,7 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 -val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +const val GEAR_RATIO = 1.0 +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From cd5f5d7d9f11a02d37113bc774dc1ea554608187 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:14:20 +0200 Subject: [PATCH 028/253] Add LinearServo --- .../frc/robot/lib/motors/LinearServo.java | 41 +++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 src/main/kotlin/frc/robot/lib/motors/LinearServo.java diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java new file mode 100644 index 000000000..bde3843f4 --- /dev/null +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java @@ -0,0 +1,41 @@ +package frc.robot.lib.motors; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Timer; + +public class LinearServo extends Servo { + double m_speed; + double m_length; + double setPos; + double curPos; + + public LinearServo(int channel, int length, int speed) { + super(channel); +// setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + m_length = length; + m_speed = speed; + } + + public void setPosition(double setpoint) { + setPos = MathUtil.clamp(setpoint, 0, m_length); + setSpeed((setPos / m_length * 2) - 1); + } + + double lastTime = 0; + + public void updateCurPos() { + double dt = Timer.getFPGATimestamp() - lastTime; + if (curPos > setPos + m_speed * dt) { + curPos -= m_speed * dt; + } else if (curPos < setPos - m_speed * dt) { + curPos += m_speed * dt; + } else { + curPos = setPos; + } + } + + public double getPosition() { + return curPos; + } +} From 34366e40cb2726058c61f6cf923a55db4b98518b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:20:59 +0200 Subject: [PATCH 029/253] Add Control request to set power and angle --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index ffad9196b..e3ce2be62 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,6 +1,8 @@ package frc.robot.subsystems.climber import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle @@ -11,6 +13,8 @@ class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var mainMotor = TalonFX(Port.Climber.mainMotor) var auxMotor = TalonFX(Port.Climber.auxMotor) + var dutyCycleOut = DutyCycleOut(0.0) + var positionVoltage = PositionVoltage(0.0) init { auxMotor.setControl(StrictFollower(mainMotor.deviceID)) @@ -22,10 +26,11 @@ class ClimberIOReal:ClimberIO { } override fun setPower(power: Double) { + mainMotor.setControl(dutyCycleOut.withOutput(power)) } override fun setAngle(angle: Angle) { - + mainMotor.setControl(positionVoltage.withPosition(angle)) } override fun lock() { @@ -33,7 +38,6 @@ class ClimberIOReal:ClimberIO { } override fun unlock() { - super.unlock() } override fun updateInput() { From 9ecf0941f3e0d63241a58b7db7f1818dad326109 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:21:15 +0200 Subject: [PATCH 030/253] add inputs to updatedInputs --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index e3ce2be62..6ea64b2ea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -41,6 +41,8 @@ class ClimberIOReal:ClimberIO { } override fun updateInput() { - super.updateInput() + inputs.angle = mainMotor.position.value + inputs.appliedVoltage = mainMotor.supplyVoltage.value +// inputs.latchPosition = } } \ No newline at end of file From c87a35325712b7bf1977aa55fb3f6fa61e4723e7 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:55:02 +0200 Subject: [PATCH 031/253] Changed constants to upper case letters --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 ++-- .../frc/robot/subsystems/climber/ClimberConstants.kt | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 282aef646..85df2c070 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -10,9 +10,9 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } + private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c9cb76fb9..47ece7d39 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,7 +11,7 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 -val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +const val GEAR_RATIO = 1.0 +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From 456fbde8e0a75f0a74591b98f7833b1fbd9ac59e Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:24:02 +0200 Subject: [PATCH 032/253] Add more ports --- src/main/kotlin/frc/robot/subsystems/Port.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 7d1f84884..e13d89c75 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -4,6 +4,8 @@ object Port { object Climber { const val mainMotor = 0 const val auxMotor = 1 - const val Sensor = 2 + const val servo1 = 2 + const val servo2 = 3 + const val Sensor = 4 } } \ No newline at end of file From df77c8b9b945d98c79a3f7961bafef3c61b0b62f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:10:30 +0200 Subject: [PATCH 033/253] Create Climber io real --- .../robot/subsystems/climber/ClimberIOReal.kt | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt new file mode 100644 index 000000000..eeda08aac --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -0,0 +1,32 @@ +package frc.robot.subsystems.climber + +import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance + +class ClimberIOReal:ClimberIO { + override var inputs: LoggedInputClimber = LoggedInputClimber() +// var mainMotor = TalonFX() + override fun setLatchPosition(position: Distance) { + + } + + override fun setPower(power: Double) { + } + + override fun setAngle(angle: Angle) { + + } + + override fun lock() { + + } + + override fun unlock() { + super.unlock() + } + + override fun updateInput() { + super.updateInput() + } +} \ No newline at end of file From ee8938a3a403b4f1609bba09ed01be7c2a471878 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:40:09 +0200 Subject: [PATCH 034/253] Add pid to constants --- .../subsystems/climber/ClimberConstants.kt | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 47ece7d39..e80b28c93 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -4,6 +4,8 @@ import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia +import frc.robot.lib.Gains +import frc.robot.lib.selectGainsBasedOnMode const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 @@ -11,7 +13,16 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val GEAR_RATIO = 1.0 -val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +const val gearRation = 1.0 +val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var distanceThreshold: Distance = Units.Centimeter.of(0.4) +var latchTolerance: Distance = Units.Centimeter.of(0.4) +var Gains = selectGainsBasedOnMode( + Gains( + 0.0, + 0.0, + ), Gains( + 0.0, + 0.0, + ) +) From 99faa3a49c9d83abe58105a920b6defa36b2607f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:54:03 +0200 Subject: [PATCH 035/253] Changed constants to upper case letters --- .../subsystems/climber/ClimberConstants.kt | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e80b28c93..d7a18be42 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -1,5 +1,8 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance @@ -13,10 +16,25 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 -val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) +const val GEAR_RATIO = 1.0 +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) +var MOTOR_CONFIG = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.Clockwise_Positive + } + CurrentLimits.apply { + StatorCurrentLimitEnable = false + SupplyCurrentLimitEnable = false + } + Slot0.apply { + kD = Gains.kD + kP = Gains.kP + kI = Gains.kI + } +} var Gains = selectGainsBasedOnMode( Gains( 0.0, From dfaea0dadaca530a6c9307482022c8718a89aa7f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:54:18 +0200 Subject: [PATCH 036/253] Add pid and config to motors --- .../frc/robot/subsystems/climber/ClimberIOReal.kt | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index eeda08aac..ffad9196b 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,12 +1,22 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import frc.robot.subsystems.Port class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() -// var mainMotor = TalonFX() + var mainMotor = TalonFX(Port.Climber.mainMotor) + var auxMotor = TalonFX(Port.Climber.auxMotor) + + init { + auxMotor.setControl(StrictFollower(mainMotor.deviceID)) + listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } + } + override fun setLatchPosition(position: Distance) { } From 5ac710acf397bf70b088da88c7b5b6547d0d9d95 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:20:59 +0200 Subject: [PATCH 037/253] Add Control request to set power and angle --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index ffad9196b..e3ce2be62 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,6 +1,8 @@ package frc.robot.subsystems.climber import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle @@ -11,6 +13,8 @@ class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var mainMotor = TalonFX(Port.Climber.mainMotor) var auxMotor = TalonFX(Port.Climber.auxMotor) + var dutyCycleOut = DutyCycleOut(0.0) + var positionVoltage = PositionVoltage(0.0) init { auxMotor.setControl(StrictFollower(mainMotor.deviceID)) @@ -22,10 +26,11 @@ class ClimberIOReal:ClimberIO { } override fun setPower(power: Double) { + mainMotor.setControl(dutyCycleOut.withOutput(power)) } override fun setAngle(angle: Angle) { - + mainMotor.setControl(positionVoltage.withPosition(angle)) } override fun lock() { @@ -33,7 +38,6 @@ class ClimberIOReal:ClimberIO { } override fun unlock() { - super.unlock() } override fun updateInput() { From 29c8563fe883e2bcb775ce5d07a86143de07829f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:21:15 +0200 Subject: [PATCH 038/253] add inputs to updatedInputs --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index e3ce2be62..6ea64b2ea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -41,6 +41,8 @@ class ClimberIOReal:ClimberIO { } override fun updateInput() { - super.updateInput() + inputs.angle = mainMotor.position.value + inputs.appliedVoltage = mainMotor.supplyVoltage.value +// inputs.latchPosition = } } \ No newline at end of file From 700be741f5b8a7e3790b0b9fa565c9a7e503c98a Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 039/253] change latch position from distance to double --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 47ece7d39..c35393bb2 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -7,8 +7,8 @@ import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +const val OPEN_LATCH_POSITION = 0.8 +const val CLOSE_LATCH_POSITION = 0.2 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 0917e96fa..f93f1192c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -8,7 +8,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Distance) {} + fun setLatchPosition(position: Double) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} From 7d2790c56da8c44d53af932441a613563e9565d4 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 040/253] change latch position from distance to double --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index d7a18be42..5ba475492 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -12,8 +12,8 @@ import frc.robot.lib.selectGainsBasedOnMode const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +const val OPEN_LATCH_POSITION = 0.8 +const val CLOSE_LATCH_POSITION = 0.2 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 0917e96fa..f93f1192c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -8,7 +8,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Distance) {} + fun setLatchPosition(position: Double) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} From f2214f0cefc080f4578ed10fba5c58ab102d4d42 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:30:55 +0200 Subject: [PATCH 041/253] Add servo to code --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 6ea64b2ea..689c154aa 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix.motorcontrol.NeutralMode import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage @@ -7,11 +8,14 @@ import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj.Servo import frc.robot.subsystems.Port class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var mainMotor = TalonFX(Port.Climber.mainMotor) + var servo1 = Servo(Port.Climber.servo1) + var servo2 = Servo(Port.Climber.servo2) var auxMotor = TalonFX(Port.Climber.auxMotor) var dutyCycleOut = DutyCycleOut(0.0) var positionVoltage = PositionVoltage(0.0) @@ -21,8 +25,8 @@ class ClimberIOReal:ClimberIO { listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } } - override fun setLatchPosition(position: Distance) { - + override fun setLatchPosition(position: Double) { +// listOf(servo2,servo1).forEach{it.set()} } override fun setPower(power: Double) { From 1a3870c93224c4deb1fd56c06c943909e7fc2206 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 042/253] change latch position from distance to double --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index f93f1192c..866afccaf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -19,7 +19,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Distance = Units.Centimeter.zero() + var latchPosition: Double = 0.0 var sensorDistance: Distance = Units.Centimeter.zero() } } From 94d2be949efe592bdb1f91849babf57b679ce8df Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 043/253] change latch position from distance to double --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index f93f1192c..866afccaf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -19,7 +19,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Distance = Units.Centimeter.zero() + var latchPosition: Double = 0.0 var sensorDistance: Distance = Units.Centimeter.zero() } } From a29a20991222e8ceee762e2186662e3f8ed0812f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:50:44 +0200 Subject: [PATCH 044/253] Add setlatch function --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 689c154aa..5aa44dd1f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -26,7 +26,7 @@ class ClimberIOReal:ClimberIO { } override fun setLatchPosition(position: Double) { -// listOf(servo2,servo1).forEach{it.set()} + listOf(servo2, servo1).forEach { it.set(position) } } override fun setPower(power: Double) { From b1c94175ff58869e58c6a4959d8485cf13b5488c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:51:06 +0200 Subject: [PATCH 045/253] add input pose to updateinput --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 5aa44dd1f..b8549df73 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -42,11 +42,12 @@ class ClimberIOReal:ClimberIO { } override fun unlock() { + } override fun updateInput() { inputs.angle = mainMotor.position.value inputs.appliedVoltage = mainMotor.supplyVoltage.value -// inputs.latchPosition = + inputs.latchPosition = servo1.position } } \ No newline at end of file From b6df259db454800cafde6f4e337e1b81369df88d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 046/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 85df2c070..0e676f0bf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -12,7 +12,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c35393bb2..f7cd65c0f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -13,5 +13,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 \ No newline at end of file From c205142d3bd5b8192e6cfb9202c356dee093269b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 047/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 0e676f0bf..be8a77068 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -12,7 +12,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { From 09068e060b6fe7abbaedea1bc2cc4c09fb8ca880 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 048/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 85df2c070..be8a77068 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -12,7 +12,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 5ba475492..c94e715df 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -18,8 +18,8 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 var MOTOR_CONFIG = TalonFXConfiguration().apply { MotorOutput.apply { NeutralMode = NeutralModeValue.Brake From 0c4a5c32fe497c521c120d0663332a855cb530b5 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:55:02 +0200 Subject: [PATCH 049/253] Changed constants to upper case letters --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 ++-- .../frc/robot/subsystems/climber/ClimberConstants.kt | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 833500f48..d6f9203c2 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -12,9 +12,9 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } + private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c9cb76fb9..47ece7d39 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,7 +11,7 @@ val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val gearRation = 1.0 -val momentOfInertia: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var distanceThreshold: Distance = Units.Centimeter.of(0.4) -var latchTolerance: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +const val GEAR_RATIO = 1.0 +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From 3d5f7f0024277fe7ede2d6994ab26ebaae393138 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:24:02 +0200 Subject: [PATCH 050/253] Add more ports --- src/main/kotlin/frc/robot/subsystems/Port.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 7d1f84884..e13d89c75 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -4,6 +4,8 @@ object Port { object Climber { const val mainMotor = 0 const val auxMotor = 1 - const val Sensor = 2 + const val servo1 = 2 + const val servo2 = 3 + const val Sensor = 4 } } \ No newline at end of file From 18d4d69a40c8faafc7e691430fc58c403d2d4ccf Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 051/253] change latch position from distance to double --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 47ece7d39..c35393bb2 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -7,8 +7,8 @@ import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +const val OPEN_LATCH_POSITION = 0.8 +const val CLOSE_LATCH_POSITION = 0.2 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 0917e96fa..f93f1192c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -8,7 +8,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Distance) {} + fun setLatchPosition(position: Double) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} From 7e0e18ba62ffe61931693aac735990a6557e9df1 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 21:33:12 +0200 Subject: [PATCH 052/253] change latch position from distance to double --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index f93f1192c..866afccaf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -19,7 +19,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Distance = Units.Centimeter.zero() + var latchPosition: Double = 0.0 var sensorDistance: Distance = Units.Centimeter.zero() } } From 2e8891e722bcd7ab5f6b410dd093ebe84d8c2d67 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 053/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index d6f9203c2..80b73c54e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -14,7 +14,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(LATCH_TOLERANCE) } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c35393bb2..f7cd65c0f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -13,5 +13,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 \ No newline at end of file From 2468f9028785fa5564c61b2c4988753228fd5413 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 054/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 80b73c54e..de8ec5401 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -14,7 +14,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { From 023653f1d4b68fa8d039bf73a8b96710dfe4aa5c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:12:33 +0200 Subject: [PATCH 055/253] Changed constants to upper case letters --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 88d22acf1..ce1e2f02c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -11,12 +11,18 @@ import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() - var motor = TalonFXSim(2, gearRation, momentOfInertia.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + var motor = TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) var dutyCycle = DutyCycleOut(0.0) var positionControler =PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { + override fun lock() { + //TODO + } + + override fun unlock() { + //TODO } override fun lock() { From c84b9c4c240f3f2d0a92c4ba496c2c02c561bf74 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:19:12 +0200 Subject: [PATCH 056/253] oops 2 of the same functions --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index ce1e2f02c..80d65d1e2 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -17,12 +17,6 @@ class ClimberIOSim : ClimberIO { override fun setLatchPosition(position: Distance) { - override fun lock() { - //TODO - } - - override fun unlock() { - //TODO } override fun lock() { From 2d1026b0384c22078b952c93aec2c171e31bdc6c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:38:04 +0200 Subject: [PATCH 057/253] Add servo as solenoid --- .../robot/subsystems/climber/ClimberIOSim.kt | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 80d65d1e2..9436bbb04 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -2,21 +2,28 @@ package frc.robot.subsystems.climber import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage +import com.revrobotics.servohub.ServoHubSim import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj.simulation.SolenoidSim import frc.robot.lib.motors.TalonFXSim import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var motor = TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + var servo = SolenoidSim(PneumaticsModuleType.REVPH, 0) var dutyCycle = DutyCycleOut(0.0) - var positionControler =PositionVoltage(0.0) - - override fun setLatchPosition(position: Distance) { - + var positionControler = PositionVoltage(0.0) + + override fun setLatchPosition(position: Double) { + if (Double.equals(OPEN_LATCH_POSITION)) { + servo.output = true + } else { + servo.output = false + } } override fun lock() { @@ -27,7 +34,7 @@ class ClimberIOSim : ClimberIO { //TODO } - override fun setPower(power:Double) { + override fun setPower(power: Double) { motor.setControl(dutyCycle.withOutput(power)) } @@ -37,7 +44,7 @@ class ClimberIOSim : ClimberIO { override fun updateInput() { motor.update(Timer.getFPGATimestamp()) - inputs.angle = Units.Rotations.of(motor.position) + inputs.angle = Units.Rotations.of(motor.position) inputs.appliedVoltage = Units.Volt.of(motor.appliedVoltage) } } \ No newline at end of file From 5cdbd29dbc55e3d95cf4195542980e3c2314cfb5 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:43:03 +0200 Subject: [PATCH 058/253] Change servo to liner servo --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index b8549df73..a84cf9d76 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -9,13 +9,14 @@ import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo +import frc.robot.lib.motors.LinearServo import frc.robot.subsystems.Port class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var mainMotor = TalonFX(Port.Climber.mainMotor) - var servo1 = Servo(Port.Climber.servo1) - var servo2 = Servo(Port.Climber.servo2) + var servo1 = LinearServo(Port.Climber.servo1,1,1) + var servo2 = LinearServo(Port.Climber.servo2,1,1) var auxMotor = TalonFX(Port.Climber.auxMotor) var dutyCycleOut = DutyCycleOut(0.0) var positionVoltage = PositionVoltage(0.0) @@ -26,7 +27,7 @@ class ClimberIOReal:ClimberIO { } override fun setLatchPosition(position: Double) { - listOf(servo2, servo1).forEach { it.set(position) } + listOf(servo2, servo1).forEach { it.position = position } } override fun setPower(power: Double) { From c9c91a823887e0509a069babd3f5db48654b33ea Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:59:45 +0200 Subject: [PATCH 059/253] Add port for lockServo --- src/main/kotlin/frc/robot/subsystems/Port.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index e13d89c75..8c5768535 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -7,5 +7,6 @@ object Port { const val servo1 = 2 const val servo2 = 3 const val Sensor = 4 + const val lockServo = 5 } } \ No newline at end of file From 8fb524adf995da86f7dde313386589ccd0e4d536 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:07:42 +0200 Subject: [PATCH 060/253] Add port for lockServo --- src/main/kotlin/frc/robot/subsystems/Port.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index e13d89c75..8c5768535 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -7,5 +7,6 @@ object Port { const val servo1 = 2 const val servo2 = 3 const val Sensor = 4 + const val lockServo = 5 } } \ No newline at end of file From 9cbcbaacb8f87cf094851e4addef1603b9e6586d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:08:15 +0200 Subject: [PATCH 061/253] Accdently been removed. reAdded certain functions --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index be8a77068..ff71b4db4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -36,5 +37,11 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = Commands.runOnce({ io.lock() }) + fun unlock(): Command = Commands.runOnce({ io.unlock() }) + fun unfold() {} //TODO + fun fold() {} //TODO + private fun setAngle(angle: Angle): Command = Commands.runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = Commands.runOnce({ io.setPower(power) }) } \ No newline at end of file From aa19667368ad275e4ecb9c368453ff556a592458 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:09:06 +0200 Subject: [PATCH 062/253] Add lock position and unlock position --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index c94e715df..77ac78d81 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -14,6 +14,8 @@ const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 const val OPEN_LATCH_POSITION = 0.8 const val CLOSE_LATCH_POSITION = 0.2 +const val LOCK_POSITION = 0.8 +const val UNLOCK_POSITION = 0.2 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 From 466a76dc3f9d0445332c9f303a1ca47dfcd4fc4b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:09:45 +0200 Subject: [PATCH 063/253] Add the code to lock and unlock function --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index a84cf9d76..0173587e6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,6 +1,8 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix.motorcontrol.ControlMode import com.ctre.phoenix.motorcontrol.NeutralMode +import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage @@ -18,6 +20,7 @@ class ClimberIOReal:ClimberIO { var servo1 = LinearServo(Port.Climber.servo1,1,1) var servo2 = LinearServo(Port.Climber.servo2,1,1) var auxMotor = TalonFX(Port.Climber.auxMotor) + var lockServo = TalonSRX(Port.Climber.lockServo) var dutyCycleOut = DutyCycleOut(0.0) var positionVoltage = PositionVoltage(0.0) @@ -39,10 +42,11 @@ class ClimberIOReal:ClimberIO { } override fun lock() { - + lockServo.set(ControlMode.Position,LOCK_POSITION) } override fun unlock() { + lockServo.set(ControlMode.Position, UNLOCK_POSITION) } From 4d6bf80b563ec09055adf78aa92da642b5fe337e Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:31:21 +0200 Subject: [PATCH 064/253] Change the Command .run to be .run --- .../frc/robot/subsystems/climber/Climber.kt | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index ff71b4db4..b210a1ba6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -35,13 +35,13 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) - fun lock(): Command = Commands.runOnce({ io.lock() }) - fun unlock(): Command = Commands.runOnce({ io.unlock() }) - fun unfold() {} //TODO - fun fold() {} //TODO - private fun setAngle(angle: Angle): Command = Commands.runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = Commands.runOnce({ io.setPower(power) }) + fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() {} + fun fold(){} + private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) } \ No newline at end of file From 193f60316747d6ba7613682642ad4080fe9f43ef Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:32:05 +0200 Subject: [PATCH 065/253] Accdently been removed. reAdded certain functions --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index be8a77068..b210a1ba6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -34,7 +35,13 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() {} + fun fold(){} + private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) } \ No newline at end of file From bc5982ebcce2c4234a3c4394bbaf7dedf4c310d6 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:05 +0200 Subject: [PATCH 066/253] Add fold unfold climb and unClimb --- .../frc/robot/subsystems/climber/Climber.kt | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index b210a1ba6..a71704b00 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -35,13 +35,36 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = + runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + + fun openLatch(): Command = + runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() {} - fun fold(){} - private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + fun unfold() { + io.setAngle(UNFOLDED_ANGLE) + } + + fun fold() { + io.setAngle(FOLDED_ANGLE) + } + + fun climb(): Command = + run { closeLatch() } + .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) + .finallyDo(lock().onlyIf(isFolded)) + + fun unClimb(): Command = + run { setPower(-0.4) } + .andThen(unlock()) + .andThen({ unfold() }) + .finallyDo(openLatch()) + + private fun setAngle(angle: Angle): Command = + runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = + runOnce({ io.setPower(power) }) } \ No newline at end of file From 6d9c77103ca9cefda8363e2ec9a577f95f1897b0 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:24 +0200 Subject: [PATCH 067/253] spotless Apply --- src/main/kotlin/frc/robot/BuildConstants.java | 10 +- src/main/kotlin/frc/robot/subsystems/Port.kt | 2 +- .../frc/robot/subsystems/climber/Climber.kt | 20 ++-- .../subsystems/climber/ClimberConstants.kt | 5 +- .../frc/robot/subsystems/drive/Drive.java | 107 ++++++------------ .../robot/subsystems/drive/DriveCommands.java | 14 +-- .../frc/robot/subsystems/drive/GyroIO.java | 10 +- .../robot/subsystems/drive/GyroIONavX.java | 5 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 52 +++------ .../frc/robot/subsystems/drive/ModuleIO.java | 41 +++---- .../robot/subsystems/drive/ModuleIOSim.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 3 +- .../drive/PhoenixOdometryThread.java | 13 +-- .../subsystems/drive/TunerConstants.java | 72 ++++++------ .../frc/robot/subsystems/vision/Vision.java | 2 - .../subsystems/vision/VisionConstants.java | 9 +- .../frc/robot/subsystems/vision/VisionIO.java | 17 +-- .../subsystems/vision/VisionIOLimelight.java | 17 +-- .../vision/VisionIOPhotonVision.java | 8 +- .../vision/VisionIOPhotonVisionSim.java | 8 +- 21 files changed, 155 insertions(+), 271 deletions(-) diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java index f1081e045..403fdda95 100644 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ b/src/main/kotlin/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Alt-Swerve"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 114; - public static final String GIT_SHA = "d47594e8d8d8aa278bb659bda8a08f688a3f5ce5"; - public static final String GIT_DATE = "2025-01-15 01:04 IST"; + public static final int GIT_REVISION = 142; + public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; + public static final String GIT_DATE = "2025-01-17 15:32 IST"; public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-15 16:48 IST"; - public static final long BUILD_UNIX_TIME = 1736952481060L; + public static final String BUILD_DATE = "2025-01-17 15:51 IST"; + public static final long BUILD_UNIX_TIME = 1737121863309L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 8c5768535..7f0cd3fe8 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -9,4 +9,4 @@ object Port { const val Sensor = 4 const val lockServo = 5 } -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index a71704b00..02b78dc0e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,19 +2,24 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } + private var isTouching = Trigger { + inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isLatchClosed = Trigger { + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + } private var isAttached = Trigger(isLatchClosed.and(isTouching)) + private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { @Volatile @@ -29,9 +34,10 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun getInstance(): Climber { - return instance ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) + return instance + ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) } } @@ -67,4 +73,4 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index f7cd65c0f..09a6db6a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 @@ -13,5 +12,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index 8fea22e17..a0e84f1b9 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java @@ -52,11 +52,9 @@ import frc.robot.ConstantsKt; import frc.robot.Mode; import frc.robot.lib.LocalADStarAK; - import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -108,17 +106,16 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - @AutoLogOutput - private Rotation2d desiredHeading; + @AutoLogOutput private Rotation2d desiredHeading; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[]{ - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( @@ -221,8 +218,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } // Update odometry @@ -288,27 +285,21 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** - * Runs the drive in a straight line with the specified drive output. - */ + /** Runs the drive in a straight line with the specified drive output. */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runCharacterization(output); } } - /** - * NOTE: DO NOT USE WITH TorqueCurrentFOC - */ + /** NOTE: DO NOT USE WITH TorqueCurrentFOC */ public void runTurnCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runTurnCharacterization(output); } } - /** - * Stops the drive. - */ + /** Stops the drive. */ public void stop() { runVelocity(new ChassisSpeeds()); } @@ -326,18 +317,14 @@ public void stopWithX() { stop(); } - /** - * Returns a command to run a quasistatic test in the specified direction. - */ + /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) .andThen(sysId.quasistatic(direction)); } - /** - * Returns a command to run a dynamic test in the specified direction. - */ + /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) @@ -361,18 +348,14 @@ public Command runAllTurnSysID() { turnSysIdDynamic(SysIdRoutine.Direction.kReverse)); } - /** - * Returns a command to run a dynamic test in the specified direction on the turn modules. - */ + /** Returns a command to run a dynamic test in the specified direction on the turn modules. */ public Command turnSysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runTurnCharacterization(0.0)) .withTimeout(1.0) .andThen(turnSysId.dynamic(direction)); } - /** - * Returns the module states (turn angles and drive velocities) for all of the modules. - */ + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; @@ -382,9 +365,7 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** - * Returns the module positions (turn angles and drive positions) for all of the modules. - */ + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -393,17 +374,13 @@ private SwerveModulePosition[] getModulePositions() { return states; } - /** - * Returns the measured chassis speeds of the robot. - */ + /** Returns the measured chassis speeds of the robot. */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } - /** - * Returns the position of each module in radians. - */ + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; for (int i = 0; i < 4; i++) { @@ -412,9 +389,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** - * Returns the average velocity of the modules in rotations/sec (Phoenix native units). - */ + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -423,17 +398,13 @@ public double getFFCharacterizationVelocity() { return output; } - /** - * Returns the current odometry pose. - */ + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } - /** - * Returns the current gyro rotation. - */ + /** Returns the current gyro rotation. */ public Rotation2d getRotation() { return gyroInputs.yawPosition; } @@ -457,9 +428,7 @@ public Command setDesiredHeading(Rotation2d rotation) { return Commands.runOnce(() -> desiredHeading = rotation); } - /** - * Resets the current odometry pose. - */ + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -469,9 +438,7 @@ public void resetGyro() { desiredHeading = new Rotation2d(); } - /** - * Adds a new timestamped vision measurement. - */ + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -480,32 +447,26 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** - * Returns the maximum linear speed in meters per sec. - */ + /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } - /** - * Returns the maximum angular speed in radians per sec. - */ + /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } - /** - * Returns an array of module translations. - */ + /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { - return new Translation2d[]{ - new Translation2d( - TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), - new Translation2d( - TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), - new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - new Translation2d( - TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) + return new Translation2d[] { + new Translation2d( + TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d( + TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d( + TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java index 65ef7f8aa..3d59b9b4a 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java @@ -27,7 +27,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; - import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -47,8 +46,7 @@ public class DriveCommands { private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 private static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.4); - private DriveCommands() { - } + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { // Apply deadband @@ -150,7 +148,7 @@ public static Command joystickDriveAtAngle( speeds, isFlipped ? drive.getRotation() - .plus(new Rotation2d(Math.PI)) + .plus(new Rotation2d(Math.PI)) : drive.getRotation())); }, drive) @@ -226,9 +224,7 @@ public static Command feedforwardCharacterization(Drive drive) { })); } - /** - * Measures the robot's wheel radius by spinning in a circle. - */ + /** Measures the robot's wheel radius by spinning in a circle. */ public static Command wheelRadiusCharacterization(Drive drive) { SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); @@ -306,8 +302,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { + formatter.format(wheelRadius) + " meters, " + formatter.format( - Units.metersToInches( - wheelRadius)) + Units.metersToInches( + wheelRadius)) + " inches"); }))); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java index a87dce8e4..1ea03e137 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java @@ -22,13 +22,11 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public double yawVelocityRadPerSec = 0.0; - public double[] odometryYawTimestamps = new double[]{}; - public Rotation2d[] odometryYawPositions = new Rotation2d[]{}; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } - public default void zeroGyro() { - } + public default void zeroGyro() {} - public default void updateInputs(GyroIOInputs inputs) { - } + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java index 3caa3b63f..3e2c5e9ae 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java @@ -17,12 +17,9 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; - import java.util.Queue; -/** - * IO implementation for NavX. - */ +/** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { private final AHRS navX = new AHRS(NavXComType.kUSB1, (byte) Drive.ODOMETRY_FREQUENCY); private final Queue yawPositionQueue; diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java index 40d5007c8..f56f370e1 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -22,12 +22,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; - import java.util.Queue; -/** - * IO implementation for Pigeon 2. - */ +/** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2( diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Module.java b/src/main/kotlin/frc/robot/subsystems/drive/Module.java index 538ff2992..06f91457e 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Module.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Module.java @@ -29,13 +29,13 @@ public class Module { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{}; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module( ModuleIO io, @@ -78,9 +78,7 @@ public void periodic() { turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); } - /** - * Runs the module with the specified setpoint state. Mutates the state to optimize it. - */ + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint state.optimize(getAngle()); @@ -91,9 +89,7 @@ public void runSetpoint(SwerveModuleState state) { io.setTurnPosition(state.angle); } - /** - * Runs the module with the specified output while controlling to zero degrees. - */ + /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); io.setTurnPosition(new Rotation2d()); @@ -104,73 +100,53 @@ public void runTurnCharacterization(double output) { io.setTurnOpenLoop(output); } - /** - * Disables all outputs to motors. - */ + /** Disables all outputs to motors. */ public void stop() { io.setDriveOpenLoop(0.0); io.setTurnOpenLoop(0.0); } - /** - * Returns the current turn angle of the module. - */ + /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { return inputs.turnPosition; } - /** - * Returns the current drive position of the module in meters. - */ + /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { return inputs.drivePositionRad * constants.WheelRadius; } - /** - * Returns the current drive velocity of the module in meters per second. - */ + /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * constants.WheelRadius; } - /** - * Returns the module position (turn angle and drive position). - */ + /** Returns the module position (turn angle and drive position). */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** - * Returns the module state (turn angle and drive velocity). - */ + /** Returns the module state (turn angle and drive velocity). */ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** - * Returns the module positions received this cycle. - */ + /** Returns the module positions received this cycle. */ public SwerveModulePosition[] getOdometryPositions() { return odometryPositions; } - /** - * Returns the timestamps of the samples received this cycle. - */ + /** Returns the timestamps of the samples received this cycle. */ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** - * Returns the module position in radians. - */ + /** Returns the module position in radians. */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; } - /** - * Returns the module velocity in rotations/sec (Phoenix native units). - */ + /** Returns the module velocity in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { return Units.radiansToRotations(inputs.driveVelocityRadPerSec); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java index 649922d28..8a202cd27 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java @@ -34,38 +34,23 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double[] odometryTimestamps = new double[]{}; - public double[] odometryDrivePositionsRad = new double[]{}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; } - /** - * Updates the set of loggable inputs. - */ - public default void updateInputs(ModuleIOInputs inputs) { - } + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** - * Run the drive motor at the specified open loop value. - */ - public default void setDriveOpenLoop(double output) { - } + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} - /** - * Run the turn motor at the specified open loop value. - */ - public default void setTurnOpenLoop(double output) { - } + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} - /** - * Run the drive motor at the specified velocity. - */ - public default void setDriveVelocity(double velocityRadPerSec) { - } + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} - /** - * Run the turn motor to the specified rotation. - */ - public default void setTurnPosition(Rotation2d rotation) { - } + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java index a3e2f5f60..1e478376b 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java @@ -116,9 +116,9 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[]{inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[]{inputs.turnPosition}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 5c56a83ab..c904649fc 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -37,7 +37,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; - import java.util.Queue; /** @@ -48,7 +47,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; // Hardware objects diff --git a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 00a2f7069..8958af762 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -18,7 +18,6 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; - import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -67,9 +66,7 @@ public void start() { } } - /** - * Registers a Phoenix signal to be read from the thread. - */ + /** Registers a Phoenix signal to be read from the thread. */ public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -87,9 +84,7 @@ public Queue registerSignal(StatusSignal signal) { return queue; } - /** - * Registers a generic signal to be read from the thread. - */ + /** Registers a generic signal to be read from the thread. */ public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -104,9 +99,7 @@ public Queue registerSignal(DoubleSupplier signal) { return queue; } - /** - * Returns a new queue that returns timestamp values for each sample. - */ + /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); diff --git a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java index efd67b134..00988ee62 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java @@ -37,16 +37,16 @@ public class TunerConstants { public static SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants(); public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight; public static void init() { @@ -126,8 +126,8 @@ public static void init() { double[] offsets; if (ConstantsKt.getROBORIO_SERIAL_NUMBER().equals(ALT_ROBORIO_SERIAL)) { offsets = - new double[]{ - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 + new double[] { + 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 }; steerGains = @@ -233,11 +233,11 @@ public static void init() { kBackRightYPos = Meters.of(-0.24); } else { offsets = - new double[]{ - -2.9329712664373457, - -1.4818254410975293, - 0.11351457830353745, - 2.0586022173425302 + new double[] { + -2.9329712664373457, + -1.4818254410975293, + 0.11351457830353745, + 2.0586022173425302 }; steerGains = @@ -350,32 +350,32 @@ public static void init() { .withPigeon2Configs(pigeonConfigs); SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, - TalonFXConfiguration, - CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); FrontLeft = ConstantCreator.createModuleConstants( diff --git a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java index df73830c6..bb33178bb 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java @@ -26,10 +26,8 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; - import java.util.LinkedList; import java.util.List; - import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java index 7fa113858..4a4780b71 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java @@ -17,7 +17,6 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashMap; import java.util.Map; @@ -70,10 +69,10 @@ public class VisionConstants { // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = - new double[]{ - 1.0, // OV1 - 1.0, // OV2 - 1.0 // OV3 + new double[] { + 1.0, // OV1 + 1.0, // OV2 + 1.0 // OV3 }; // Multipliers to apply for MegaTag 2 observations diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java index 6119adfd2..f2ed010a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java @@ -27,23 +27,17 @@ public static class VisionIOInputs { public int[] tagIds = new int[0]; } - /** - * Represents the angle to a simple target, not used for pose estimation. - */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) { - } + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} - /** - * Represents a robot pose sample used for pose estimation. - */ + /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance, - PoseObservationType type) { - } + PoseObservationType type) {} public static enum PoseObservationType { MEGATAG_1, @@ -51,6 +45,5 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) { - } + public default void updateInputs(VisionIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java index 31fbfe682..f40f10141 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -22,16 +22,13 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; import java.util.function.Supplier; -/** - * IO implementation for real Limelight hardware. - */ +/** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -45,7 +42,7 @@ public class VisionIOLimelight implements VisionIO { /** * Creates a new VisionIOLimelight. * - * @param name The configured name of the Limelight. + * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ public VisionIOLimelight(String name, Supplier rotationSupplier) { @@ -56,9 +53,9 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); megatag1Subscriber = - table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); } @Override @@ -75,7 +72,7 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 orientationPublisher.accept( - new double[]{rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); NetworkTableInstance.getDefault() .flush(); // Increases network traffic but recommended by Limelight @@ -148,9 +145,7 @@ public void updateInputs(VisionIOInputs inputs) { } } - /** - * Parses the 3D pose from a Limelight botpose array. - */ + /** Parses the 3D pose from a Limelight botpose array. */ private static Pose3d parsePose(double[] rawLLArray) { return new Pose3d( rawLLArray[0], diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java index d36c26eb0..c037b84f9 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -16,17 +16,13 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; - import org.photonvision.PhotonCamera; -/** - * IO implementation for real PhotonVision hardware. - */ +/** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; @@ -34,7 +30,7 @@ public class VisionIOPhotonVision implements VisionIO { /** * Creates a new VisionIOPhotonVision. * - * @param name The configured name of the camera. + * @param name The configured name of the camera. * @param rotationSupplier The 3D position of the camera relative to the robot. */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 2049e1498..a645bd1a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -17,16 +17,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.function.Supplier; - import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -/** - * IO implementation for physics sim using PhotonVision simulator. - */ +/** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { private static VisionSystemSim visionSim; @@ -36,7 +32,7 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { /** * Creates a new VisionIOPhotonVisionSim. * - * @param name The name of the camera. + * @param name The name of the camera. * @param poseSupplier Supplier for the robot pose to use in simulation. */ public VisionIOPhotonVisionSim( From 996dbf61e5e966cfd3b103ac76a5132add59a5b2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:56:48 +0200 Subject: [PATCH 068/253] Chang the place and how ports are in climber --- src/main/kotlin/frc/robot/subsystems/Port.kt | 12 ------------ src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 8 ++++++++ 2 files changed, 8 insertions(+), 12 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/Port.kt create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt deleted file mode 100644 index 7f0cd3fe8..000000000 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems - -object Port { - object Climber { - const val mainMotor = 0 - const val auxMotor = 1 - const val servo1 = 2 - const val servo2 = 3 - const val Sensor = 4 - const val lockServo = 5 - } -} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt new file mode 100644 index 000000000..1d159ec95 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -0,0 +1,8 @@ +package frc.robot.subsystems.climber + +const val mainMotor = 0 +const val auxMotor = 1 +const val servo1 = 2 +const val servo2 = 3 +const val Sensor = 4 +const val lockServo = 5 From 7a3a7c61b6fd8d2497cb6d03222639a795cb616a Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 16:04:45 +0200 Subject: [PATCH 069/253] unTrack build constants --- src/main/kotlin/frc/robot/BuildConstants.java | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/BuildConstants.java diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java deleted file mode 100644 index 403fdda95..000000000 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "Alt-Swerve"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 142; - public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; - public static final String GIT_DATE = "2025-01-17 15:32 IST"; - public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-17 15:51 IST"; - public static final long BUILD_UNIX_TIME = 1737121863309L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} From 2de4f0987b48c5dc3c7a0197179c3135bf0cf4e8 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 16:29:10 +0200 Subject: [PATCH 070/253] Add gitignore to BuildConstants --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 795a7eb8f..12b767c11 100644 --- a/.gitignore +++ b/.gitignore @@ -164,10 +164,9 @@ annotation/build/ # Simulation GUI and other tools window save file *-window.json -.\src\main\kotlin\frc\robot\BuildConstants.java - simgui* networktables.json /.idea /ctre_sim/ /log-downloader/venv/ +/src/main/kotlin/frc/robot/BuildConstants.java From 5030321d4dd7737ad57ff49eaba96cacbc3f4868 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:59:45 +0200 Subject: [PATCH 071/253] Add port for lockServo --- src/main/kotlin/frc/robot/subsystems/Port.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index e13d89c75..8c5768535 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -7,5 +7,6 @@ object Port { const val servo1 = 2 const val servo2 = 3 const val Sensor = 4 + const val lockServo = 5 } } \ No newline at end of file From 656fa6fccb3c7fcbb396fc6cc5ec47b628e53ba2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:32:05 +0200 Subject: [PATCH 072/253] Accdently been removed. reAdded certain functions --- .../frc/robot/subsystems/climber/Climber.kt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index de8ec5401..b210a1ba6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,6 +1,5 @@ package frc.robot.subsystems.climber -import edu.wpi.first.networktables.DoubleEntry import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -35,13 +34,14 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { ) } } - fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) - fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) - fun lock():Command = Commands.runOnce({io.lock()}) - fun unlock():Command = Commands.runOnce({io.unlock()}) - fun unfold(){} //TODO - fun fold(){} //TODO - private fun setAngle(angle:Angle):Command = Commands.runOnce({io.setAngle(angle)}) - private fun setPower(power:Double):Command = Commands.runOnce({io.setPower(power)}) + + fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() {} + fun fold(){} + private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) } \ No newline at end of file From 810b1ce3bf11f40e9a69cfd30e6bac52d7b55e2b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:05 +0200 Subject: [PATCH 073/253] Add fold unfold climb and unClimb --- .../frc/robot/subsystems/climber/Climber.kt | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index b210a1ba6..a71704b00 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -35,13 +35,36 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = + runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + + fun openLatch(): Command = + runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() {} - fun fold(){} - private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + fun unfold() { + io.setAngle(UNFOLDED_ANGLE) + } + + fun fold() { + io.setAngle(FOLDED_ANGLE) + } + + fun climb(): Command = + run { closeLatch() } + .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) + .finallyDo(lock().onlyIf(isFolded)) + + fun unClimb(): Command = + run { setPower(-0.4) } + .andThen(unlock()) + .andThen({ unfold() }) + .finallyDo(openLatch()) + + private fun setAngle(angle: Angle): Command = + runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = + runOnce({ io.setPower(power) }) } \ No newline at end of file From c56e76f311cd482349033a5a23e9b19102b0e265 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:24 +0200 Subject: [PATCH 074/253] spotless Apply --- src/main/kotlin/frc/robot/BuildConstants.java | 10 +- src/main/kotlin/frc/robot/subsystems/Port.kt | 2 +- .../frc/robot/subsystems/climber/Climber.kt | 20 ++-- .../subsystems/climber/ClimberConstants.kt | 5 +- .../frc/robot/subsystems/drive/Drive.java | 107 ++++++------------ .../robot/subsystems/drive/DriveCommands.java | 14 +-- .../frc/robot/subsystems/drive/GyroIO.java | 10 +- .../robot/subsystems/drive/GyroIONavX.java | 5 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 52 +++------ .../frc/robot/subsystems/drive/ModuleIO.java | 41 +++---- .../robot/subsystems/drive/ModuleIOSim.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 3 +- .../drive/PhoenixOdometryThread.java | 13 +-- .../subsystems/drive/TunerConstants.java | 72 ++++++------ .../frc/robot/subsystems/vision/Vision.java | 2 - .../subsystems/vision/VisionConstants.java | 9 +- .../frc/robot/subsystems/vision/VisionIO.java | 17 +-- .../subsystems/vision/VisionIOLimelight.java | 17 +-- .../vision/VisionIOPhotonVision.java | 8 +- .../vision/VisionIOPhotonVisionSim.java | 8 +- 21 files changed, 155 insertions(+), 271 deletions(-) diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java index f1081e045..403fdda95 100644 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ b/src/main/kotlin/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Alt-Swerve"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 114; - public static final String GIT_SHA = "d47594e8d8d8aa278bb659bda8a08f688a3f5ce5"; - public static final String GIT_DATE = "2025-01-15 01:04 IST"; + public static final int GIT_REVISION = 142; + public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; + public static final String GIT_DATE = "2025-01-17 15:32 IST"; public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-15 16:48 IST"; - public static final long BUILD_UNIX_TIME = 1736952481060L; + public static final String BUILD_DATE = "2025-01-17 15:51 IST"; + public static final long BUILD_UNIX_TIME = 1737121863309L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 8c5768535..7f0cd3fe8 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -9,4 +9,4 @@ object Port { const val Sensor = 4 const val lockServo = 5 } -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index a71704b00..02b78dc0e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,19 +2,24 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } + private var isTouching = Trigger { + inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isLatchClosed = Trigger { + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + } private var isAttached = Trigger(isLatchClosed.and(isTouching)) + private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { @Volatile @@ -29,9 +34,10 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun getInstance(): Climber { - return instance ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) + return instance + ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) } } @@ -67,4 +73,4 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index f7cd65c0f..09a6db6a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 @@ -13,5 +12,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index 8fea22e17..a0e84f1b9 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java @@ -52,11 +52,9 @@ import frc.robot.ConstantsKt; import frc.robot.Mode; import frc.robot.lib.LocalADStarAK; - import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -108,17 +106,16 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - @AutoLogOutput - private Rotation2d desiredHeading; + @AutoLogOutput private Rotation2d desiredHeading; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[]{ - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( @@ -221,8 +218,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } // Update odometry @@ -288,27 +285,21 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** - * Runs the drive in a straight line with the specified drive output. - */ + /** Runs the drive in a straight line with the specified drive output. */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runCharacterization(output); } } - /** - * NOTE: DO NOT USE WITH TorqueCurrentFOC - */ + /** NOTE: DO NOT USE WITH TorqueCurrentFOC */ public void runTurnCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runTurnCharacterization(output); } } - /** - * Stops the drive. - */ + /** Stops the drive. */ public void stop() { runVelocity(new ChassisSpeeds()); } @@ -326,18 +317,14 @@ public void stopWithX() { stop(); } - /** - * Returns a command to run a quasistatic test in the specified direction. - */ + /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) .andThen(sysId.quasistatic(direction)); } - /** - * Returns a command to run a dynamic test in the specified direction. - */ + /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) @@ -361,18 +348,14 @@ public Command runAllTurnSysID() { turnSysIdDynamic(SysIdRoutine.Direction.kReverse)); } - /** - * Returns a command to run a dynamic test in the specified direction on the turn modules. - */ + /** Returns a command to run a dynamic test in the specified direction on the turn modules. */ public Command turnSysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runTurnCharacterization(0.0)) .withTimeout(1.0) .andThen(turnSysId.dynamic(direction)); } - /** - * Returns the module states (turn angles and drive velocities) for all of the modules. - */ + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; @@ -382,9 +365,7 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** - * Returns the module positions (turn angles and drive positions) for all of the modules. - */ + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -393,17 +374,13 @@ private SwerveModulePosition[] getModulePositions() { return states; } - /** - * Returns the measured chassis speeds of the robot. - */ + /** Returns the measured chassis speeds of the robot. */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } - /** - * Returns the position of each module in radians. - */ + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; for (int i = 0; i < 4; i++) { @@ -412,9 +389,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** - * Returns the average velocity of the modules in rotations/sec (Phoenix native units). - */ + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -423,17 +398,13 @@ public double getFFCharacterizationVelocity() { return output; } - /** - * Returns the current odometry pose. - */ + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } - /** - * Returns the current gyro rotation. - */ + /** Returns the current gyro rotation. */ public Rotation2d getRotation() { return gyroInputs.yawPosition; } @@ -457,9 +428,7 @@ public Command setDesiredHeading(Rotation2d rotation) { return Commands.runOnce(() -> desiredHeading = rotation); } - /** - * Resets the current odometry pose. - */ + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -469,9 +438,7 @@ public void resetGyro() { desiredHeading = new Rotation2d(); } - /** - * Adds a new timestamped vision measurement. - */ + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -480,32 +447,26 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** - * Returns the maximum linear speed in meters per sec. - */ + /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } - /** - * Returns the maximum angular speed in radians per sec. - */ + /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } - /** - * Returns an array of module translations. - */ + /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { - return new Translation2d[]{ - new Translation2d( - TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), - new Translation2d( - TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), - new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - new Translation2d( - TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) + return new Translation2d[] { + new Translation2d( + TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d( + TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d( + TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java index 65ef7f8aa..3d59b9b4a 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java @@ -27,7 +27,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; - import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -47,8 +46,7 @@ public class DriveCommands { private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 private static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.4); - private DriveCommands() { - } + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { // Apply deadband @@ -150,7 +148,7 @@ public static Command joystickDriveAtAngle( speeds, isFlipped ? drive.getRotation() - .plus(new Rotation2d(Math.PI)) + .plus(new Rotation2d(Math.PI)) : drive.getRotation())); }, drive) @@ -226,9 +224,7 @@ public static Command feedforwardCharacterization(Drive drive) { })); } - /** - * Measures the robot's wheel radius by spinning in a circle. - */ + /** Measures the robot's wheel radius by spinning in a circle. */ public static Command wheelRadiusCharacterization(Drive drive) { SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); @@ -306,8 +302,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { + formatter.format(wheelRadius) + " meters, " + formatter.format( - Units.metersToInches( - wheelRadius)) + Units.metersToInches( + wheelRadius)) + " inches"); }))); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java index a87dce8e4..1ea03e137 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java @@ -22,13 +22,11 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public double yawVelocityRadPerSec = 0.0; - public double[] odometryYawTimestamps = new double[]{}; - public Rotation2d[] odometryYawPositions = new Rotation2d[]{}; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } - public default void zeroGyro() { - } + public default void zeroGyro() {} - public default void updateInputs(GyroIOInputs inputs) { - } + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java index 3caa3b63f..3e2c5e9ae 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java @@ -17,12 +17,9 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; - import java.util.Queue; -/** - * IO implementation for NavX. - */ +/** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { private final AHRS navX = new AHRS(NavXComType.kUSB1, (byte) Drive.ODOMETRY_FREQUENCY); private final Queue yawPositionQueue; diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java index 40d5007c8..f56f370e1 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -22,12 +22,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; - import java.util.Queue; -/** - * IO implementation for Pigeon 2. - */ +/** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2( diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Module.java b/src/main/kotlin/frc/robot/subsystems/drive/Module.java index 538ff2992..06f91457e 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Module.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Module.java @@ -29,13 +29,13 @@ public class Module { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{}; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module( ModuleIO io, @@ -78,9 +78,7 @@ public void periodic() { turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); } - /** - * Runs the module with the specified setpoint state. Mutates the state to optimize it. - */ + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint state.optimize(getAngle()); @@ -91,9 +89,7 @@ public void runSetpoint(SwerveModuleState state) { io.setTurnPosition(state.angle); } - /** - * Runs the module with the specified output while controlling to zero degrees. - */ + /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); io.setTurnPosition(new Rotation2d()); @@ -104,73 +100,53 @@ public void runTurnCharacterization(double output) { io.setTurnOpenLoop(output); } - /** - * Disables all outputs to motors. - */ + /** Disables all outputs to motors. */ public void stop() { io.setDriveOpenLoop(0.0); io.setTurnOpenLoop(0.0); } - /** - * Returns the current turn angle of the module. - */ + /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { return inputs.turnPosition; } - /** - * Returns the current drive position of the module in meters. - */ + /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { return inputs.drivePositionRad * constants.WheelRadius; } - /** - * Returns the current drive velocity of the module in meters per second. - */ + /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * constants.WheelRadius; } - /** - * Returns the module position (turn angle and drive position). - */ + /** Returns the module position (turn angle and drive position). */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** - * Returns the module state (turn angle and drive velocity). - */ + /** Returns the module state (turn angle and drive velocity). */ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** - * Returns the module positions received this cycle. - */ + /** Returns the module positions received this cycle. */ public SwerveModulePosition[] getOdometryPositions() { return odometryPositions; } - /** - * Returns the timestamps of the samples received this cycle. - */ + /** Returns the timestamps of the samples received this cycle. */ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** - * Returns the module position in radians. - */ + /** Returns the module position in radians. */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; } - /** - * Returns the module velocity in rotations/sec (Phoenix native units). - */ + /** Returns the module velocity in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { return Units.radiansToRotations(inputs.driveVelocityRadPerSec); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java index 649922d28..8a202cd27 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java @@ -34,38 +34,23 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double[] odometryTimestamps = new double[]{}; - public double[] odometryDrivePositionsRad = new double[]{}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; } - /** - * Updates the set of loggable inputs. - */ - public default void updateInputs(ModuleIOInputs inputs) { - } + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** - * Run the drive motor at the specified open loop value. - */ - public default void setDriveOpenLoop(double output) { - } + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} - /** - * Run the turn motor at the specified open loop value. - */ - public default void setTurnOpenLoop(double output) { - } + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} - /** - * Run the drive motor at the specified velocity. - */ - public default void setDriveVelocity(double velocityRadPerSec) { - } + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} - /** - * Run the turn motor to the specified rotation. - */ - public default void setTurnPosition(Rotation2d rotation) { - } + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java index a3e2f5f60..1e478376b 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java @@ -116,9 +116,9 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[]{inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[]{inputs.turnPosition}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 5c56a83ab..c904649fc 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -37,7 +37,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; - import java.util.Queue; /** @@ -48,7 +47,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; // Hardware objects diff --git a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 00a2f7069..8958af762 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -18,7 +18,6 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; - import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -67,9 +66,7 @@ public void start() { } } - /** - * Registers a Phoenix signal to be read from the thread. - */ + /** Registers a Phoenix signal to be read from the thread. */ public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -87,9 +84,7 @@ public Queue registerSignal(StatusSignal signal) { return queue; } - /** - * Registers a generic signal to be read from the thread. - */ + /** Registers a generic signal to be read from the thread. */ public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -104,9 +99,7 @@ public Queue registerSignal(DoubleSupplier signal) { return queue; } - /** - * Returns a new queue that returns timestamp values for each sample. - */ + /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); diff --git a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java index efd67b134..00988ee62 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java @@ -37,16 +37,16 @@ public class TunerConstants { public static SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants(); public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight; public static void init() { @@ -126,8 +126,8 @@ public static void init() { double[] offsets; if (ConstantsKt.getROBORIO_SERIAL_NUMBER().equals(ALT_ROBORIO_SERIAL)) { offsets = - new double[]{ - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 + new double[] { + 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 }; steerGains = @@ -233,11 +233,11 @@ public static void init() { kBackRightYPos = Meters.of(-0.24); } else { offsets = - new double[]{ - -2.9329712664373457, - -1.4818254410975293, - 0.11351457830353745, - 2.0586022173425302 + new double[] { + -2.9329712664373457, + -1.4818254410975293, + 0.11351457830353745, + 2.0586022173425302 }; steerGains = @@ -350,32 +350,32 @@ public static void init() { .withPigeon2Configs(pigeonConfigs); SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, - TalonFXConfiguration, - CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); FrontLeft = ConstantCreator.createModuleConstants( diff --git a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java index df73830c6..bb33178bb 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java @@ -26,10 +26,8 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; - import java.util.LinkedList; import java.util.List; - import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java index 7fa113858..4a4780b71 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java @@ -17,7 +17,6 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashMap; import java.util.Map; @@ -70,10 +69,10 @@ public class VisionConstants { // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = - new double[]{ - 1.0, // OV1 - 1.0, // OV2 - 1.0 // OV3 + new double[] { + 1.0, // OV1 + 1.0, // OV2 + 1.0 // OV3 }; // Multipliers to apply for MegaTag 2 observations diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java index 6119adfd2..f2ed010a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java @@ -27,23 +27,17 @@ public static class VisionIOInputs { public int[] tagIds = new int[0]; } - /** - * Represents the angle to a simple target, not used for pose estimation. - */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) { - } + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} - /** - * Represents a robot pose sample used for pose estimation. - */ + /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance, - PoseObservationType type) { - } + PoseObservationType type) {} public static enum PoseObservationType { MEGATAG_1, @@ -51,6 +45,5 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) { - } + public default void updateInputs(VisionIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java index 31fbfe682..f40f10141 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -22,16 +22,13 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; import java.util.function.Supplier; -/** - * IO implementation for real Limelight hardware. - */ +/** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -45,7 +42,7 @@ public class VisionIOLimelight implements VisionIO { /** * Creates a new VisionIOLimelight. * - * @param name The configured name of the Limelight. + * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ public VisionIOLimelight(String name, Supplier rotationSupplier) { @@ -56,9 +53,9 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); megatag1Subscriber = - table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); } @Override @@ -75,7 +72,7 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 orientationPublisher.accept( - new double[]{rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); NetworkTableInstance.getDefault() .flush(); // Increases network traffic but recommended by Limelight @@ -148,9 +145,7 @@ public void updateInputs(VisionIOInputs inputs) { } } - /** - * Parses the 3D pose from a Limelight botpose array. - */ + /** Parses the 3D pose from a Limelight botpose array. */ private static Pose3d parsePose(double[] rawLLArray) { return new Pose3d( rawLLArray[0], diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java index d36c26eb0..c037b84f9 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -16,17 +16,13 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; - import org.photonvision.PhotonCamera; -/** - * IO implementation for real PhotonVision hardware. - */ +/** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; @@ -34,7 +30,7 @@ public class VisionIOPhotonVision implements VisionIO { /** * Creates a new VisionIOPhotonVision. * - * @param name The configured name of the camera. + * @param name The configured name of the camera. * @param rotationSupplier The 3D position of the camera relative to the robot. */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 2049e1498..a645bd1a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -17,16 +17,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.function.Supplier; - import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -/** - * IO implementation for physics sim using PhotonVision simulator. - */ +/** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { private static VisionSystemSim visionSim; @@ -36,7 +32,7 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { /** * Creates a new VisionIOPhotonVisionSim. * - * @param name The name of the camera. + * @param name The name of the camera. * @param poseSupplier Supplier for the robot pose to use in simulation. */ public VisionIOPhotonVisionSim( From 80aa2925c303d8d2fd06805926121019a840b27b Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:56:48 +0200 Subject: [PATCH 075/253] Chang the place and how ports are in climber --- src/main/kotlin/frc/robot/subsystems/Port.kt | 12 ------------ src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 8 ++++++++ 2 files changed, 8 insertions(+), 12 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/Port.kt create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt deleted file mode 100644 index 7f0cd3fe8..000000000 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems - -object Port { - object Climber { - const val mainMotor = 0 - const val auxMotor = 1 - const val servo1 = 2 - const val servo2 = 3 - const val Sensor = 4 - const val lockServo = 5 - } -} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt new file mode 100644 index 000000000..1d159ec95 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -0,0 +1,8 @@ +package frc.robot.subsystems.climber + +const val mainMotor = 0 +const val auxMotor = 1 +const val servo1 = 2 +const val servo2 = 3 +const val Sensor = 4 +const val lockServo = 5 From 253b32e9f0909718d0aae91928515b9b77ed69bc Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 16:04:45 +0200 Subject: [PATCH 076/253] unTrack build constants --- src/main/kotlin/frc/robot/BuildConstants.java | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/BuildConstants.java diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java deleted file mode 100644 index 403fdda95..000000000 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "Alt-Swerve"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 142; - public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; - public static final String GIT_DATE = "2025-01-17 15:32 IST"; - public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-17 15:51 IST"; - public static final long BUILD_UNIX_TIME = 1737121863309L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} From b95e638fdc04f642a2c08a199fa70f0949100139 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 16:29:10 +0200 Subject: [PATCH 077/253] Add gitignore to BuildConstants --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 795a7eb8f..12b767c11 100644 --- a/.gitignore +++ b/.gitignore @@ -164,10 +164,9 @@ annotation/build/ # Simulation GUI and other tools window save file *-window.json -.\src\main\kotlin\frc\robot\BuildConstants.java - simgui* networktables.json /.idea /ctre_sim/ /log-downloader/venv/ +/src/main/kotlin/frc/robot/BuildConstants.java From 05ef5278eb02536b6bb9b50a4cf7f417c4f7e142 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:52:36 +0200 Subject: [PATCH 078/253] Add servo2 and lockServo --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 9436bbb04..aaa11b4fb 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -14,7 +14,9 @@ import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var motor = TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) - var servo = SolenoidSim(PneumaticsModuleType.REVPH, 0) + var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + var lockServo = SolenoidSim(PneumaticsModuleType.REVPH, 0) var dutyCycle = DutyCycleOut(0.0) var positionControler = PositionVoltage(0.0) From b235f13d98c490ea63351411b6edadb528b62e8c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 14:57:10 +0200 Subject: [PATCH 079/253] Add servo 2 --- .../frc/robot/subsystems/climber/ClimberIOSim.kt | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index aaa11b4fb..c5713fc4c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -1,8 +1,8 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage -import com.revrobotics.servohub.ServoHubSim import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj.PneumaticsModuleType @@ -16,24 +16,22 @@ class ClimberIOSim : ClimberIO { var motor = TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) - var lockServo = SolenoidSim(PneumaticsModuleType.REVPH, 0) var dutyCycle = DutyCycleOut(0.0) var positionControler = PositionVoltage(0.0) override fun setLatchPosition(position: Double) { if (Double.equals(OPEN_LATCH_POSITION)) { - servo.output = true + listOf(servo2, servo1).forEach { it.output = true } } else { - servo.output = false + listOf(servo2, servo1).forEach { it.output = false } } } override fun lock() { - //TODO } override fun unlock() { - //TODO + } override fun setPower(power: Double) { From 5e140176a462051a000cc72a7b1fbba53ed33127 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:11:45 +0200 Subject: [PATCH 080/253] Add Unlock and lock angle --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 09a6db6a7..9d1e579f7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -8,6 +8,8 @@ const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 const val OPEN_LATCH_POSITION = 0.8 const val CLOSE_LATCH_POSITION = 0.2 +val UNLOCK_ANGLE = Units.Degree.of(90.0) +val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 From d5f229743ce5b46358ef65c7a9e59b338ab2a547 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:12:34 +0200 Subject: [PATCH 081/253] Add moment of inertia for lock motor --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 9d1e579f7..824e30304 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -13,6 +13,7 @@ val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 -val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val MOMENT_OF_INERTIA_MAIN: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) var LATCH_TOLERANCE = 0.03 From 47c4c2b8864a1d01dfa3e10a0f8526c2e7e1d76d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:13:30 +0200 Subject: [PATCH 082/253] Change the names for ports --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt index 1d159ec95..f5bb4bd7c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -1,8 +1,8 @@ package frc.robot.subsystems.climber -const val mainMotor = 0 -const val auxMotor = 1 -const val servo1 = 2 -const val servo2 = 3 -const val Sensor = 4 -const val lockServo = 5 +const val MAIN_MOTOR_ID = 0 +const val AUX_MOTOR_ID = 1 +const val SERVO_1_ID = 2 +const val SERVO_2_ID = 3 +const val SENSOR_ID = 4 +const val LOCK_MOTOR_ID = 5 From 86427eac6da8f1579c88ac0bc95a0bed80606e02 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:14:37 +0200 Subject: [PATCH 083/253] Add code for lockmotor --- .../robot/subsystems/climber/ClimberIOSim.kt | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index c5713fc4c..6e20ed1e9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -1,24 +1,36 @@ package frc.robot.subsystems.climber -import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.math.system.plant.LinearSystemId import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj.simulation.DCMotorSim import edu.wpi.first.wpilibj.simulation.SolenoidSim import frc.robot.lib.motors.TalonFXSim import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() - var motor = TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + var motor = + TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA_MAIN.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + val lockMotor = DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getBag(1), + MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), + 1.0 + ), + DCMotor.getBag(1) + ) var dutyCycle = DutyCycleOut(0.0) var positionControler = PositionVoltage(0.0) + override fun setLatchPosition(position: Double) { if (Double.equals(OPEN_LATCH_POSITION)) { listOf(servo2, servo1).forEach { it.output = true } @@ -28,10 +40,11 @@ class ClimberIOSim : ClimberIO { } override fun lock() { + lockMotor.setAngle(LOCK_ANGLE.`in`(Units.Radians)) } override fun unlock() { - + lockMotor.setAngle(UNLOCK_ANGLE.`in`(Units.Radians)) } override fun setPower(power: Double) { @@ -44,7 +57,13 @@ class ClimberIOSim : ClimberIO { override fun updateInput() { motor.update(Timer.getFPGATimestamp()) + lockMotor.update(Timer.getFPGATimestamp()) inputs.angle = Units.Rotations.of(motor.position) inputs.appliedVoltage = Units.Volt.of(motor.appliedVoltage) + if (servo1.output) { + inputs.latchPosition = OPEN_LATCH_POSITION + } else { + inputs.latchPosition = CLOSE_LATCH_POSITION + } } } \ No newline at end of file From 88b10a448b50d98968c185f2b81b636933137c0d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:16:56 +0200 Subject: [PATCH 084/253] Chang the name of the ports --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt index 1d159ec95..f5bb4bd7c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -1,8 +1,8 @@ package frc.robot.subsystems.climber -const val mainMotor = 0 -const val auxMotor = 1 -const val servo1 = 2 -const val servo2 = 3 -const val Sensor = 4 -const val lockServo = 5 +const val MAIN_MOTOR_ID = 0 +const val AUX_MOTOR_ID = 1 +const val SERVO_1_ID = 2 +const val SERVO_2_ID = 3 +const val SENSOR_ID = 4 +const val LOCK_MOTOR_ID = 5 From 4534977703f7c43f22361416bd801f87cc8c0b5f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 085/253] Latch is servo then change the tolerance to double --- .../frc/robot/subsystems/climber/Climber.kt | 13 ++------ .../subsystems/climber/ClimberConstants.kt | 33 +------------------ 2 files changed, 4 insertions(+), 42 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index b210a1ba6..0e676f0bf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,6 +1,5 @@ package frc.robot.subsystems.climber -import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -13,7 +12,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { @@ -35,13 +34,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) - fun lock(): Command = runOnce({ io.lock() }) - fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() {} - fun fold(){} - private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 77ac78d81..f7cd65c0f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -1,48 +1,17 @@ package frc.robot.subsystems.climber -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.signals.InvertedValue -import com.ctre.phoenix6.signals.NeutralModeValue import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia -import frc.robot.lib.Gains -import frc.robot.lib.selectGainsBasedOnMode const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 const val OPEN_LATCH_POSITION = 0.8 const val CLOSE_LATCH_POSITION = 0.2 -const val LOCK_POSITION = 0.8 -const val UNLOCK_POSITION = 0.2 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 -var MOTOR_CONFIG = TalonFXConfiguration().apply { - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - Inverted = InvertedValue.Clockwise_Positive - } - CurrentLimits.apply { - StatorCurrentLimitEnable = false - SupplyCurrentLimitEnable = false - } - Slot0.apply { - kD = Gains.kD - kP = Gains.kP - kI = Gains.kI - } -} -var Gains = selectGainsBasedOnMode( - Gains( - 0.0, - 0.0, - ), Gains( - 0.0, - 0.0, - ) -) +var LATCH_TOLERANCE = 0.03 \ No newline at end of file From 75d6e25db8179592e7716fe9b0a486a89ec69c04 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 13:56:37 +0200 Subject: [PATCH 086/253] Latch is servo then change the tolerance to double --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 0e676f0bf..be8a77068 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -12,7 +12,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) companion object { From 0665cb5e5cf9d03fe91de5bab56bed95cf2ff806 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:32:05 +0200 Subject: [PATCH 087/253] Accdently been removed. reAdded certain functions --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index be8a77068..b210a1ba6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -34,7 +35,13 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = Commands.runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = Commands.runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() {} + fun fold(){} + private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) } \ No newline at end of file From 9b5bbc524e0b990e14e84906a6b08b2dad1ecead Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:05 +0200 Subject: [PATCH 088/253] Add fold unfold climb and unClimb --- .../frc/robot/subsystems/climber/Climber.kt | 35 +++++++++++++++---- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index b210a1ba6..a71704b00 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -35,13 +35,36 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } } - fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = + runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + + fun openLatch(): Command = + runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() {} - fun fold(){} - private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + fun unfold() { + io.setAngle(UNFOLDED_ANGLE) + } + + fun fold() { + io.setAngle(FOLDED_ANGLE) + } + + fun climb(): Command = + run { closeLatch() } + .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) + .finallyDo(lock().onlyIf(isFolded)) + + fun unClimb(): Command = + run { setPower(-0.4) } + .andThen(unlock()) + .andThen({ unfold() }) + .finallyDo(openLatch()) + + private fun setAngle(angle: Angle): Command = + runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = + runOnce({ io.setPower(power) }) } \ No newline at end of file From 5339bb12c472f2418708d1925c2b0e6c50482e39 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:53:24 +0200 Subject: [PATCH 089/253] spotless Apply --- src/main/kotlin/frc/robot/BuildConstants.java | 10 +- src/main/kotlin/frc/robot/subsystems/Port.kt | 2 +- .../frc/robot/subsystems/climber/Climber.kt | 20 ++-- .../subsystems/climber/ClimberConstants.kt | 5 +- .../frc/robot/subsystems/drive/Drive.java | 107 ++++++------------ .../robot/subsystems/drive/DriveCommands.java | 14 +-- .../frc/robot/subsystems/drive/GyroIO.java | 10 +- .../robot/subsystems/drive/GyroIONavX.java | 5 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 5 +- .../frc/robot/subsystems/drive/Module.java | 52 +++------ .../frc/robot/subsystems/drive/ModuleIO.java | 41 +++---- .../robot/subsystems/drive/ModuleIOSim.java | 6 +- .../subsystems/drive/ModuleIOTalonFX.java | 3 +- .../drive/PhoenixOdometryThread.java | 13 +-- .../subsystems/drive/TunerConstants.java | 72 ++++++------ .../frc/robot/subsystems/vision/Vision.java | 2 - .../subsystems/vision/VisionConstants.java | 9 +- .../frc/robot/subsystems/vision/VisionIO.java | 17 +-- .../subsystems/vision/VisionIOLimelight.java | 17 +-- .../vision/VisionIOPhotonVision.java | 8 +- .../vision/VisionIOPhotonVisionSim.java | 8 +- 21 files changed, 155 insertions(+), 271 deletions(-) diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java index f1081e045..403fdda95 100644 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ b/src/main/kotlin/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Alt-Swerve"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 114; - public static final String GIT_SHA = "d47594e8d8d8aa278bb659bda8a08f688a3f5ce5"; - public static final String GIT_DATE = "2025-01-15 01:04 IST"; + public static final int GIT_REVISION = 142; + public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; + public static final String GIT_DATE = "2025-01-17 15:32 IST"; public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-15 16:48 IST"; - public static final long BUILD_UNIX_TIME = 1736952481060L; + public static final String BUILD_DATE = "2025-01-17 15:51 IST"; + public static final long BUILD_UNIX_TIME = 1737121863309L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt index 8c5768535..7f0cd3fe8 100644 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/Port.kt @@ -9,4 +9,4 @@ object Port { const val Sensor = 4 const val lockServo = 5 } -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index a71704b00..02b78dc0e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,19 +2,24 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } + private var isTouching = Trigger { + inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isLatchClosed = Trigger { + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + } private var isAttached = Trigger(isLatchClosed.and(isTouching)) + private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { @Volatile @@ -29,9 +34,10 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun getInstance(): Climber { - return instance ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) + return instance + ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) } } @@ -67,4 +73,4 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index f7cd65c0f..09a6db6a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 @@ -13,5 +12,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index 8fea22e17..a0e84f1b9 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java @@ -52,11 +52,9 @@ import frc.robot.ConstantsKt; import frc.robot.Mode; import frc.robot.lib.LocalADStarAK; - import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.DoubleSupplier; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -108,17 +106,16 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - @AutoLogOutput - private Rotation2d desiredHeading; + @AutoLogOutput private Rotation2d desiredHeading; private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Rotation2d rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[]{ - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition() + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() }; private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( @@ -221,8 +218,8 @@ public void periodic() { // Log empty setpoint states when disabled if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{}); + Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); } // Update odometry @@ -288,27 +285,21 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - /** - * Runs the drive in a straight line with the specified drive output. - */ + /** Runs the drive in a straight line with the specified drive output. */ public void runCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runCharacterization(output); } } - /** - * NOTE: DO NOT USE WITH TorqueCurrentFOC - */ + /** NOTE: DO NOT USE WITH TorqueCurrentFOC */ public void runTurnCharacterization(double output) { for (int i = 0; i < 4; i++) { modules[i].runTurnCharacterization(output); } } - /** - * Stops the drive. - */ + /** Stops the drive. */ public void stop() { runVelocity(new ChassisSpeeds()); } @@ -326,18 +317,14 @@ public void stopWithX() { stop(); } - /** - * Returns a command to run a quasistatic test in the specified direction. - */ + /** Returns a command to run a quasistatic test in the specified direction. */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) .andThen(sysId.quasistatic(direction)); } - /** - * Returns a command to run a dynamic test in the specified direction. - */ + /** Returns a command to run a dynamic test in the specified direction. */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runCharacterization(0.0)) .withTimeout(1.0) @@ -361,18 +348,14 @@ public Command runAllTurnSysID() { turnSysIdDynamic(SysIdRoutine.Direction.kReverse)); } - /** - * Returns a command to run a dynamic test in the specified direction on the turn modules. - */ + /** Returns a command to run a dynamic test in the specified direction on the turn modules. */ public Command turnSysIdDynamic(SysIdRoutine.Direction direction) { return run(() -> runTurnCharacterization(0.0)) .withTimeout(1.0) .andThen(turnSysId.dynamic(direction)); } - /** - * Returns the module states (turn angles and drive velocities) for all of the modules. - */ + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; @@ -382,9 +365,7 @@ private SwerveModuleState[] getModuleStates() { return states; } - /** - * Returns the module positions (turn angles and drive positions) for all of the modules. - */ + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ private SwerveModulePosition[] getModulePositions() { SwerveModulePosition[] states = new SwerveModulePosition[4]; for (int i = 0; i < 4; i++) { @@ -393,17 +374,13 @@ private SwerveModulePosition[] getModulePositions() { return states; } - /** - * Returns the measured chassis speeds of the robot. - */ + /** Returns the measured chassis speeds of the robot. */ @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") private ChassisSpeeds getChassisSpeeds() { return kinematics.toChassisSpeeds(getModuleStates()); } - /** - * Returns the position of each module in radians. - */ + /** Returns the position of each module in radians. */ public double[] getWheelRadiusCharacterizationPositions() { double[] values = new double[4]; for (int i = 0; i < 4; i++) { @@ -412,9 +389,7 @@ public double[] getWheelRadiusCharacterizationPositions() { return values; } - /** - * Returns the average velocity of the modules in rotations/sec (Phoenix native units). - */ + /** Returns the average velocity of the modules in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { double output = 0.0; for (int i = 0; i < 4; i++) { @@ -423,17 +398,13 @@ public double getFFCharacterizationVelocity() { return output; } - /** - * Returns the current odometry pose. - */ + /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { return poseEstimator.getEstimatedPosition(); } - /** - * Returns the current gyro rotation. - */ + /** Returns the current gyro rotation. */ public Rotation2d getRotation() { return gyroInputs.yawPosition; } @@ -457,9 +428,7 @@ public Command setDesiredHeading(Rotation2d rotation) { return Commands.runOnce(() -> desiredHeading = rotation); } - /** - * Resets the current odometry pose. - */ + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); } @@ -469,9 +438,7 @@ public void resetGyro() { desiredHeading = new Rotation2d(); } - /** - * Adds a new timestamped vision measurement. - */ + /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( Pose2d visionRobotPoseMeters, double timestampSeconds, @@ -480,32 +447,26 @@ public void addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } - /** - * Returns the maximum linear speed in meters per sec. - */ + /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { return TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); } - /** - * Returns the maximum angular speed in radians per sec. - */ + /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { return getMaxLinearSpeedMetersPerSec() / DRIVE_BASE_RADIUS; } - /** - * Returns an array of module translations. - */ + /** Returns an array of module translations. */ public static Translation2d[] getModuleTranslations() { - return new Translation2d[]{ - new Translation2d( - TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), - new Translation2d( - TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), - new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), - new Translation2d( - TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) + return new Translation2d[] { + new Translation2d( + TunerConstants.FrontLeft.LocationX, TunerConstants.FrontLeft.LocationY), + new Translation2d( + TunerConstants.FrontRight.LocationX, TunerConstants.FrontRight.LocationY), + new Translation2d(TunerConstants.BackLeft.LocationX, TunerConstants.BackLeft.LocationY), + new Translation2d( + TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java index 65ef7f8aa..3d59b9b4a 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/DriveCommands.java @@ -27,7 +27,6 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; - import java.text.DecimalFormat; import java.text.NumberFormat; import java.util.LinkedList; @@ -47,8 +46,7 @@ public class DriveCommands { private static final double WHEEL_RADIUS_RAMP_RATE = 0.05; // Rad/Sec^2 private static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.4); - private DriveCommands() { - } + private DriveCommands() {} private static Translation2d getLinearVelocityFromJoysticks(double x, double y) { // Apply deadband @@ -150,7 +148,7 @@ public static Command joystickDriveAtAngle( speeds, isFlipped ? drive.getRotation() - .plus(new Rotation2d(Math.PI)) + .plus(new Rotation2d(Math.PI)) : drive.getRotation())); }, drive) @@ -226,9 +224,7 @@ public static Command feedforwardCharacterization(Drive drive) { })); } - /** - * Measures the robot's wheel radius by spinning in a circle. - */ + /** Measures the robot's wheel radius by spinning in a circle. */ public static Command wheelRadiusCharacterization(Drive drive) { SlewRateLimiter limiter = new SlewRateLimiter(WHEEL_RADIUS_RAMP_RATE); WheelRadiusCharacterizationState state = new WheelRadiusCharacterizationState(); @@ -306,8 +302,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { + formatter.format(wheelRadius) + " meters, " + formatter.format( - Units.metersToInches( - wheelRadius)) + Units.metersToInches( + wheelRadius)) + " inches"); }))); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java index a87dce8e4..1ea03e137 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIO.java @@ -22,13 +22,11 @@ public static class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public double yawVelocityRadPerSec = 0.0; - public double[] odometryYawTimestamps = new double[]{}; - public Rotation2d[] odometryYawPositions = new Rotation2d[]{}; + public double[] odometryYawTimestamps = new double[] {}; + public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; } - public default void zeroGyro() { - } + public default void zeroGyro() {} - public default void updateInputs(GyroIOInputs inputs) { - } + public default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java index 3caa3b63f..3e2c5e9ae 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIONavX.java @@ -17,12 +17,9 @@ import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; - import java.util.Queue; -/** - * IO implementation for NavX. - */ +/** IO implementation for NavX. */ public class GyroIONavX implements GyroIO { private final AHRS navX = new AHRS(NavXComType.kUSB1, (byte) Drive.ODOMETRY_FREQUENCY); private final Queue yawPositionQueue; diff --git a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java index 40d5007c8..f56f370e1 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -22,12 +22,9 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; - import java.util.Queue; -/** - * IO implementation for Pigeon 2. - */ +/** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = new Pigeon2( diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Module.java b/src/main/kotlin/frc/robot/subsystems/drive/Module.java index 538ff2992..06f91457e 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Module.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Module.java @@ -29,13 +29,13 @@ public class Module { private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); private final int index; private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private final Alert turnEncoderDisconnectedAlert; - private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[]{}; + private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; public Module( ModuleIO io, @@ -78,9 +78,7 @@ public void periodic() { turnEncoderDisconnectedAlert.set(!inputs.turnEncoderConnected); } - /** - * Runs the module with the specified setpoint state. Mutates the state to optimize it. - */ + /** Runs the module with the specified setpoint state. Mutates the state to optimize it. */ public void runSetpoint(SwerveModuleState state) { // Optimize velocity setpoint state.optimize(getAngle()); @@ -91,9 +89,7 @@ public void runSetpoint(SwerveModuleState state) { io.setTurnPosition(state.angle); } - /** - * Runs the module with the specified output while controlling to zero degrees. - */ + /** Runs the module with the specified output while controlling to zero degrees. */ public void runCharacterization(double output) { io.setDriveOpenLoop(output); io.setTurnPosition(new Rotation2d()); @@ -104,73 +100,53 @@ public void runTurnCharacterization(double output) { io.setTurnOpenLoop(output); } - /** - * Disables all outputs to motors. - */ + /** Disables all outputs to motors. */ public void stop() { io.setDriveOpenLoop(0.0); io.setTurnOpenLoop(0.0); } - /** - * Returns the current turn angle of the module. - */ + /** Returns the current turn angle of the module. */ public Rotation2d getAngle() { return inputs.turnPosition; } - /** - * Returns the current drive position of the module in meters. - */ + /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { return inputs.drivePositionRad * constants.WheelRadius; } - /** - * Returns the current drive velocity of the module in meters per second. - */ + /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { return inputs.driveVelocityRadPerSec * constants.WheelRadius; } - /** - * Returns the module position (turn angle and drive position). - */ + /** Returns the module position (turn angle and drive position). */ public SwerveModulePosition getPosition() { return new SwerveModulePosition(getPositionMeters(), getAngle()); } - /** - * Returns the module state (turn angle and drive velocity). - */ + /** Returns the module state (turn angle and drive velocity). */ public SwerveModuleState getState() { return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); } - /** - * Returns the module positions received this cycle. - */ + /** Returns the module positions received this cycle. */ public SwerveModulePosition[] getOdometryPositions() { return odometryPositions; } - /** - * Returns the timestamps of the samples received this cycle. - */ + /** Returns the timestamps of the samples received this cycle. */ public double[] getOdometryTimestamps() { return inputs.odometryTimestamps; } - /** - * Returns the module position in radians. - */ + /** Returns the module position in radians. */ public double getWheelRadiusCharacterizationPosition() { return inputs.drivePositionRad; } - /** - * Returns the module velocity in rotations/sec (Phoenix native units). - */ + /** Returns the module velocity in rotations/sec (Phoenix native units). */ public double getFFCharacterizationVelocity() { return Units.radiansToRotations(inputs.driveVelocityRadPerSec); } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java index 649922d28..8a202cd27 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIO.java @@ -34,38 +34,23 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double turnCurrentAmps = 0.0; - public double[] odometryTimestamps = new double[]{}; - public double[] odometryDrivePositionsRad = new double[]{}; - public Rotation2d[] odometryTurnPositions = new Rotation2d[]{}; + public double[] odometryTimestamps = new double[] {}; + public double[] odometryDrivePositionsRad = new double[] {}; + public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; } - /** - * Updates the set of loggable inputs. - */ - public default void updateInputs(ModuleIOInputs inputs) { - } + /** Updates the set of loggable inputs. */ + public default void updateInputs(ModuleIOInputs inputs) {} - /** - * Run the drive motor at the specified open loop value. - */ - public default void setDriveOpenLoop(double output) { - } + /** Run the drive motor at the specified open loop value. */ + public default void setDriveOpenLoop(double output) {} - /** - * Run the turn motor at the specified open loop value. - */ - public default void setTurnOpenLoop(double output) { - } + /** Run the turn motor at the specified open loop value. */ + public default void setTurnOpenLoop(double output) {} - /** - * Run the drive motor at the specified velocity. - */ - public default void setDriveVelocity(double velocityRadPerSec) { - } + /** Run the drive motor at the specified velocity. */ + public default void setDriveVelocity(double velocityRadPerSec) {} - /** - * Run the turn motor to the specified rotation. - */ - public default void setTurnPosition(Rotation2d rotation) { - } + /** Run the turn motor to the specified rotation. */ + public default void setTurnPosition(Rotation2d rotation) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java index a3e2f5f60..1e478376b 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOSim.java @@ -116,9 +116,9 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps()); // Update odometry inputs (50Hz because high-frequency odometry in sim doesn't matter) - inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()}; - inputs.odometryDrivePositionsRad = new double[]{inputs.drivePositionRad}; - inputs.odometryTurnPositions = new Rotation2d[]{inputs.turnPosition}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; + inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override diff --git a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 5c56a83ab..c904649fc 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -37,7 +37,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; - import java.util.Queue; /** @@ -48,7 +47,7 @@ */ public class ModuleIOTalonFX implements ModuleIO { private final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> constants; // Hardware objects diff --git a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 00a2f7069..8958af762 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -18,7 +18,6 @@ import com.ctre.phoenix6.StatusSignal; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.RobotController; - import java.util.ArrayList; import java.util.List; import java.util.Queue; @@ -67,9 +66,7 @@ public void start() { } } - /** - * Registers a Phoenix signal to be read from the thread. - */ + /** Registers a Phoenix signal to be read from the thread. */ public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -87,9 +84,7 @@ public Queue registerSignal(StatusSignal signal) { return queue; } - /** - * Registers a generic signal to be read from the thread. - */ + /** Registers a generic signal to be read from the thread. */ public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); @@ -104,9 +99,7 @@ public Queue registerSignal(DoubleSupplier signal) { return queue; } - /** - * Returns a new queue that returns timestamp values for each sample. - */ + /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); Drive.odometryLock.lock(); diff --git a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java index efd67b134..00988ee62 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/TunerConstants.java @@ -37,16 +37,16 @@ public class TunerConstants { public static SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants(); public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft; public static SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight; public static void init() { @@ -126,8 +126,8 @@ public static void init() { double[] offsets; if (ConstantsKt.getROBORIO_SERIAL_NUMBER().equals(ALT_ROBORIO_SERIAL)) { offsets = - new double[]{ - 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 + new double[] { + 5.393476450205914, 5.3627968344482015, -3.5619033894704586, -1.1965050145508 }; steerGains = @@ -233,11 +233,11 @@ public static void init() { kBackRightYPos = Meters.of(-0.24); } else { offsets = - new double[]{ - -2.9329712664373457, - -1.4818254410975293, - 0.11351457830353745, - 2.0586022173425302 + new double[] { + -2.9329712664373457, + -1.4818254410975293, + 0.11351457830353745, + 2.0586022173425302 }; steerGains = @@ -350,32 +350,32 @@ public static void init() { .withPigeon2Configs(pigeonConfigs); SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, - TalonFXConfiguration, - CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + new SwerveModuleConstantsFactory< + TalonFXConfiguration, + TalonFXConfiguration, + CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); FrontLeft = ConstantCreator.createModuleConstants( diff --git a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java index df73830c6..bb33178bb 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/Vision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/Vision.java @@ -26,10 +26,8 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; - import java.util.LinkedList; import java.util.List; - import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java index 7fa113858..4a4780b71 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionConstants.java @@ -17,7 +17,6 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashMap; import java.util.Map; @@ -70,10 +69,10 @@ public class VisionConstants { // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = - new double[]{ - 1.0, // OV1 - 1.0, // OV2 - 1.0 // OV3 + new double[] { + 1.0, // OV1 + 1.0, // OV2 + 1.0 // OV3 }; // Multipliers to apply for MegaTag 2 observations diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java index 6119adfd2..f2ed010a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIO.java @@ -27,23 +27,17 @@ public static class VisionIOInputs { public int[] tagIds = new int[0]; } - /** - * Represents the angle to a simple target, not used for pose estimation. - */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) { - } + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} - /** - * Represents a robot pose sample used for pose estimation. - */ + /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance, - PoseObservationType type) { - } + PoseObservationType type) {} public static enum PoseObservationType { MEGATAG_1, @@ -51,6 +45,5 @@ public static enum PoseObservationType { PHOTONVISION } - public default void updateInputs(VisionIOInputs inputs) { - } + public default void updateInputs(VisionIOInputs inputs) {} } diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java index 31fbfe682..f40f10141 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -22,16 +22,13 @@ import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.RobotController; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; import java.util.function.Supplier; -/** - * IO implementation for real Limelight hardware. - */ +/** IO implementation for real Limelight hardware. */ public class VisionIOLimelight implements VisionIO { private final Supplier rotationSupplier; private final DoubleArrayPublisher orientationPublisher; @@ -45,7 +42,7 @@ public class VisionIOLimelight implements VisionIO { /** * Creates a new VisionIOLimelight. * - * @param name The configured name of the Limelight. + * @param name The configured name of the Limelight. * @param rotationSupplier Supplier for the current estimated rotation, used for MegaTag 2. */ public VisionIOLimelight(String name, Supplier rotationSupplier) { @@ -56,9 +53,9 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { txSubscriber = table.getDoubleTopic("tx").subscribe(0.0); tySubscriber = table.getDoubleTopic("ty").subscribe(0.0); megatag1Subscriber = - table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = - table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[]{}); + table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); } @Override @@ -75,7 +72,7 @@ public void updateInputs(VisionIOInputs inputs) { // Update orientation for MegaTag 2 orientationPublisher.accept( - new double[]{rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); + new double[] {rotationSupplier.get().getDegrees(), 0.0, 0.0, 0.0, 0.0, 0.0}); NetworkTableInstance.getDefault() .flush(); // Increases network traffic but recommended by Limelight @@ -148,9 +145,7 @@ public void updateInputs(VisionIOInputs inputs) { } } - /** - * Parses the 3D pose from a Limelight botpose array. - */ + /** Parses the 3D pose from a Limelight botpose array. */ private static Pose3d parsePose(double[] rawLLArray) { return new Pose3d( rawLLArray[0], diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java index d36c26eb0..c037b84f9 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -16,17 +16,13 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.HashSet; import java.util.LinkedList; import java.util.List; import java.util.Set; - import org.photonvision.PhotonCamera; -/** - * IO implementation for real PhotonVision hardware. - */ +/** IO implementation for real PhotonVision hardware. */ public class VisionIOPhotonVision implements VisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; @@ -34,7 +30,7 @@ public class VisionIOPhotonVision implements VisionIO { /** * Creates a new VisionIOPhotonVision. * - * @param name The configured name of the camera. + * @param name The configured name of the camera. * @param rotationSupplier The 3D position of the camera relative to the robot. */ public VisionIOPhotonVision(String name, Transform3d robotToCamera) { diff --git a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index 2049e1498..a645bd1a1 100644 --- a/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/kotlin/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -17,16 +17,12 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; - import java.util.function.Supplier; - import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; -/** - * IO implementation for physics sim using PhotonVision simulator. - */ +/** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { private static VisionSystemSim visionSim; @@ -36,7 +32,7 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { /** * Creates a new VisionIOPhotonVisionSim. * - * @param name The name of the camera. + * @param name The name of the camera. * @param poseSupplier Supplier for the robot pose to use in simulation. */ public VisionIOPhotonVisionSim( From abb9f8c995a0d83caf9621edeabe34acad856071 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:56:48 +0200 Subject: [PATCH 090/253] Chang the place and how ports are in climber --- src/main/kotlin/frc/robot/subsystems/Port.kt | 12 ------------ src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 8 ++++++++ 2 files changed, 8 insertions(+), 12 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/Port.kt create mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/Port.kt b/src/main/kotlin/frc/robot/subsystems/Port.kt deleted file mode 100644 index 7f0cd3fe8..000000000 --- a/src/main/kotlin/frc/robot/subsystems/Port.kt +++ /dev/null @@ -1,12 +0,0 @@ -package frc.robot.subsystems - -object Port { - object Climber { - const val mainMotor = 0 - const val auxMotor = 1 - const val servo1 = 2 - const val servo2 = 3 - const val Sensor = 4 - const val lockServo = 5 - } -} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt new file mode 100644 index 000000000..1d159ec95 --- /dev/null +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -0,0 +1,8 @@ +package frc.robot.subsystems.climber + +const val mainMotor = 0 +const val auxMotor = 1 +const val servo1 = 2 +const val servo2 = 3 +const val Sensor = 4 +const val lockServo = 5 From 008099d9f18a4650d0f704bc03566fb547035fd8 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 16:04:45 +0200 Subject: [PATCH 091/253] unTrack build constants --- src/main/kotlin/frc/robot/BuildConstants.java | 19 ------------------- 1 file changed, 19 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/BuildConstants.java diff --git a/src/main/kotlin/frc/robot/BuildConstants.java b/src/main/kotlin/frc/robot/BuildConstants.java deleted file mode 100644 index 403fdda95..000000000 --- a/src/main/kotlin/frc/robot/BuildConstants.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot; - -/** - * Automatically generated file containing build version information. - */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "Alt-Swerve"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 142; - public static final String GIT_SHA = "193f60316747d6ba7613682642ad4080fe9f43ef"; - public static final String GIT_DATE = "2025-01-17 15:32 IST"; - public static final String GIT_BRANCH = "feature/climber/subsystem-io"; - public static final String BUILD_DATE = "2025-01-17 15:51 IST"; - public static final long BUILD_UNIX_TIME = 1737121863309L; - public static final int DIRTY = 1; - - private BuildConstants(){} -} From 3701f8108c3dd78cd56033d17712cc4345be865d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 16:29:10 +0200 Subject: [PATCH 092/253] Add gitignore to BuildConstants --- .gitignore | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.gitignore b/.gitignore index 795a7eb8f..12b767c11 100644 --- a/.gitignore +++ b/.gitignore @@ -164,10 +164,9 @@ annotation/build/ # Simulation GUI and other tools window save file *-window.json -.\src\main\kotlin\frc\robot\BuildConstants.java - simgui* networktables.json /.idea /ctre_sim/ /log-downloader/venv/ +/src/main/kotlin/frc/robot/BuildConstants.java From 7b1d76d7aba46e383418f3b4b295d5f176e0a1e2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:16:56 +0200 Subject: [PATCH 093/253] Chang the name of the ports --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt index 1d159ec95..f5bb4bd7c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -1,8 +1,8 @@ package frc.robot.subsystems.climber -const val mainMotor = 0 -const val auxMotor = 1 -const val servo1 = 2 -const val servo2 = 3 -const val Sensor = 4 -const val lockServo = 5 +const val MAIN_MOTOR_ID = 0 +const val AUX_MOTOR_ID = 1 +const val SERVO_1_ID = 2 +const val SERVO_2_ID = 3 +const val SENSOR_ID = 4 +const val LOCK_MOTOR_ID = 5 From 29f912802d51b3356efdd0a2130681f43b2cc0fe Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:40:09 +0200 Subject: [PATCH 094/253] Add pid to constants --- .../robot/subsystems/climber/ClimberConstants.kt | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 09a6db6a7..e7f1917f7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,7 +2,10 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia +import frc.robot.lib.Gains +import frc.robot.lib.selectGainsBasedOnMode const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 @@ -14,3 +17,14 @@ const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) var LATCH_TOLERANCE = 0.03 +var Gains = selectGainsBasedOnMode( + Gains( + 0.0, + 0.0, + ), Gains( + 0.0, + 0.0, + ) +) + + From bcb4b4b63df88051542ca4add0d61dcc00d970bc Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:54:03 +0200 Subject: [PATCH 095/253] Changed constants to upper case letters --- .../subsystems/climber/ClimberConstants.kt | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e7f1917f7..3d3c8784c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -1,5 +1,8 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance @@ -15,8 +18,23 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) var LATCH_TOLERANCE = 0.03 +var MOTOR_CONFIG = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.Clockwise_Positive + } + CurrentLimits.apply { + StatorCurrentLimitEnable = false + SupplyCurrentLimitEnable = false + } + Slot0.apply { + kD = Gains.kD + kP = Gains.kP + kI = Gains.kI + } +} var Gains = selectGainsBasedOnMode( Gains( 0.0, @@ -26,5 +44,3 @@ var Gains = selectGainsBasedOnMode( 0.0, ) ) - - From 132017215a06305912134877a20abb9e0791fda3 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:24:08 +0200 Subject: [PATCH 096/253] Change the position to be angles --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 ++ .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 3d3c8784c..7b33a131c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -14,6 +14,8 @@ const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 const val OPEN_LATCH_POSITION = 0.8 const val CLOSE_LATCH_POSITION = 0.2 +val UNLOCK_ANGLE = Units.Degree.of(90.0) +val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 0173587e6..fcef29230 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -8,6 +8,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo @@ -42,11 +43,11 @@ class ClimberIOReal:ClimberIO { } override fun lock() { - lockServo.set(ControlMode.Position,LOCK_POSITION) + lockServo.set(ControlMode.Position, LOCK_ANGLE.`in`(Units.Radians)) } override fun unlock() { - lockServo.set(ControlMode.Position, UNLOCK_POSITION) + lockServo.set(ControlMode.Position, UNLOCK_ANGLE.`in`(Units.Radians)) } From d0eb8086f6a25a346e04212a7ef8d699f755a4b2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:37:02 +0200 Subject: [PATCH 097/253] Change ports place nad names --- .../frc/robot/subsystems/climber/ClimberIOReal.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index fcef29230..0be165dee 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -13,15 +13,15 @@ import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo import frc.robot.lib.motors.LinearServo -import frc.robot.subsystems.Port + class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() - var mainMotor = TalonFX(Port.Climber.mainMotor) - var servo1 = LinearServo(Port.Climber.servo1,1,1) - var servo2 = LinearServo(Port.Climber.servo2,1,1) - var auxMotor = TalonFX(Port.Climber.auxMotor) - var lockServo = TalonSRX(Port.Climber.lockServo) + var mainMotor = TalonFX(MAIN_MOTOR_ID) + var servo1 = LinearServo(SERVO_1_ID,1,1) + var servo2 = LinearServo(SERVO_2_ID,1,1) + var auxMotor = TalonFX(AUX_MOTOR_ID) + var lockServo = TalonSRX(LOCK_MOTOR_ID) var dutyCycleOut = DutyCycleOut(0.0) var positionVoltage = PositionVoltage(0.0) From 24aeb37248d345bc78145861f7f927121d1bfdc4 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:30:25 +0200 Subject: [PATCH 098/253] Change motors to val --- .../frc/robot/subsystems/climber/ClimberIOReal.kt | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 0be165dee..e65a13d5f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -17,13 +17,14 @@ import frc.robot.lib.motors.LinearServo class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() - var mainMotor = TalonFX(MAIN_MOTOR_ID) - var servo1 = LinearServo(SERVO_1_ID,1,1) - var servo2 = LinearServo(SERVO_2_ID,1,1) - var auxMotor = TalonFX(AUX_MOTOR_ID) - var lockServo = TalonSRX(LOCK_MOTOR_ID) - var dutyCycleOut = DutyCycleOut(0.0) - var positionVoltage = PositionVoltage(0.0) + val mainMotor = TalonFX(MAIN_MOTOR_ID) + val servo1 = LinearServo(SERVO_1_ID,1,1) + val servo2 = LinearServo(SERVO_2_ID,1,1) + val auxMotor = TalonFX(AUX_MOTOR_ID) + val lockServo = TalonSRX(LOCK_MOTOR_ID) + val dutyCycleOut = DutyCycleOut(0.0) + val positionVoltage = PositionVoltage(0.0) + init { auxMotor.setControl(StrictFollower(mainMotor.deviceID)) From 58c984570636cca3e513580cc648dc37ae4fe1e4 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:30:41 +0200 Subject: [PATCH 099/253] Add sensor to code --- .../robot/subsystems/climber/ClimberIOReal.kt | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index e65a13d5f..4889d18f4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -1,30 +1,30 @@ package frc.robot.subsystems.climber import com.ctre.phoenix.motorcontrol.ControlMode -import com.ctre.phoenix.motorcontrol.NeutralMode import com.ctre.phoenix.motorcontrol.can.TalonSRX -import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.StrictFollower import com.ctre.phoenix6.hardware.TalonFX +import edu.wpi.first.math.filter.MedianFilter import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance -import edu.wpi.first.wpilibj.Servo +import edu.wpi.first.wpilibj.AnalogInput import frc.robot.lib.motors.LinearServo class ClimberIOReal:ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() val mainMotor = TalonFX(MAIN_MOTOR_ID) - val servo1 = LinearServo(SERVO_1_ID,1,1) - val servo2 = LinearServo(SERVO_2_ID,1,1) + val servo1 = LinearServo(SERVO_1_ID, 1, 1) + val servo2 = LinearServo(SERVO_2_ID, 1, 1) val auxMotor = TalonFX(AUX_MOTOR_ID) val lockServo = TalonSRX(LOCK_MOTOR_ID) val dutyCycleOut = DutyCycleOut(0.0) val positionVoltage = PositionVoltage(0.0) - + val sensor = AnalogInput(SENSOR_ID) + private val distanceFilter = MedianFilter(3) + private val distance = AnalogInput(0) init { auxMotor.setControl(StrictFollower(mainMotor.deviceID)) @@ -56,5 +56,11 @@ class ClimberIOReal:ClimberIO { inputs.angle = mainMotor.position.value inputs.appliedVoltage = mainMotor.supplyVoltage.value inputs.latchPosition = servo1.position + var calculatedDistance = distanceFilter.calculate(4800 / (200 * distance.getVoltage() - 20.0)) + if (calculatedDistance < 0) { + calculatedDistance = 80.0 + } + + inputs.sensorDistance = Units.Meters.of(calculatedDistance) } } \ No newline at end of file From 23ec475ef8550c65162a27d440a09a6b38efae63 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:34:27 +0200 Subject: [PATCH 100/253] Change names for servos --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt index f5bb4bd7c..194b9e8dd 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -2,7 +2,7 @@ package frc.robot.subsystems.climber const val MAIN_MOTOR_ID = 0 const val AUX_MOTOR_ID = 1 -const val SERVO_1_ID = 2 -const val SERVO_2_ID = 3 +const val LATCH_SERVO_ID = 2 +const val FOLLOW_LATCH_SERVO_ID = 3 const val SENSOR_ID = 4 const val LOCK_MOTOR_ID = 5 From 0e0f75b2336d198c22f2c0d38c64c3f734ac8957 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 101/253] Change latchposition to Dimention less --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 3 ++- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 02b78dc0e..6651a0f43 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -16,7 +17,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition < Units.Percent.of(LATCH_TOLERANCE + CLOSE_LATCH_POSITION) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 866afccaf..99d413a03 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged @@ -19,7 +20,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Double = 0.0 + var latchPosition: Dimensionless = Units.Percent.of(0.0) var sensorDistance: Distance = Units.Centimeter.zero() } } From 543b2bc20e8de51726b0f3e300682cf8ce20408a Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 102/253] Change latchposition to Dimention less --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 3 ++- .../frc/robot/subsystems/climber/ClimberConstants.kt | 7 ++++--- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 6651a0f43..bb25ac2a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger @@ -17,7 +18,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { - inputs.latchPosition < Units.Percent.of(LATCH_TOLERANCE + CLOSE_LATCH_POSITION) + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 09a6db6a7..2daf6a02e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,15 +2,16 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -const val OPEN_LATCH_POSITION = 0.8 -const val CLOSE_LATCH_POSITION = 0.2 +val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) +val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 +var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 99d413a03..c018126ac 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -9,7 +9,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Double) {} + fun setLatchPosition(position: Dimensionless) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} From ccd106e73e66fb8cad2bd17fe76f7f281fb85cd0 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:59:19 +0200 Subject: [PATCH 103/253] Reduce the code repitition. --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bb25ac2a7..123a4378f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,6 +1,5 @@ package frc.robot.subsystems.climber -import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command @@ -44,10 +43,12 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun closeLatch(): Command = - runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) + + private fun setLatchPose(latchPose: Dimensionless) = io.setLatchPosition(latchPose) fun openLatch(): Command = - runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) From 958ca3aaf27dbcf3cdacf2ba91bc2e546d621d0c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:21:21 +0200 Subject: [PATCH 104/253] Change the way it constuct --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 123a4378f..afb3f4521 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput -class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { +class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput From b824e8d9591e57f8f2b55b6a11a56cdb42e795f8 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:22:53 +0200 Subject: [PATCH 105/253] Change it from lt to < --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index afb3f4521..1a8cc51a0 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -13,7 +13,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { - inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + inputs.sensorDistance < DISTANCE_THRESHOLD } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { From ecf96e2a143ec2c56ddded8898b0dc3162602e68 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:23:44 +0200 Subject: [PATCH 106/253] Change it from lt to is near --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 1a8cc51a0..758d773c3 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,7 +15,11 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } - private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + + @AutoLogOutput + private val hasClimbed = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_ANGLE_TOLERANCE) } + + @AutoLogOutput private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } From b15800b5033d42aba1ebbf30740291f6aad79629 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:25:58 +0200 Subject: [PATCH 107/253] Change it to be outside of the brackets, Add autoLog --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 758d773c3..7e4882597 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -23,7 +23,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } - private var isAttached = Trigger(isLatchClosed.and(isTouching)) + + @AutoLogOutput + private var isAttached = Trigger(isLatchClosed).and(isTouching) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { From 8319afab357873b738db9c6b0e6621ec32fdad24 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:29:41 +0200 Subject: [PATCH 108/253] Made isLatchClosed work with isnear --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 7e4882597..febf536e4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -21,7 +21,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) } @AutoLogOutput From ae8f4b2e7c994392165a7c91ced22ed496a77cd2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:35:41 +0200 Subject: [PATCH 109/253] Rename hasClimbed to isFolded --- .../frc/robot/subsystems/climber/Climber.kt | 27 ++----------------- 1 file changed, 2 insertions(+), 25 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index febf536e4..f6946a066 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,10 +15,6 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } - - @AutoLogOutput - private val hasClimbed = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_ANGLE_TOLERANCE) } - @AutoLogOutput private var isLatchClosed = Trigger { inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) @@ -26,27 +22,8 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isAttached = Trigger(isLatchClosed).and(isTouching) - private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } - - companion object { - @Volatile - private var instance: Climber? = null - - fun initialize(io: ClimberIO) { - synchronized(this) { - if (instance == null) { - instance = Climber(io) - } - } - } - - fun getInstance(): Climber { - return instance - ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) - } - } + @AutoLogOutput + private val isFolded = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) From 6f610d8146abada95faea250793e5cc08f24b069 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:38:28 +0200 Subject: [PATCH 110/253] Change finallyDo to AndThen --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index f6946a066..c714f99e8 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -52,7 +52,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { run { setPower(-0.4) } .andThen(unlock()) .andThen({ unfold() }) - .finallyDo(openLatch()) + .andThen(openLatch()) private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) From e87fc890c799d26915092173eae95e537f90f809 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:50:10 +0200 Subject: [PATCH 111/253] Change LatchPose and use Until to time each of the steps in climb --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index c714f99e8..bedad7d0f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -44,12 +44,14 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run { closeLatch() } - .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) - .finallyDo(lock().onlyIf(isFolded)) + run {setLatchPose(CLOSE_LATCH_POSITION)} + .until(isLatchClosed) + .andThen(setAngle(FOLDED_ANGLE)) + .until(isFolded) + .andThen(lock()) fun unClimb(): Command = - run { setPower(-0.4) } + run {setLatchPose(CLOSE_LATCH_POSITION)} .andThen(unlock()) .andThen({ unfold() }) .andThen(openLatch()) From 3eb0d62963cbf80d3e397490f0d6c78971a4e625 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:51:30 +0200 Subject: [PATCH 112/253] Add periodic and update the inputs --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bedad7d0f..38758678d 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput +import org.littletonrobotics.junction.Logger class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @@ -61,4 +62,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + + override fun periodic() { + io.updateInput() + Logger.processInputs(this::class.simpleName, io.inputs) + } } From 3414d60b559d5f7137a56c5fac509ff19d32b75d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:53:27 +0200 Subject: [PATCH 113/253] Change Folded tolerance to degrees --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2daf6a02e..2cde8b969 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,7 +11,7 @@ val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val GEAR_RATIO = 1.0 +val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From f707ba4e4f543219269b618aeaa41245925bf4c3 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:54:11 +0200 Subject: [PATCH 114/253] spotlessApply --- .../frc/robot/subsystems/climber/Climber.kt | 21 ++++++++++--------- .../subsystems/climber/ClimberConstants.kt | 4 ++-- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 38758678d..801534ed9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -5,7 +5,6 @@ import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger -import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger @@ -18,21 +17,23 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } @AutoLogOutput private var isAttached = Trigger(isLatchClosed).and(isTouching) + @AutoLogOutput - private val isFolded = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } + private val isFolded = Trigger { + inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) + } - fun closeLatch(): Command = - runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - private fun setLatchPose(latchPose: Dimensionless) = io.setLatchPosition(latchPose) + private fun setLatchPose(latchPose: Dimensionless) = + io.setLatchPosition(latchPose) - fun openLatch(): Command = - runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) @@ -45,14 +46,14 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run {setLatchPose(CLOSE_LATCH_POSITION)} + run { setLatchPose(CLOSE_LATCH_POSITION) } .until(isLatchClosed) .andThen(setAngle(FOLDED_ANGLE)) .until(isFolded) .andThen(lock()) fun unClimb(): Command = - run {setLatchPose(CLOSE_LATCH_POSITION)} + run { setLatchPose(CLOSE_LATCH_POSITION) } .andThen(unlock()) .andThen({ unfold() }) .andThen(openLatch()) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2cde8b969..2a4b02598 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -13,5 +13,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) +val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From 09f50461a8ef382e3332f31200ffbd01da75004d Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:55:02 +0200 Subject: [PATCH 115/253] Changed constants to upper case letters --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 824e30304..fecc2836a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,12 +2,13 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -const val OPEN_LATCH_POSITION = 0.8 -const val CLOSE_LATCH_POSITION = 0.2 +val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) +val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNLOCK_ANGLE = Units.Degree.of(90.0) val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) @@ -16,4 +17,4 @@ const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA_MAIN: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From 5c60ede4b655359da43eb47d9d63d72d5c26d4eb Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:34:27 +0200 Subject: [PATCH 116/253] Change names for servos --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt index f5bb4bd7c..194b9e8dd 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt @@ -2,7 +2,7 @@ package frc.robot.subsystems.climber const val MAIN_MOTOR_ID = 0 const val AUX_MOTOR_ID = 1 -const val SERVO_1_ID = 2 -const val SERVO_2_ID = 3 +const val LATCH_SERVO_ID = 2 +const val FOLLOW_LATCH_SERVO_ID = 3 const val SENSOR_ID = 4 const val LOCK_MOTOR_ID = 5 From 304e42f7e360a2c31a18fad3bcf35a6a72a15e82 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 117/253] Change latchposition to Dimention less --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 3 ++- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 02b78dc0e..6651a0f43 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -16,7 +17,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition < Units.Percent.of(LATCH_TOLERANCE + CLOSE_LATCH_POSITION) } private var isAttached = Trigger(isLatchClosed.and(isTouching)) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 866afccaf..99d413a03 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged @@ -19,7 +20,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Double = 0.0 + var latchPosition: Dimensionless = Units.Percent.of(0.0) var sensorDistance: Distance = Units.Centimeter.zero() } } From eb4ff57c4cebcd29b258bc4b893523cf7d0b3c16 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 118/253] Change latchposition to Dimention less --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 3 ++- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 6651a0f43..bb25ac2a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger @@ -17,7 +18,7 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { - inputs.latchPosition < Units.Percent.of(LATCH_TOLERANCE + CLOSE_LATCH_POSITION) + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } private var isAttached = Trigger(isLatchClosed.and(isTouching)) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 99d413a03..c018126ac 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -9,7 +9,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Double) {} + fun setLatchPosition(position: Dimensionless) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} From 6b069be2beb047fe112432da9632aaee66099d6f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:59:19 +0200 Subject: [PATCH 119/253] Reduce the code repitition. --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bb25ac2a7..123a4378f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,6 +1,5 @@ package frc.robot.subsystems.climber -import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command @@ -44,10 +43,12 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun closeLatch(): Command = - runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) + + private fun setLatchPose(latchPose: Dimensionless) = io.setLatchPosition(latchPose) fun openLatch(): Command = - runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) From 3e0f8fefc3002e475916ba53014c98f84f2aad28 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:21:21 +0200 Subject: [PATCH 120/253] Change the way it constuct --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 123a4378f..afb3f4521 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput -class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { +class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput From cb8921d15475b57680e9860cbb05a4bd525edef9 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:22:53 +0200 Subject: [PATCH 121/253] Change it from lt to < --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index afb3f4521..1a8cc51a0 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -13,7 +13,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isTouching = Trigger { - inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + inputs.sensorDistance < DISTANCE_THRESHOLD } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } private var isLatchClosed = Trigger { From 457491fdcdf1f060e35f4dc1a281489abf893d8c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:23:44 +0200 Subject: [PATCH 122/253] Change it from lt to is near --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 1a8cc51a0..758d773c3 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,7 +15,11 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } - private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + + @AutoLogOutput + private val hasClimbed = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_ANGLE_TOLERANCE) } + + @AutoLogOutput private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } From 3268aac6d307c7a84944b819c107372da4902723 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:25:58 +0200 Subject: [PATCH 123/253] Change it to be outside of the brackets, Add autoLog --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 758d773c3..7e4882597 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -23,7 +23,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } - private var isAttached = Trigger(isLatchClosed.and(isTouching)) + + @AutoLogOutput + private var isAttached = Trigger(isLatchClosed).and(isTouching) private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { From 9ee58af2369b7f2c9456593f10c200ec8f31f3bd Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:29:41 +0200 Subject: [PATCH 124/253] Made isLatchClosed work with isnear --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 7e4882597..febf536e4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -21,7 +21,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) } @AutoLogOutput From 127048a1f4346102175dd1ed91f2932f229ef085 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:35:41 +0200 Subject: [PATCH 125/253] Rename hasClimbed to isFolded --- .../frc/robot/subsystems/climber/Climber.kt | 27 ++----------------- 1 file changed, 2 insertions(+), 25 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index febf536e4..f6946a066 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,10 +15,6 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } - - @AutoLogOutput - private val hasClimbed = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_ANGLE_TOLERANCE) } - @AutoLogOutput private var isLatchClosed = Trigger { inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) @@ -26,27 +22,8 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isAttached = Trigger(isLatchClosed).and(isTouching) - private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } - - companion object { - @Volatile - private var instance: Climber? = null - - fun initialize(io: ClimberIO) { - synchronized(this) { - if (instance == null) { - instance = Climber(io) - } - } - } - - fun getInstance(): Climber { - return instance - ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) - } - } + @AutoLogOutput + private val isFolded = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) From 34a27ea715feb44863ed012e54b74a5688975100 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:38:28 +0200 Subject: [PATCH 126/253] Change finallyDo to AndThen --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index f6946a066..c714f99e8 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -52,7 +52,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { run { setPower(-0.4) } .andThen(unlock()) .andThen({ unfold() }) - .finallyDo(openLatch()) + .andThen(openLatch()) private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) From 74314f7840f5ac789f4cfb68f69a86da7e04ced0 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:50:10 +0200 Subject: [PATCH 127/253] Change LatchPose and use Until to time each of the steps in climb --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index c714f99e8..bedad7d0f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -44,12 +44,14 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run { closeLatch() } - .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) - .finallyDo(lock().onlyIf(isFolded)) + run {setLatchPose(CLOSE_LATCH_POSITION)} + .until(isLatchClosed) + .andThen(setAngle(FOLDED_ANGLE)) + .until(isFolded) + .andThen(lock()) fun unClimb(): Command = - run { setPower(-0.4) } + run {setLatchPose(CLOSE_LATCH_POSITION)} .andThen(unlock()) .andThen({ unfold() }) .andThen(openLatch()) From 0d534d6aa111c944b00a9a1996a850f45070b9ce Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:51:30 +0200 Subject: [PATCH 128/253] Add periodic and update the inputs --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bedad7d0f..38758678d 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput +import org.littletonrobotics.junction.Logger class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @@ -61,4 +62,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + + override fun periodic() { + io.updateInput() + Logger.processInputs(this::class.simpleName, io.inputs) + } } From 7b54626774921ee4b95c4391bf7641c32296bfe8 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:54:11 +0200 Subject: [PATCH 129/253] spotlessApply --- .../frc/robot/subsystems/climber/Climber.kt | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 38758678d..801534ed9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -5,7 +5,6 @@ import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger -import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger @@ -18,21 +17,23 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION,LATCH_TOLERANCE) + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } @AutoLogOutput private var isAttached = Trigger(isLatchClosed).and(isTouching) + @AutoLogOutput - private val isFolded = Trigger { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } + private val isFolded = Trigger { + inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) + } - fun closeLatch(): Command = - runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - private fun setLatchPose(latchPose: Dimensionless) = io.setLatchPosition(latchPose) + private fun setLatchPose(latchPose: Dimensionless) = + io.setLatchPosition(latchPose) - fun openLatch(): Command = - runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) @@ -45,14 +46,14 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run {setLatchPose(CLOSE_LATCH_POSITION)} + run { setLatchPose(CLOSE_LATCH_POSITION) } .until(isLatchClosed) .andThen(setAngle(FOLDED_ANGLE)) .until(isFolded) .andThen(lock()) fun unClimb(): Command = - run {setLatchPose(CLOSE_LATCH_POSITION)} + run { setLatchPose(CLOSE_LATCH_POSITION) } .andThen(unlock()) .andThen({ unfold() }) .andThen(openLatch()) From 81ba648af7f612dcfe6680ca4111b186985b6f74 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:12:34 +0200 Subject: [PATCH 130/253] Add moment of inertia for lock motor --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index fecc2836a..e2d17d354 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,19 +2,20 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) +val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNLOCK_ANGLE = Units.Degree.of(90.0) val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 +val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA_MAIN: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From 0fcd3bcb0c377cde02dc03c9700acc90ff453ff0 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 20:14:44 +0200 Subject: [PATCH 131/253] Change to dimentionless --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 6e20ed1e9..f5682eb51 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -6,6 +6,7 @@ import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.math.system.plant.LinearSystemId import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.simulation.DCMotorSim @@ -31,7 +32,7 @@ class ClimberIOSim : ClimberIO { var positionControler = PositionVoltage(0.0) - override fun setLatchPosition(position: Double) { + override fun setLatchPosition(position: Dimensionless) { if (Double.equals(OPEN_LATCH_POSITION)) { listOf(servo2, servo1).forEach { it.output = true } } else { From 35d2bd915121b9b1bf1ec77b3017e461aa6a83b0 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 20:28:14 +0200 Subject: [PATCH 132/253] Spotless apply --- .../robot/subsystems/climber/ClimberIOSim.kt | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index f5682eb51..ea4df1180 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -17,21 +17,27 @@ import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedInputClimber = LoggedInputClimber() var motor = - TalonFXSim(2, GEAR_RATIO, MOMENT_OF_INERTIA_MAIN.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN) + TalonFXSim( + 2, + GEAR_RATIO, + MOMENT_OF_INERTIA_MAIN.`in`(Units.KilogramSquareMeters), + 1.0, + TalonType.KRAKEN + ) var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) - val lockMotor = DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getBag(1), - MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), - 1.0 - ), - DCMotor.getBag(1) - ) + val lockMotor = + DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getBag(1), + MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), + 1.0 + ), + DCMotor.getBag(1) + ) var dutyCycle = DutyCycleOut(0.0) var positionControler = PositionVoltage(0.0) - override fun setLatchPosition(position: Dimensionless) { if (Double.equals(OPEN_LATCH_POSITION)) { listOf(servo2, servo1).forEach { it.output = true } @@ -67,4 +73,4 @@ class ClimberIOSim : ClimberIO { inputs.latchPosition = CLOSE_LATCH_POSITION } } -} \ No newline at end of file +} From 65bc208389b69ec8059806d0b2adceec4bed8544 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:54:08 +0200 Subject: [PATCH 133/253] Take tunerConstants init out of `when` statements --- src/main/kotlin/frc/robot/Robot.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/Robot.kt b/src/main/kotlin/frc/robot/Robot.kt index 67c9da7e2..2b1203ceb 100644 --- a/src/main/kotlin/frc/robot/Robot.kt +++ b/src/main/kotlin/frc/robot/Robot.kt @@ -74,7 +74,6 @@ object Robot : LoggedRobot() { ) Logger.addDataReceiver(WPILOGWriter()) Logger.addDataReceiver(NT4Publisher()) - TunerConstants.init() } SIM -> Logger.addDataReceiver(NT4Publisher()) REPLAY -> { @@ -88,6 +87,7 @@ object Robot : LoggedRobot() { } Logger.start() + TunerConstants.init() RobotContainer // Initialize robot container. compressor.enableDigital() From d719bede6f31bd40c680a36518b28e7673bcde21 Mon Sep 17 00:00:00 2001 From: Emma Date: Thu, 16 Jan 2025 23:30:29 +0200 Subject: [PATCH 134/253] Fix getAppliedVoltage --- src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java index ac5313e32..1b9aa8500 100644 --- a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java +++ b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java @@ -101,6 +101,6 @@ public double getAppliedCurrent() { } public double getAppliedVoltage() { - return voltageRequest.getAsDouble(); + return motorSim.getInputVoltage(); } } From 6d86e840342659e54339a97792fcd1154b2f7fb2 Mon Sep 17 00:00:00 2001 From: Emma Date: Thu, 16 Jan 2025 23:20:56 +0200 Subject: [PATCH 135/253] Add extension functions to convert linear to rotational measures and vice versa --- .../kotlin/frc/robot/lib/ExtensionFunctions.kt | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index 76619dcc8..6f41c8622 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -7,9 +7,13 @@ import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Rotation3d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.WrapperCommand import frc.robot.IS_RED +import kotlin.math.PI import kotlin.math.hypot import org.littletonrobotics.junction.LogTable @@ -96,3 +100,13 @@ fun Translation2d.flipIfNeeded(): Translation2d = fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this) fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this + +fun Distance.toAngle(radius: Distance): Angle = + Units.Degree.of( + this.`in`(Units.Meters) / (2 * PI * radius.`in`(Units.Meters)) + ) + +fun Angle.toDistance(radius: Distance): Distance = + Units.Meters.of( + this.`in`(Units.Rotations) * 2 * PI * radius.`in`(Units.Meters) + ) From 81d2d2123f44ed30ddb448b76b105d3be19141c8 Mon Sep 17 00:00:00 2001 From: Emma Date: Fri, 17 Jan 2025 01:45:04 +0200 Subject: [PATCH 136/253] Fix Distance.toAngle() using degrees instead of rotations --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index 6f41c8622..e079c52b8 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -102,7 +102,7 @@ fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this) fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this fun Distance.toAngle(radius: Distance): Angle = - Units.Degree.of( + Units.Rotations.of( this.`in`(Units.Meters) / (2 * PI * radius.`in`(Units.Meters)) ) From 44a5c65c6287f0f5be06a7fd1f1d3bf31405f932 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Fri, 17 Jan 2025 15:53:52 +0200 Subject: [PATCH 137/253] Simplify Angle.toDistance() --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index e079c52b8..bca163043 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -13,9 +13,9 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.WrapperCommand import frc.robot.IS_RED +import org.littletonrobotics.junction.LogTable import kotlin.math.PI import kotlin.math.hypot -import org.littletonrobotics.junction.LogTable fun ChassisSpeeds.getSpeed() = hypot(vxMetersPerSecond, vyMetersPerSecond) @@ -107,6 +107,4 @@ fun Distance.toAngle(radius: Distance): Angle = ) fun Angle.toDistance(radius: Distance): Distance = - Units.Meters.of( - this.`in`(Units.Rotations) * 2 * PI * radius.`in`(Units.Meters) - ) + radius.times(this.`in`(Units.Rotations) * 2 * PI) From 330803d7fa41fe5bbb53cbfe002241427f0c897a Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Fri, 17 Jan 2025 16:03:33 +0200 Subject: [PATCH 138/253] Simplify Angle.toDistance() again --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index bca163043..ed8b3be42 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -107,4 +107,4 @@ fun Distance.toAngle(radius: Distance): Angle = ) fun Angle.toDistance(radius: Distance): Distance = - radius.times(this.`in`(Units.Rotations) * 2 * PI) + radius * this.`in`(Units.Rotations) * 2.0 * PI From d2bef3565071dd4724ec81c098b73b83d15aa131 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 20:55:02 +0200 Subject: [PATCH 139/253] Changed constants to upper case letters --- .../robot/subsystems/climber/ClimberConstants.kt | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e2d17d354..47ece7d39 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,20 +2,16 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless +import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) -val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) -val UNLOCK_ANGLE = Units.Degree.of(90.0) -val LOCK_ANGLE = Units.Degree.of(10.0) +val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) +val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 -val FOLDED_TOLERANCE = Units.Degree.of(1.0) -val MOMENT_OF_INERTIA_MAIN: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file From b21d65df5f3b7e0f6ab597ff535a57b9bd5a0ad6 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Fri, 17 Jan 2025 15:32:05 +0200 Subject: [PATCH 140/253] Accdently been removed. reAdded certain functions --- .../frc/robot/subsystems/climber/Climber.kt | 86 +++++++------------ 1 file changed, 31 insertions(+), 55 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 801534ed9..b210a1ba6 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,71 +1,47 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import org.littletonrobotics.junction.AutoLogOutput -import org.littletonrobotics.junction.Logger -class Climber(private val io: ClimberIO) : SubsystemBase() { +class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { - inputs.sensorDistance < DISTANCE_THRESHOLD + private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } + private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isAttached = Trigger(isLatchClosed.and(isTouching)) + + companion object { + @Volatile + private var instance: Climber? = null + + fun initialize(io: ClimberIO) { + synchronized(this) { + if (instance == null) { + instance = Climber(io) + } + } + } + + fun getInstance(): Climber { + return instance ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) + } } - @AutoLogOutput - private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) - } - - @AutoLogOutput - private var isAttached = Trigger(isLatchClosed).and(isTouching) - - @AutoLogOutput - private val isFolded = Trigger { - inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) - } - - fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - - private fun setLatchPose(latchPose: Dimensionless) = - io.setLatchPosition(latchPose) - - fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() { - io.setAngle(UNFOLDED_ANGLE) - } - - fun fold() { - io.setAngle(FOLDED_ANGLE) - } - - fun climb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } - .until(isLatchClosed) - .andThen(setAngle(FOLDED_ANGLE)) - .until(isFolded) - .andThen(lock()) + fun unfold() {} + fun fold(){} + private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) + private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) - fun unClimb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } - .andThen(unlock()) - .andThen({ unfold() }) - .andThen(openLatch()) - - private fun setAngle(angle: Angle): Command = - runOnce({ io.setAngle(angle) }) - - private fun setPower(power: Double): Command = - runOnce({ io.setPower(power) }) - - override fun periodic() { - io.updateInput() - Logger.processInputs(this::class.simpleName, io.inputs) - } -} +} \ No newline at end of file From 552bc94c405c3e4a253992feec2ecad560f7dfaf Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 141/253] Change latchposition to Dimention less --- .../frc/robot/subsystems/climber/Climber.kt | 57 ++++++++++++++----- .../subsystems/climber/ClimberConstants.kt | 10 ++-- 2 files changed, 49 insertions(+), 18 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index b210a1ba6..bb25ac2a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,20 +1,27 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(DISTANCE_THRESHOLD) } + private var isTouching = Trigger { + inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION } + private var isLatchClosed = Trigger { + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + } private var isAttached = Trigger(isLatchClosed.and(isTouching)) + private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { @Volatile @@ -29,19 +36,43 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun getInstance(): Climber { - return instance ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) + return instance + ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) } } - fun closeLatch(): Command = runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun closeLatch(): Command = + runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + + fun openLatch(): Command = + runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() {} - fun fold(){} - private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) - private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + fun unfold() { + io.setAngle(UNFOLDED_ANGLE) + } + + fun fold() { + io.setAngle(FOLDED_ANGLE) + } + + fun climb(): Command = + run { closeLatch() } + .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) + .finallyDo(lock().onlyIf(isFolded)) + + fun unClimb(): Command = + run { setPower(-0.4) } + .andThen(unlock()) + .andThen({ unfold() }) + .finallyDo(openLatch()) + + private fun setAngle(angle: Angle): Command = + runOnce({ io.setAngle(angle) }) -} \ No newline at end of file + private fun setPower(power: Double): Command = + runOnce({ io.setPower(power) }) +} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 47ece7d39..2daf6a02e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,16 +2,16 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Distance = Units.Centimeter.of(0.8) -val CLOSE_LATCH_POSITION: Distance = Units.Centimeter.of(0.2) +val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) +val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Distance = Units.Centimeter.of(0.4) \ No newline at end of file +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From 8dbfee250bf678ad0356944e5725a44c89e1adc2 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:53:27 +0200 Subject: [PATCH 142/253] Change Folded tolerance to degrees --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2daf6a02e..2cde8b969 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,7 +11,7 @@ val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -const val GEAR_RATIO = 1.0 +val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From 946b2016084d02bb6279a126e6e02d4c6056985f Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:54:11 +0200 Subject: [PATCH 143/253] spotlessApply --- .../frc/robot/subsystems/climber/Climber.kt | 61 ++++++++----------- .../subsystems/climber/ClimberConstants.kt | 4 +- 2 files changed, 29 insertions(+), 36 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bb25ac2a7..801534ed9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,53 +1,39 @@ package frc.robot.subsystems.climber -import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger -import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput +import org.littletonrobotics.junction.Logger -class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { +class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput private var isTouching = Trigger { - inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + inputs.sensorDistance < DISTANCE_THRESHOLD } - private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } - private var isAttached = Trigger(isLatchClosed.and(isTouching)) - private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } - - companion object { - @Volatile - private var instance: Climber? = null - fun initialize(io: ClimberIO) { - synchronized(this) { - if (instance == null) { - instance = Climber(io) - } - } - } + @AutoLogOutput + private var isAttached = Trigger(isLatchClosed).and(isTouching) - fun getInstance(): Climber { - return instance - ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) - } + @AutoLogOutput + private val isFolded = Trigger { + inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } - fun closeLatch(): Command = - runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = - runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + private fun setLatchPose(latchPose: Dimensionless) = + io.setLatchPosition(latchPose) + + fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) @@ -60,19 +46,26 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run { closeLatch() } - .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) - .finallyDo(lock().onlyIf(isFolded)) + run { setLatchPose(CLOSE_LATCH_POSITION) } + .until(isLatchClosed) + .andThen(setAngle(FOLDED_ANGLE)) + .until(isFolded) + .andThen(lock()) fun unClimb(): Command = - run { setPower(-0.4) } + run { setLatchPose(CLOSE_LATCH_POSITION) } .andThen(unlock()) .andThen({ unfold() }) - .finallyDo(openLatch()) + .andThen(openLatch()) private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + + override fun periodic() { + io.updateInput() + Logger.processInputs(this::class.simpleName, io.inputs) + } } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2cde8b969..2a4b02598 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -13,5 +13,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) +val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From dca9f8da6834b7048262320c972d544ef9f5bc41 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 15 Jan 2025 16:54:08 +0200 Subject: [PATCH 144/253] Take tunerConstants init out of `when` statements --- src/main/kotlin/frc/robot/Robot.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/Robot.kt b/src/main/kotlin/frc/robot/Robot.kt index 67c9da7e2..2b1203ceb 100644 --- a/src/main/kotlin/frc/robot/Robot.kt +++ b/src/main/kotlin/frc/robot/Robot.kt @@ -74,7 +74,6 @@ object Robot : LoggedRobot() { ) Logger.addDataReceiver(WPILOGWriter()) Logger.addDataReceiver(NT4Publisher()) - TunerConstants.init() } SIM -> Logger.addDataReceiver(NT4Publisher()) REPLAY -> { @@ -88,6 +87,7 @@ object Robot : LoggedRobot() { } Logger.start() + TunerConstants.init() RobotContainer // Initialize robot container. compressor.enableDigital() From 0f04140c3f60576b955290ecbf8b52bc67b283b2 Mon Sep 17 00:00:00 2001 From: Emma Date: Thu, 16 Jan 2025 23:30:29 +0200 Subject: [PATCH 145/253] Fix getAppliedVoltage --- src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java index ac5313e32..1b9aa8500 100644 --- a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java +++ b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java @@ -101,6 +101,6 @@ public double getAppliedCurrent() { } public double getAppliedVoltage() { - return voltageRequest.getAsDouble(); + return motorSim.getInputVoltage(); } } From 1940a57a1a9c55810477c76ea1d800cd61bedd2c Mon Sep 17 00:00:00 2001 From: Emma Date: Thu, 16 Jan 2025 23:20:56 +0200 Subject: [PATCH 146/253] Add extension functions to convert linear to rotational measures and vice versa --- .../kotlin/frc/robot/lib/ExtensionFunctions.kt | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index 76619dcc8..6f41c8622 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -7,9 +7,13 @@ import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Rotation3d import edu.wpi.first.math.geometry.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.WrapperCommand import frc.robot.IS_RED +import kotlin.math.PI import kotlin.math.hypot import org.littletonrobotics.junction.LogTable @@ -96,3 +100,13 @@ fun Translation2d.flipIfNeeded(): Translation2d = fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this) fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this + +fun Distance.toAngle(radius: Distance): Angle = + Units.Degree.of( + this.`in`(Units.Meters) / (2 * PI * radius.`in`(Units.Meters)) + ) + +fun Angle.toDistance(radius: Distance): Distance = + Units.Meters.of( + this.`in`(Units.Rotations) * 2 * PI * radius.`in`(Units.Meters) + ) From 8f9111f3808ae0a4d5c87a2056e86d115f16d424 Mon Sep 17 00:00:00 2001 From: Emma Date: Fri, 17 Jan 2025 01:45:04 +0200 Subject: [PATCH 147/253] Fix Distance.toAngle() using degrees instead of rotations --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index 6f41c8622..e079c52b8 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -102,7 +102,7 @@ fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this) fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this fun Distance.toAngle(radius: Distance): Angle = - Units.Degree.of( + Units.Rotations.of( this.`in`(Units.Meters) / (2 * PI * radius.`in`(Units.Meters)) ) From baa656ddf2dfd6f36bd9fccc2f798ea4ecb462b1 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Fri, 17 Jan 2025 15:53:52 +0200 Subject: [PATCH 148/253] Simplify Angle.toDistance() --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index e079c52b8..bca163043 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -13,9 +13,9 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.WrapperCommand import frc.robot.IS_RED +import org.littletonrobotics.junction.LogTable import kotlin.math.PI import kotlin.math.hypot -import org.littletonrobotics.junction.LogTable fun ChassisSpeeds.getSpeed() = hypot(vxMetersPerSecond, vyMetersPerSecond) @@ -107,6 +107,4 @@ fun Distance.toAngle(radius: Distance): Angle = ) fun Angle.toDistance(radius: Distance): Distance = - Units.Meters.of( - this.`in`(Units.Rotations) * 2 * PI * radius.`in`(Units.Meters) - ) + radius.times(this.`in`(Units.Rotations) * 2 * PI) From 16d033ac6d8be0d44a49448e91cb9ea54873c09e Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Fri, 17 Jan 2025 16:03:33 +0200 Subject: [PATCH 149/253] Simplify Angle.toDistance() again --- src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt index bca163043..ed8b3be42 100644 --- a/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/ExtensionFunctions.kt @@ -107,4 +107,4 @@ fun Distance.toAngle(radius: Distance): Angle = ) fun Angle.toDistance(radius: Distance): Distance = - radius.times(this.`in`(Units.Rotations) * 2 * PI) + radius * this.`in`(Units.Rotations) * 2.0 * PI From 822544e6def8b5be59326459623dd6a5dceba363 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Wed, 15 Jan 2025 19:23:55 +0200 Subject: [PATCH 150/253] Add lock unlock fold and unfold --- .../frc/robot/subsystems/climber/Climber.kt | 94 +++++++------------ 1 file changed, 35 insertions(+), 59 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 801534ed9..833500f48 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,71 +1,47 @@ package frc.robot.subsystems.climber +import edu.wpi.first.networktables.DoubleEntry import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import org.littletonrobotics.junction.AutoLogOutput -import org.littletonrobotics.junction.Logger -class Climber(private val io: ClimberIO) : SubsystemBase() { +class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { - inputs.sensorDistance < DISTANCE_THRESHOLD + private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } + private var isAttached = Trigger(isLatchClosed.and(isTouching)) + + companion object { + @Volatile + private var instance: Climber? = null + + fun initialize(io: ClimberIO) { + synchronized(this) { + if (instance == null) { + instance = Climber(io) + } + } + } + + fun getInstance(): Climber { + return instance ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) + } } - @AutoLogOutput - private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) - } - - @AutoLogOutput - private var isAttached = Trigger(isLatchClosed).and(isTouching) - - @AutoLogOutput - private val isFolded = Trigger { - inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) - } - - fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - - private fun setLatchPose(latchPose: Dimensionless) = - io.setLatchPosition(latchPose) - - fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) - - fun lock(): Command = runOnce({ io.lock() }) - fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() { - io.setAngle(UNFOLDED_ANGLE) - } - - fun fold() { - io.setAngle(FOLDED_ANGLE) - } - - fun climb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } - .until(isLatchClosed) - .andThen(setAngle(FOLDED_ANGLE)) - .until(isFolded) - .andThen(lock()) - - fun unClimb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } - .andThen(unlock()) - .andThen({ unfold() }) - .andThen(openLatch()) - - private fun setAngle(angle: Angle): Command = - runOnce({ io.setAngle(angle) }) - - private fun setPower(power: Double): Command = - runOnce({ io.setPower(power) }) - - override fun periodic() { - io.updateInput() - Logger.processInputs(this::class.simpleName, io.inputs) - } -} + fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) + fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) + fun lock():Command = Commands.runOnce({io.lock()}) + fun unlock():Command = Commands.runOnce({io.unlock()}) + fun unfold(){} //TODO + fun fold(){} //TODO + private fun setAngle(angle:Angle):Command = Commands.runOnce({io.setAngle(angle)}) + private fun setPower(power:Double):Command = Commands.runOnce({io.setPower(power)}) + +} \ No newline at end of file From 5382f1c698cc0585519ade95747ee0e00c998982 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 17:11:45 +0200 Subject: [PATCH 151/253] Add Unlock and lock angle --- .../robot/subsystems/climber/ClimberConstants.kt | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2a4b02598..9d1e579f7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,16 +2,17 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) -val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) +const val OPEN_LATCH_POSITION = 0.8 +const val CLOSE_LATCH_POSITION = 0.2 +val UNLOCK_ANGLE = Units.Degree.of(90.0) +val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -val FOLDED_TOLERANCE = Units.Degree.of(1.0) +const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) +var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +var LATCH_TOLERANCE = 0.03 From 7863c6a428bb942d15fb6a5ed9496286827d8e4e Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 18:45:14 +0200 Subject: [PATCH 152/253] Change latchposition to Dimention less --- .../frc/robot/subsystems/climber/Climber.kt | 65 ++++++++++++++----- 1 file changed, 48 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 833500f48..bb25ac2a7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,21 +1,27 @@ package frc.robot.subsystems.climber -import edu.wpi.first.networktables.DoubleEntry +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { inputs.sensorDistance.lt(distanceThreshold) } + private var isTouching = Trigger { + inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + } private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } - private var isLatchClosed = Trigger { inputs.latchPosition.lt(latchTolerance) } + private var isLatchClosed = Trigger { + inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + } private var isAttached = Trigger(isLatchClosed.and(isTouching)) + private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } companion object { @Volatile @@ -30,18 +36,43 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun getInstance(): Climber { - return instance ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) + return instance + ?: throw IllegalStateException( + "Climber has not been initialized. Call initialize(io: ClimberIO) first." + ) } } - fun closeLatch():Command = Commands.runOnce({io.setLatchPosition(CLOSE_LATCH_POSITION)}) - fun openLatch():Command = Commands.runOnce({io.setLatchPosition(OPEN_LATCH_POSITION)}) - fun lock():Command = Commands.runOnce({io.lock()}) - fun unlock():Command = Commands.runOnce({io.unlock()}) - fun unfold(){} //TODO - fun fold(){} //TODO - private fun setAngle(angle:Angle):Command = Commands.runOnce({io.setAngle(angle)}) - private fun setPower(power:Double):Command = Commands.runOnce({io.setPower(power)}) - -} \ No newline at end of file + + fun closeLatch(): Command = + runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + + fun openLatch(): Command = + runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + + fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() { + io.setAngle(UNFOLDED_ANGLE) + } + + fun fold() { + io.setAngle(FOLDED_ANGLE) + } + + fun climb(): Command = + run { closeLatch() } + .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) + .finallyDo(lock().onlyIf(isFolded)) + + fun unClimb(): Command = + run { setPower(-0.4) } + .andThen(unlock()) + .andThen({ unfold() }) + .finallyDo(openLatch()) + + private fun setAngle(angle: Angle): Command = + runOnce({ io.setAngle(angle) }) + + private fun setPower(power: Double): Command = + runOnce({ io.setPower(power) }) +} From fbfa85151abb3b5c25fd11746793510a615492c7 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 19:54:11 +0200 Subject: [PATCH 153/253] spotlessApply --- .../frc/robot/subsystems/climber/Climber.kt | 61 ++++++++----------- 1 file changed, 27 insertions(+), 34 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index bb25ac2a7..801534ed9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,53 +1,39 @@ package frc.robot.subsystems.climber -import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger -import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput +import org.littletonrobotics.junction.Logger -class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { +class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput private var isTouching = Trigger { - inputs.sensorDistance.lt(DISTANCE_THRESHOLD) + inputs.sensorDistance < DISTANCE_THRESHOLD } - private val hasClimbed = Trigger { inputs.angle.lt(FOLDED_ANGLE) } + @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition < LATCH_TOLERANCE + CLOSE_LATCH_POSITION + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } - private var isAttached = Trigger(isLatchClosed.and(isTouching)) - private val isFolded = Trigger { inputs.angle == FOLDED_ANGLE } - - companion object { - @Volatile - private var instance: Climber? = null - fun initialize(io: ClimberIO) { - synchronized(this) { - if (instance == null) { - instance = Climber(io) - } - } - } + @AutoLogOutput + private var isAttached = Trigger(isLatchClosed).and(isTouching) - fun getInstance(): Climber { - return instance - ?: throw IllegalStateException( - "Climber has not been initialized. Call initialize(io: ClimberIO) first." - ) - } + @AutoLogOutput + private val isFolded = Trigger { + inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } - fun closeLatch(): Command = - runOnce({ io.setLatchPosition(CLOSE_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - fun openLatch(): Command = - runOnce({ io.setLatchPosition(OPEN_LATCH_POSITION) }) + private fun setLatchPose(latchPose: Dimensionless) = + io.setLatchPosition(latchPose) + + fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) @@ -60,19 +46,26 @@ class Climber private constructor(private val io: ClimberIO) : SubsystemBase() { } fun climb(): Command = - run { closeLatch() } - .andThen(setAngle(FOLDED_ANGLE).onlyIf(isLatchClosed)) - .finallyDo(lock().onlyIf(isFolded)) + run { setLatchPose(CLOSE_LATCH_POSITION) } + .until(isLatchClosed) + .andThen(setAngle(FOLDED_ANGLE)) + .until(isFolded) + .andThen(lock()) fun unClimb(): Command = - run { setPower(-0.4) } + run { setLatchPose(CLOSE_LATCH_POSITION) } .andThen(unlock()) .andThen({ unfold() }) - .finallyDo(openLatch()) + .andThen(openLatch()) private fun setAngle(angle: Angle): Command = runOnce({ io.setAngle(angle) }) private fun setPower(power: Double): Command = runOnce({ io.setPower(power) }) + + override fun periodic() { + io.updateInput() + Logger.processInputs(this::class.simpleName, io.inputs) + } } From 0f8769c0209441492812413724657aad8bc114ee Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sat, 18 Jan 2025 20:38:57 +0200 Subject: [PATCH 154/253] Fix idk i hate rebase --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 9d1e579f7..e2d17d354 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,17 +2,20 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -const val OPEN_LATCH_POSITION = 0.8 -const val CLOSE_LATCH_POSITION = 0.2 +val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) +val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) val UNLOCK_ANGLE = Units.Degree.of(90.0) val LOCK_ANGLE = Units.Degree.of(10.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) const val GEAR_RATIO = 1.0 -val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val FOLDED_TOLERANCE = Units.Degree.of(1.0) +val MOMENT_OF_INERTIA_MAIN: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) var DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -var LATCH_TOLERANCE = 0.03 +val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) From fe15b270706d5a2775e1997973001d2fd568d635 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 09:59:48 +0200 Subject: [PATCH 155/253] Change latch position to angle from dimentionless --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 +++-- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 6 +++--- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 4 ++-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 801534ed9..d472f12c0 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command @@ -17,7 +18,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE.`in`(Units.Degree)) } @AutoLogOutput @@ -30,7 +31,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) - private fun setLatchPose(latchPose: Dimensionless) = + private fun setLatchPose(latchPose: Angle) = io.setLatchPosition(latchPose) fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2a4b02598..22806b981 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -7,11 +7,11 @@ import edu.wpi.first.units.measure.MomentOfInertia const val UNFOLD_POWER = 1 const val FOLD_POWER = 1 -val OPEN_LATCH_POSITION: Dimensionless = Units.Percent.of(0.8) -val CLOSE_LATCH_POSITION = Units.Percent.of(0.2) +val OPEN_LATCH_POSITION: Angle = Units.Degree.of(0.8) +val CLOSE_LATCH_POSITION:Angle = Units.Degree.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -val LATCH_TOLERANCE: Dimensionless = Units.Percent.of(0.03) +val LATCH_TOLERANCE: Angle = Units.Degree.of(1.0) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index c018126ac..05f4b4ce5 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -9,7 +9,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Dimensionless) {} + fun setLatchPosition(position: Angle) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} fun lock() {} @@ -20,7 +20,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Dimensionless = Units.Percent.of(0.0) + var latchPosition: Angle = Units.Degree.of(0.0) var sensorDistance: Distance = Units.Centimeter.zero() } } From ac88c6f03bbd428e17d5fad19b23a91e39164f08 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:01:51 +0200 Subject: [PATCH 156/253] Change the command to end only after it close the latch --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index d472f12c0..5a73129f8 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -29,7 +29,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } - fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }) + fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }).until(isLatchClosed) private fun setLatchPose(latchPose: Angle) = io.setLatchPosition(latchPose) From 118ef5b8a2168ee55afc2410c53b9f875a3e022c Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:01:54 +0200 Subject: [PATCH 157/253] Change latch position to angle from dimentionless --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 5a73129f8..8f3065573 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger From 83b6ff92a2e798f65ab7d8a046aea32164806459 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:04:50 +0200 Subject: [PATCH 158/253] resized to 1 line --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 8f3065573..311554cea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -37,13 +37,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() { - io.setAngle(UNFOLDED_ANGLE) - } + fun unfold() = io.setAngle(UNFOLDED_ANGLE) - fun fold() { - io.setAngle(FOLDED_ANGLE) - } + fun fold() = io.setAngle(FOLDED_ANGLE) fun climb(): Command = run { setLatchPose(CLOSE_LATCH_POSITION) } From eb2cd658719328ab7e0394e1ce9c5243a418d114 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:22:34 +0200 Subject: [PATCH 159/253] ok i didn't understood you my bad change it to be a function without return and change it to run the commands that we created --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 311554cea..59df2ac27 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -41,12 +41,13 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun fold() = io.setAngle(FOLDED_ANGLE) - fun climb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } + fun climb() { + run({ closeLatch() }) .until(isLatchClosed) - .andThen(setAngle(FOLDED_ANGLE)) + .andThen({ fold() }) .until(isFolded) .andThen(lock()) + } fun unClimb(): Command = run { setLatchPose(CLOSE_LATCH_POSITION) } From 255fbb8e95e3fcb2262363389de643b8d3185a22 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:04:50 +0200 Subject: [PATCH 160/253] Resized to 1 line --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 8f3065573..311554cea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -37,13 +37,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun lock(): Command = runOnce({ io.lock() }) fun unlock(): Command = runOnce({ io.unlock() }) - fun unfold() { - io.setAngle(UNFOLDED_ANGLE) - } + fun unfold() = io.setAngle(UNFOLDED_ANGLE) - fun fold() { - io.setAngle(FOLDED_ANGLE) - } + fun fold() = io.setAngle(FOLDED_ANGLE) fun climb(): Command = run { setLatchPose(CLOSE_LATCH_POSITION) } From 1a09b1c9005c7022b68d146074377e30c1954c36 Mon Sep 17 00:00:00 2001 From: yahali david mor Date: Sun, 19 Jan 2025 10:22:34 +0200 Subject: [PATCH 161/253] Change it to be a function without return and change it to run the commands that we created --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 311554cea..59df2ac27 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -41,12 +41,13 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun fold() = io.setAngle(FOLDED_ANGLE) - fun climb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } + fun climb() { + run({ closeLatch() }) .until(isLatchClosed) - .andThen(setAngle(FOLDED_ANGLE)) + .andThen({ fold() }) .until(isFolded) .andThen(lock()) + } fun unClimb(): Command = run { setLatchPose(CLOSE_LATCH_POSITION) } From fd9aa52fca92831d1b3aaa4215c9d17a04144233 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 22:56:36 +0200 Subject: [PATCH 162/253] Add LinearServo class --- .../frc/robot/lib/motors/LinearServo.java | 62 ++++++++++++++----- 1 file changed, 47 insertions(+), 15 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java index bde3843f4..9ab48ae79 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java @@ -10,32 +10,64 @@ public class LinearServo extends Servo { double setPos; double curPos; + /** + * Parameters for L16-R Actuonix Linear Actuators + * + * @param channel PWM channel used to control the servo + * @param length max length of the servo [mm] + * @param speed max speed of the servo [mm/second] + */ public LinearServo(int channel, int length, int speed) { super(channel); -// setBounds(2.0, 1.8, 1.5, 1.2, 1.0); + setBoundsMicroseconds(2, 2, 1, 1, 1); m_length = length; m_speed = speed; } - - public void setPosition(double setpoint) { + + /** + Miniature Linear Servo Actuators - + User Guide (Rev 1) + Page 10Table of Contentswcproducts.com + * Run this method in any periodic function to update the position estimation of your + servo + * + * @param setpoint the target position of the servo [mm] + */ + public void setPosition(double setpoint){ setPos = MathUtil.clamp(setpoint, 0, m_length); - setSpeed((setPos / m_length * 2) - 1); + setSpeed( (setPos/m_length *2)-1); } - double lastTime = 0; - - public void updateCurPos() { + /** + * Run this method in any periodic function to update the position estimation of your + servo + */ + public void updateCurPos(){ double dt = Timer.getFPGATimestamp() - lastTime; - if (curPos > setPos + m_speed * dt) { - curPos -= m_speed * dt; - } else if (curPos < setPos - m_speed * dt) { - curPos += m_speed * dt; - } else { + if (curPos > setPos + m_speed *dt){ + curPos -= m_speed *dt; + } else if(curPos < setPos - m_speed *dt){ + curPos += m_speed *dt; + }else{ curPos = setPos; } } - - public double getPosition() { + /** + * Current position of the servo, must be calling {@link #updateCurPos() + updateCurPos()} periodically + * + * @return Servo Position [mm] + */ + public double getPosition(){ return curPos; } -} + + /** + * Checks if the servo is at its target position, must be calling {@link #updateCurPos() + updateCurPos()} periodically + * @return true when servo is at its target + */ + public boolean isFinished(){ + return curPos == setPos; + } +} \ No newline at end of file From 60cbb8834999b6b2ab69d31cd6f5aa5e7d80d259 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:06:48 +0200 Subject: [PATCH 163/253] Rename Port to ClimberPorts --- .../frc/robot/subsystems/climber/{Port.kt => ClimberPorts.kt} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/kotlin/frc/robot/subsystems/climber/{Port.kt => ClimberPorts.kt} (100%) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt similarity index 100% rename from src/main/kotlin/frc/robot/subsystems/climber/Port.kt rename to src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt From c5d4284275a813934711b0803d52bc3bb8b3abc0 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:09:25 +0200 Subject: [PATCH 164/253] Add line separator --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 59df2ac27..490eb6be9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -36,7 +36,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) fun lock(): Command = runOnce({ io.lock() }) + fun unlock(): Command = runOnce({ io.unlock() }) + fun unfold() = io.setAngle(UNFOLDED_ANGLE) fun fold() = io.setAngle(FOLDED_ANGLE) From fbfd2babd304ec258c4d6e7436a2d6d69764a58a Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:11:32 +0200 Subject: [PATCH 165/253] Change seconds to microseconds --- src/main/kotlin/frc/robot/lib/motors/LinearServo.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java index 9ab48ae79..283cfb507 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.java @@ -19,7 +19,7 @@ public class LinearServo extends Servo { */ public LinearServo(int channel, int length, int speed) { super(channel); - setBoundsMicroseconds(2, 2, 1, 1, 1); + setBoundsMicroseconds(2000000, 1800000, 1500000, 1200000, 1000000); m_length = length; m_speed = speed; } From e8af40b598891e03542954a9a563b40c5223e3c6 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:30:06 +0200 Subject: [PATCH 166/253] Rename .java to .kt --- .../frc/robot/lib/motors/{LinearServo.java => LinearServo.kt} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/kotlin/frc/robot/lib/motors/{LinearServo.java => LinearServo.kt} (100%) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.java b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt similarity index 100% rename from src/main/kotlin/frc/robot/lib/motors/LinearServo.java rename to src/main/kotlin/frc/robot/lib/motors/LinearServo.kt From ed26d0437a8505854e7cb0e9de0ab15caf00109d Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:30:07 +0200 Subject: [PATCH 167/253] Convert LinearServo to kotlin and fix init --- .../frc/robot/lib/motors/LinearServo.kt | 98 ++++++++++--------- 1 file changed, 51 insertions(+), 47 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 283cfb507..06fc4ab6e 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -1,73 +1,77 @@ -package frc.robot.lib.motors; +package frc.robot.lib.motors -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.MathUtil +import edu.wpi.first.units.Units +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj.Servo +import edu.wpi.first.wpilibj.Timer -public class LinearServo extends Servo { - double m_speed; - double m_length; - double setPos; - double curPos; +class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { + var m_speed: Double + var m_length: Double + var setPos = 0.0 + var curPos = 0.0 + var lastTime = 0.0 /** - * Parameters for L16-R Actuonix Linear Actuators + * Miniature Linear Servo Actuators - + * User Guide (Rev 1) + * Page 10Table of Contentswcproducts.com + * Run this method in any periodic function to update the position estimation of your + * servo * - * @param channel PWM channel used to control the servo - * @param length max length of the servo [mm] - * @param speed max speed of the servo [mm/second] + * @param setpoint the target position of the servo [mm] */ - public LinearServo(int channel, int length, int speed) { - super(channel); - setBoundsMicroseconds(2000000, 1800000, 1500000, 1200000, 1000000); - m_length = length; - m_speed = speed; + override fun setPosition(setpoint: Double) { + setPos = MathUtil.clamp(setpoint, 0.0, m_length) + speed = (setPos / m_length * 2.0) - 1.0 + } + + fun setPosition(setpoint: Distance) { + setPosition(setpoint.`in`(Units.Millimeters)) } - + /** - Miniature Linear Servo Actuators - - User Guide (Rev 1) - Page 10Table of Contentswcproducts.com - * Run this method in any periodic function to update the position estimation of your - servo + * Parameters for L16-R Actuonix Linear Actuators * - * @param setpoint the target position of the servo [mm] + * @param channel PWM channel used to control the servo + * @param length max length of the servo [mm] + * @param speed max speed of the servo [mm/second] */ - public void setPosition(double setpoint){ - setPos = MathUtil.clamp(setpoint, 0, m_length); - setSpeed( (setPos/m_length *2)-1); + init { + setBoundsMicroseconds(2000, 0, 0, 0,1000) + m_length = length.toDouble() + m_speed = speed.toDouble() } - double lastTime = 0; + /** * Run this method in any periodic function to update the position estimation of your - servo + * servo */ - public void updateCurPos(){ - double dt = Timer.getFPGATimestamp() - lastTime; - if (curPos > setPos + m_speed *dt){ - curPos -= m_speed *dt; - } else if(curPos < setPos - m_speed *dt){ - curPos += m_speed *dt; - }else{ - curPos = setPos; + fun updateCurPos() { + val dt = Timer.getFPGATimestamp() - lastTime + if (curPos > setPos + m_speed * dt) { + curPos -= m_speed * dt + } else if (curPos < setPos - m_speed * dt) { + curPos += m_speed * dt + } else { + curPos = setPos } } + /** - * Current position of the servo, must be calling {@link #updateCurPos() - updateCurPos()} periodically + * Current position of the servo, must be calling [updateCurPos()][.updateCurPos] periodically * * @return Servo Position [mm] */ - public double getPosition(){ - return curPos; + override fun getPosition(): Double { + return curPos } /** - * Checks if the servo is at its target position, must be calling {@link #updateCurPos() - updateCurPos()} periodically + * Checks if the servo is at its target position, must be calling [updateCurPos()][.updateCurPos] periodically * @return true when servo is at its target */ - public boolean isFinished(){ - return curPos == setPos; - } + val isFinished: Boolean + get() = curPos == setPos } \ No newline at end of file From d73c41060cb80d4a62fa128cc8d4f405fe306d64 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:34:44 +0200 Subject: [PATCH 168/253] Apply spotless --- .../frc/robot/lib/motors/LinearServo.kt | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 06fc4ab6e..f3317d53e 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -14,11 +14,9 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { var lastTime = 0.0 /** - * Miniature Linear Servo Actuators - - * User Guide (Rev 1) - * Page 10Table of Contentswcproducts.com - * Run this method in any periodic function to update the position estimation of your - * servo + * Miniature Linear Servo Actuators - User Guide (Rev 1) Page 10Table of + * Contentswcproducts.com Run this method in any periodic function to update + * the position estimation of your servo * * @param setpoint the target position of the servo [mm] */ @@ -39,14 +37,14 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * @param speed max speed of the servo [mm/second] */ init { - setBoundsMicroseconds(2000, 0, 0, 0,1000) + setBoundsMicroseconds(2000, 0, 0, 0, 1000) m_length = length.toDouble() m_speed = speed.toDouble() } /** - * Run this method in any periodic function to update the position estimation of your - * servo + * Run this method in any periodic function to update the position + * estimation of your servo */ fun updateCurPos() { val dt = Timer.getFPGATimestamp() - lastTime @@ -60,7 +58,8 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { } /** - * Current position of the servo, must be calling [updateCurPos()][.updateCurPos] periodically + * Current position of the servo, must be calling + * [updateCurPos()][.updateCurPos] periodically * * @return Servo Position [mm] */ @@ -69,9 +68,10 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { } /** - * Checks if the servo is at its target position, must be calling [updateCurPos()][.updateCurPos] periodically + * Checks if the servo is at its target position, must be calling + * [updateCurPos()][.updateCurPos] periodically * @return true when servo is at its target */ val isFinished: Boolean get() = curPos == setPos -} \ No newline at end of file +} From ce662975ba1595d110f6a7f7b188522d8f7d17b7 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:45:47 +0200 Subject: [PATCH 169/253] Rename variables and make them private --- .../frc/robot/lib/motors/LinearServo.kt | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index f3317d53e..1767fdbcc 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -7,11 +7,11 @@ import edu.wpi.first.wpilibj.Servo import edu.wpi.first.wpilibj.Timer class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { - var m_speed: Double - var m_length: Double - var setPos = 0.0 - var curPos = 0.0 - var lastTime = 0.0 + private var speed: Double + private var length: Double + private var setpoint = 0.0 + private var position = 0.0 + private var lastTime = 0.0 /** * Miniature Linear Servo Actuators - User Guide (Rev 1) Page 10Table of @@ -21,8 +21,8 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * @param setpoint the target position of the servo [mm] */ override fun setPosition(setpoint: Double) { - setPos = MathUtil.clamp(setpoint, 0.0, m_length) - speed = (setPos / m_length * 2.0) - 1.0 + this.setpoint = MathUtil.clamp(setpoint, 0.0, length) + speed = (this.setpoint / length * 2.0) - 1.0 } fun setPosition(setpoint: Distance) { @@ -38,22 +38,22 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { */ init { setBoundsMicroseconds(2000, 0, 0, 0, 1000) - m_length = length.toDouble() - m_speed = speed.toDouble() + this.length = length.toDouble() + this.speed = speed.toDouble() } /** * Run this method in any periodic function to update the position * estimation of your servo */ - fun updateCurPos() { + fun updatePosition() { val dt = Timer.getFPGATimestamp() - lastTime - if (curPos > setPos + m_speed * dt) { - curPos -= m_speed * dt - } else if (curPos < setPos - m_speed * dt) { - curPos += m_speed * dt + if (position > setpoint + speed * dt) { + position -= speed * dt + } else if (position < setpoint - speed * dt) { + position += speed * dt } else { - curPos = setPos + position = setpoint } } @@ -64,7 +64,7 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * @return Servo Position [mm] */ override fun getPosition(): Double { - return curPos + return position } /** @@ -73,5 +73,5 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * @return true when servo is at its target */ val isFinished: Boolean - get() = curPos == setPos + get() = position == setpoint } From 3a6999a2b4faa7051ff0311a60890d483927be88 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:51:08 +0200 Subject: [PATCH 170/253] Change isFinished to reachedSetpoint and make it a trigger --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 1767fdbcc..b0ea5d93d 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -5,6 +5,7 @@ import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj2.command.button.Trigger class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { private var speed: Double @@ -72,6 +73,7 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * [updateCurPos()][.updateCurPos] periodically * @return true when servo is at its target */ - val isFinished: Boolean - get() = position == setpoint + val reachedSetpoint = Trigger { + position == setpoint + } } From 76ad37fc6eca875c4aa4c103a821ce073620c063 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:55:12 +0200 Subject: [PATCH 171/253] Add tolerance to reachedSetpoint trigger --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index b0ea5d93d..1e1c5c650 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -6,8 +6,9 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.button.Trigger +import frc.robot.lib.Utils -class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { +class LinearServo(channel: Int, length: Int, speed: Int, positionTolerance: Double) : Servo(channel) { private var speed: Double private var length: Double private var setpoint = 0.0 @@ -74,6 +75,6 @@ class LinearServo(channel: Int, length: Int, speed: Int) : Servo(channel) { * @return true when servo is at its target */ val reachedSetpoint = Trigger { - position == setpoint + Utils.epsilonEquals(position, setpoint, positionTolerance) } } From ff258ac4b3d7f70baa0f64294d630fd0f521c300 Mon Sep 17 00:00:00 2001 From: Emma Date: Sun, 19 Jan 2025 23:55:35 +0200 Subject: [PATCH 172/253] Apply spotless --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 1e1c5c650..aed9b47eb 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -8,7 +8,12 @@ import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.Utils -class LinearServo(channel: Int, length: Int, speed: Int, positionTolerance: Double) : Servo(channel) { +class LinearServo( + channel: Int, + length: Int, + speed: Int, + positionTolerance: Double +) : Servo(channel) { private var speed: Double private var length: Double private var setpoint = 0.0 From fc409fbccdbdfaaa71e4a74ddd3f51b68163b6c8 Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:05:29 +0200 Subject: [PATCH 173/253] Use isNear instead of epsilonEquals --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index aed9b47eb..1e400ae72 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -6,7 +6,6 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.wpilibj.Servo import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.button.Trigger -import frc.robot.lib.Utils class LinearServo( channel: Int, @@ -80,6 +79,6 @@ class LinearServo( * @return true when servo is at its target */ val reachedSetpoint = Trigger { - Utils.epsilonEquals(position, setpoint, positionTolerance) + MathUtil.isNear(setpoint, position, positionTolerance) } } From 84c318768d6f567262b3b079ea50904c570d60bb Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:05:50 +0200 Subject: [PATCH 174/253] Update function names in docstring --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 1e400ae72..a6954a82d 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -65,7 +65,7 @@ class LinearServo( /** * Current position of the servo, must be calling - * [updateCurPos()][.updateCurPos] periodically + * [updatePosition()][.updatePosition] periodically * * @return Servo Position [mm] */ @@ -75,7 +75,7 @@ class LinearServo( /** * Checks if the servo is at its target position, must be calling - * [updateCurPos()][.updateCurPos] periodically + * [updatePosition()][.updatePosition] periodically * @return true when servo is at its target */ val reachedSetpoint = Trigger { From 92efbafdb722c90f93b11ee67145609d763053d5 Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:09:31 +0200 Subject: [PATCH 175/253] Move documentation from init to constructor --- .../kotlin/frc/robot/lib/motors/LinearServo.kt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index a6954a82d..06eb08afb 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -7,6 +7,13 @@ import edu.wpi.first.wpilibj.Servo import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.button.Trigger +/** + * Parameters for L16-R Actuonix Linear Actuators + * + * @param channel PWM channel used to control the servo + * @param length max length of the servo [mm] + * @param speed max speed of the servo [mm/second] + */ class LinearServo( channel: Int, length: Int, @@ -35,13 +42,6 @@ class LinearServo( setPosition(setpoint.`in`(Units.Millimeters)) } - /** - * Parameters for L16-R Actuonix Linear Actuators - * - * @param channel PWM channel used to control the servo - * @param length max length of the servo [mm] - * @param speed max speed of the servo [mm/second] - */ init { setBoundsMicroseconds(2000, 0, 0, 0, 1000) this.length = length.toDouble() From e61022fd09de7d22ac4df3e681e4894d6d3d4bce Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:41:26 +0200 Subject: [PATCH 176/253] Rename io functions lock and unlock to close and open stopper --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 05f4b4ce5..fa0af43da 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -2,7 +2,6 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged @@ -12,8 +11,8 @@ interface ClimberIO { fun setLatchPosition(position: Angle) {} fun setPower(power: Double) {} fun setAngle(angle: Angle) {} - fun lock() {} - fun unlock() {} + fun closeStopper() {} + fun openStopper() {} fun updateInput() {} @Logged From 21c07b7beb01ef2637130c2cce64da4c1b4edf14 Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:42:58 +0200 Subject: [PATCH 177/253] Remove unnecessary constants and add UNLOCK_POWER --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 22806b981..7e54944f4 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -5,8 +5,7 @@ import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia -const val UNFOLD_POWER = 1 -const val FOLD_POWER = 1 +const val UNLOCK_POWER = 0.0 val OPEN_LATCH_POSITION: Angle = Units.Degree.of(0.8) val CLOSE_LATCH_POSITION:Angle = Units.Degree.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) From 93ca8729a6427ebab5437e4924423f7b6c2432fe Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:43:18 +0200 Subject: [PATCH 178/253] Reorder functions and fix some logic --- .../frc/robot/subsystems/climber/Climber.kt | 51 ++++++++++--------- 1 file changed, 27 insertions(+), 24 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 490eb6be9..faaf18b9f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -3,6 +3,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import org.littletonrobotics.junction.AutoLogOutput @@ -28,40 +29,42 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } - fun closeLatch(): Command = runOnce({ setLatchPose(CLOSE_LATCH_POSITION) }).until(isLatchClosed) + private fun setAngle(angle: Angle): Command = + runOnce { io.setAngle(angle) } - private fun setLatchPose(latchPose: Angle) = - io.setLatchPosition(latchPose) + private fun setPower(power: Double): Command = + Commands.startEnd({ io.setPower(power) }, {io.setPower(0.0)}) - fun openLatch(): Command = runOnce({ setLatchPose(OPEN_LATCH_POSITION) }) + private fun setLatchPose(latchPose: Angle): Command = + runOnce { io.setLatchPosition(latchPose) } - fun lock(): Command = runOnce({ io.lock() }) + fun closeLatch(): Command = setLatchPose(CLOSE_LATCH_POSITION) - fun unlock(): Command = runOnce({ io.unlock() }) + fun openLatch(): Command = setLatchPose(OPEN_LATCH_POSITION) - fun unfold() = io.setAngle(UNFOLDED_ANGLE) + fun lock(): Command = runOnce{ io.closeStopper() } - fun fold() = io.setAngle(FOLDED_ANGLE) + fun unlock(): Command = + setPower(UNLOCK_POWER).withTimeout(0.15) + .andThen( {io.openStopper()} ) - fun climb() { - run({ closeLatch() }) - .until(isLatchClosed) - .andThen({ fold() }) - .until(isFolded) - .andThen(lock()) - } + fun unfold() = setAngle(UNFOLDED_ANGLE) - fun unClimb(): Command = - run { setLatchPose(CLOSE_LATCH_POSITION) } - .andThen(unlock()) - .andThen({ unfold() }) - .andThen(openLatch()) + fun fold() = setAngle(FOLDED_ANGLE) - private fun setAngle(angle: Angle): Command = - runOnce({ io.setAngle(angle) }) + fun climb(): Command = + Commands.sequence( + closeLatch(), + fold(), + lock() + ) - private fun setPower(power: Double): Command = - runOnce({ io.setPower(power) }) + fun unClimb(): Command = + Commands.sequence( + unlock(), + unfold(), + openLatch() + ) override fun periodic() { io.updateInput() From 065f26c5eb227e6f6d41aad10b7f93c86d4db59c Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 00:43:47 +0200 Subject: [PATCH 179/253] Remove unnecessary wrapper --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index faaf18b9f..cb67404bf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -22,7 +22,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput - private var isAttached = Trigger(isLatchClosed).and(isTouching) + private var isAttached = isLatchClosed.and(isTouching) @AutoLogOutput private val isFolded = Trigger { From e8cbc4d9af5b35f4d0e9a1f5c0d2805db1b6ba69 Mon Sep 17 00:00:00 2001 From: Emma Date: Mon, 20 Jan 2025 01:00:02 +0200 Subject: [PATCH 180/253] Fix setPosition --- src/main/kotlin/frc/robot/lib/motors/LinearServo.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt index 06eb08afb..cf5ae7914 100644 --- a/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt +++ b/src/main/kotlin/frc/robot/lib/motors/LinearServo.kt @@ -35,7 +35,7 @@ class LinearServo( */ override fun setPosition(setpoint: Double) { this.setpoint = MathUtil.clamp(setpoint, 0.0, length) - speed = (this.setpoint / length * 2.0) - 1.0 + setSpeed((this.setpoint / length * 2.0) - 1.0) } fun setPosition(setpoint: Distance) { From 5de98d6527050a6dda5bfe2a4c121692073ad4e1 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 08:20:56 +0200 Subject: [PATCH 181/253] Change setPower to setVoltage and rename UNLOCK_POWER to UNLOCK_VOLTAGE --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 7 ++++--- .../frc/robot/subsystems/climber/ClimberConstants.kt | 3 +-- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index cb67404bf..56989822b 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.SubsystemBase @@ -32,8 +33,8 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) } - private fun setPower(power: Double): Command = - Commands.startEnd({ io.setPower(power) }, {io.setPower(0.0)}) + private fun setVoltage(voltage: Voltage): Command = + Commands.startEnd({ io.setVoltage(voltage) }, { io.setVoltage(Units.Volts.zero()) }) private fun setLatchPose(latchPose: Angle): Command = runOnce { io.setLatchPosition(latchPose) } @@ -45,7 +46,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun lock(): Command = runOnce{ io.closeStopper() } fun unlock(): Command = - setPower(UNLOCK_POWER).withTimeout(0.15) + setVoltage(UNLOCK_VOLTAGE).withTimeout(0.15) .andThen( {io.openStopper()} ) fun unfold() = setAngle(UNFOLDED_ANGLE) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 7e54944f4..f1ce5c716 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,10 +2,9 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless import edu.wpi.first.units.measure.MomentOfInertia -const val UNLOCK_POWER = 0.0 +val UNLOCK_VOLTAGE = Units.Volts.of(0.0) val OPEN_LATCH_POSITION: Angle = Units.Degree.of(0.8) val CLOSE_LATCH_POSITION:Angle = Units.Degree.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index fa0af43da..4c608804d 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -9,7 +9,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber fun setLatchPosition(position: Angle) {} - fun setPower(power: Double) {} + fun setVoltage(voltage: Voltage) {} fun setAngle(angle: Angle) {} fun closeStopper() {} fun openStopper() {} From db6970339cf2427ac3f19bf2e65b6c966183bc00 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 08:21:51 +0200 Subject: [PATCH 182/253] Apply spotless --- .../frc/robot/subsystems/climber/Climber.kt | 39 +++++++++---------- .../subsystems/climber/ClimberConstants.kt | 2 +- 2 files changed, 19 insertions(+), 22 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 56989822b..e24b9b7ec 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -19,7 +19,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE.`in`(Units.Degree)) + inputs.latchPosition.isNear( + CLOSE_LATCH_POSITION, + LATCH_TOLERANCE.`in`(Units.Degree) + ) } @AutoLogOutput @@ -30,42 +33,36 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } - private fun setAngle(angle: Angle): Command = - runOnce { io.setAngle(angle) } + private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) } private fun setVoltage(voltage: Voltage): Command = - Commands.startEnd({ io.setVoltage(voltage) }, { io.setVoltage(Units.Volts.zero()) }) + Commands.startEnd( + { io.setVoltage(voltage) }, + { io.setVoltage(Units.Volts.zero()) } + ) - private fun setLatchPose(latchPose: Angle): Command = - runOnce { io.setLatchPosition(latchPose) } + private fun setLatchPose(latchPose: Angle): Command = runOnce { + io.setLatchPosition(latchPose) + } fun closeLatch(): Command = setLatchPose(CLOSE_LATCH_POSITION) fun openLatch(): Command = setLatchPose(OPEN_LATCH_POSITION) - fun lock(): Command = runOnce{ io.closeStopper() } + fun lock(): Command = runOnce { io.closeStopper() } fun unlock(): Command = - setVoltage(UNLOCK_VOLTAGE).withTimeout(0.15) - .andThen( {io.openStopper()} ) + setVoltage(UNLOCK_VOLTAGE) + .withTimeout(0.15) + .andThen({ io.openStopper() }) fun unfold() = setAngle(UNFOLDED_ANGLE) fun fold() = setAngle(FOLDED_ANGLE) - fun climb(): Command = - Commands.sequence( - closeLatch(), - fold(), - lock() - ) + fun climb(): Command = Commands.sequence(closeLatch(), fold(), lock()) - fun unClimb(): Command = - Commands.sequence( - unlock(), - unfold(), - openLatch() - ) + fun unClimb(): Command = Commands.sequence(unlock(), unfold(), openLatch()) override fun periodic() { io.updateInput() diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index f1ce5c716..d3b2c3154 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -6,7 +6,7 @@ import edu.wpi.first.units.measure.MomentOfInertia val UNLOCK_VOLTAGE = Units.Volts.of(0.0) val OPEN_LATCH_POSITION: Angle = Units.Degree.of(0.8) -val CLOSE_LATCH_POSITION:Angle = Units.Degree.of(0.2) +val CLOSE_LATCH_POSITION: Angle = Units.Degree.of(0.2) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE = Units.Degree.of(1.0) From b2dd24e9991cb5804901ab067ecb9af01be7520f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 11:27:39 +0200 Subject: [PATCH 183/253] Change latch position to distance instead of angle --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 +++-- .../frc/robot/subsystems/climber/ClimberConstants.kt | 7 ++++--- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 4 ++-- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index e24b9b7ec..3a3173a4b 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands @@ -21,7 +22,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isLatchClosed = Trigger { inputs.latchPosition.isNear( CLOSE_LATCH_POSITION, - LATCH_TOLERANCE.`in`(Units.Degree) + LATCH_TOLERANCE ) } @@ -41,7 +42,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { { io.setVoltage(Units.Volts.zero()) } ) - private fun setLatchPose(latchPose: Angle): Command = runOnce { + private fun setLatchPose(latchPose: Distance): Command = runOnce { io.setLatchPosition(latchPose) } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index d3b2c3154..2ebe0897a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,14 +2,15 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia val UNLOCK_VOLTAGE = Units.Volts.of(0.0) -val OPEN_LATCH_POSITION: Angle = Units.Degree.of(0.8) -val CLOSE_LATCH_POSITION: Angle = Units.Degree.of(0.2) +val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) +val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) +val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) -val LATCH_TOLERANCE: Angle = Units.Degree.of(1.0) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 4c608804d..aecd11c8c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -8,7 +8,7 @@ import org.team9432.annotation.Logged interface ClimberIO { var inputs: LoggedInputClimber - fun setLatchPosition(position: Angle) {} + fun setLatchPosition(position: Distance) {} fun setVoltage(voltage: Voltage) {} fun setAngle(angle: Angle) {} fun closeStopper() {} @@ -19,7 +19,7 @@ interface ClimberIO { open class InputClimber { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() - var latchPosition: Angle = Units.Degree.of(0.0) + var latchPosition: Distance = Units.Millimeters.zero() var sensorDistance: Distance = Units.Centimeter.zero() } } From 4f8a88f5f2b2d35f43279fb6b3549ab2cf6d25f6 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 11:37:29 +0200 Subject: [PATCH 184/253] Apply spotless --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 3a3173a4b..73c30b4fb 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -20,10 +20,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput private var isLatchClosed = Trigger { - inputs.latchPosition.isNear( - CLOSE_LATCH_POSITION, - LATCH_TOLERANCE - ) + inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } @AutoLogOutput From 5c2a7f94409337d06cec65795541029147d4da3b Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 13:33:37 +0200 Subject: [PATCH 185/253] Rename unclimb to declimb --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 73c30b4fb..d9f9dc29c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -60,7 +60,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun climb(): Command = Commands.sequence(closeLatch(), fold(), lock()) - fun unClimb(): Command = Commands.sequence(unlock(), unfold(), openLatch()) + fun declimb(): Command = Commands.sequence(unlock(), unfold(), openLatch()) override fun periodic() { io.updateInput() From f4204a3f378439f12bfb7fee7e35cb253f8818ef Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 13:34:15 +0200 Subject: [PATCH 186/253] Use method reference instead of call --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index d9f9dc29c..7d9c63bfb 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -52,7 +52,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun unlock(): Command = setVoltage(UNLOCK_VOLTAGE) .withTimeout(0.15) - .andThen({ io.openStopper() }) + .andThen(io::openStopper) fun unfold() = setAngle(UNFOLDED_ANGLE) From 8475e0b0504d1326c4848b9ed617bee0ccbe6e65 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 13:40:42 +0200 Subject: [PATCH 187/253] Rename inputs class --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index aecd11c8c..fa440603f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -7,7 +7,7 @@ import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged interface ClimberIO { - var inputs: LoggedInputClimber + var inputs: LoggedClimberInputs fun setLatchPosition(position: Distance) {} fun setVoltage(voltage: Voltage) {} fun setAngle(angle: Angle) {} @@ -16,7 +16,7 @@ interface ClimberIO { fun updateInput() {} @Logged - open class InputClimber { + open class ClimberInputs { var appliedVoltage: Voltage = Units.Volts.zero() var angle: Angle = Units.Degree.zero() var latchPosition: Distance = Units.Millimeters.zero() From ec5f32c9e85f0de916a7ec8fca628670dc094029 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 13:41:06 +0200 Subject: [PATCH 188/253] Apply spotless --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 7d9c63bfb..8bc2b80d5 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -50,9 +50,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun lock(): Command = runOnce { io.closeStopper() } fun unlock(): Command = - setVoltage(UNLOCK_VOLTAGE) - .withTimeout(0.15) - .andThen(io::openStopper) + setVoltage(UNLOCK_VOLTAGE).withTimeout(0.15).andThen(io::openStopper) fun unfold() = setAngle(UNFOLDED_ANGLE) From 1b5766050c3cae8bb3490f90f4bb85f3529c9b3f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 13:42:21 +0200 Subject: [PATCH 189/253] Add type declarations --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2ebe0897a..7168a78d7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -4,13 +4,14 @@ import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia +import edu.wpi.first.units.measure.Voltage -val UNLOCK_VOLTAGE = Units.Volts.of(0.0) +val UNLOCK_VOLTAGE: Voltage = Units.Volts.of(0.0) val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) -val FOLDED_TOLERANCE = Units.Degree.of(1.0) +val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -val DISTANCE_THRESHOLD = Units.Centimeter.of(0.4) +val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) From 6a06664edd58230140a6c44206fc535a90778f17 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:04:32 +0200 Subject: [PATCH 190/253] Remove old Port file --- src/main/kotlin/frc/robot/subsystems/climber/Port.kt | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt deleted file mode 100644 index f5bb4bd7c..000000000 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ /dev/null @@ -1,8 +0,0 @@ -package frc.robot.subsystems.climber - -const val MAIN_MOTOR_ID = 0 -const val AUX_MOTOR_ID = 1 -const val SERVO_1_ID = 2 -const val SERVO_2_ID = 3 -const val SENSOR_ID = 4 -const val LOCK_MOTOR_ID = 5 From c9893b623368f8f8d22e2bc84f23823c3bb3c46e Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:06:28 +0200 Subject: [PATCH 191/253] Reorder variables' make them private and update inputs' type --- .../robot/subsystems/climber/ClimberIOReal.kt | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 4889d18f4..938026634 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -4,7 +4,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode import com.ctre.phoenix.motorcontrol.can.TalonSRX import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage -import com.ctre.phoenix6.controls.StrictFollower +import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.math.filter.MedianFilter import edu.wpi.first.units.Units @@ -14,15 +14,18 @@ import frc.robot.lib.motors.LinearServo class ClimberIOReal:ClimberIO { - override var inputs: LoggedInputClimber = LoggedInputClimber() - val mainMotor = TalonFX(MAIN_MOTOR_ID) - val servo1 = LinearServo(SERVO_1_ID, 1, 1) - val servo2 = LinearServo(SERVO_2_ID, 1, 1) - val auxMotor = TalonFX(AUX_MOTOR_ID) - val lockServo = TalonSRX(LOCK_MOTOR_ID) - val dutyCycleOut = DutyCycleOut(0.0) - val positionVoltage = PositionVoltage(0.0) - val sensor = AnalogInput(SENSOR_ID) + override var inputs: LoggedClimberInputs = LoggedClimberInputs() + + private val mainMotor = TalonFX(MAIN_MOTOR_ID) + private val auxMotor = TalonFX(AUX_MOTOR_ID) + private val servo1 = LinearServo(LATCH_SERVO_ID, 1, 1) + private val servo2 = LinearServo(FOLLOW_LATCH_SERVO_ID, 1, 1) + private val stopperMotor = TalonSRX(STOPPER_MOTOR_ID) + + private val voltageControl = VoltageOut(0.0) + private val positionControl = PositionVoltage(0.0) + + private val sensor = AnalogInput(SENSOR_ID) private val distanceFilter = MedianFilter(3) private val distance = AnalogInput(0) From 2b1e4cc16d198c62c199a6c9dfdc10cab7d388b0 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:06:47 +0200 Subject: [PATCH 192/253] Remove unnecessary variable --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 938026634..27c17de74 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -27,7 +27,6 @@ class ClimberIOReal:ClimberIO { private val sensor = AnalogInput(SENSOR_ID) private val distanceFilter = MedianFilter(3) - private val distance = AnalogInput(0) init { auxMotor.setControl(StrictFollower(mainMotor.deviceID)) @@ -59,7 +58,7 @@ class ClimberIOReal:ClimberIO { inputs.angle = mainMotor.position.value inputs.appliedVoltage = mainMotor.supplyVoltage.value inputs.latchPosition = servo1.position - var calculatedDistance = distanceFilter.calculate(4800 / (200 * distance.getVoltage() - 20.0)) + var calculatedDistance = distanceFilter.calculate(4800 / (200 * sensor.voltage - 20.0)) if (calculatedDistance < 0) { calculatedDistance = 80.0 } From 6509891cae67bf6f397a2b00cf34b1d2aeed825e Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:08:55 +0200 Subject: [PATCH 193/253] Fix updateInputs --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 27c17de74..9ed5bd9e3 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -57,7 +57,8 @@ class ClimberIOReal:ClimberIO { override fun updateInput() { inputs.angle = mainMotor.position.value inputs.appliedVoltage = mainMotor.supplyVoltage.value - inputs.latchPosition = servo1.position + inputs.latchPosition = Units.Millimeters.of(servo1.position) + var calculatedDistance = distanceFilter.calculate(4800 / (200 * sensor.voltage - 20.0)) if (calculatedDistance < 0) { calculatedDistance = 80.0 From 293160f0e1843798a219b8ff346822357d3fabb2 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:09:38 +0200 Subject: [PATCH 194/253] Rename functions to IO name --- .../robot/subsystems/climber/ClimberIOReal.kt | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 9ed5bd9e3..23d768c16 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -9,6 +9,8 @@ import com.ctre.phoenix6.hardware.TalonFX import edu.wpi.first.math.filter.MedianFilter import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj.AnalogInput import frc.robot.lib.motors.LinearServo @@ -33,24 +35,24 @@ class ClimberIOReal:ClimberIO { listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } } - override fun setLatchPosition(position: Double) { - listOf(servo2, servo1).forEach { it.position = position } + override fun setLatchPosition(position: Distance) { + listOf(servo2, servo1).forEach { it.position = position.`in`(Units.Millimeters) } } - override fun setPower(power: Double) { - mainMotor.setControl(dutyCycleOut.withOutput(power)) + override fun setVoltage(voltage: Voltage) { + mainMotor.setControl(voltageControl.withOutput(voltage)) } override fun setAngle(angle: Angle) { mainMotor.setControl(positionVoltage.withPosition(angle)) } - override fun lock() { - lockServo.set(ControlMode.Position, LOCK_ANGLE.`in`(Units.Radians)) + override fun closeStopper() { + stopperMotor.set(ControlMode.Position, LOCK_ANGLE.`in`(Units.Radians)) } - override fun unlock() { - lockServo.set(ControlMode.Position, UNLOCK_ANGLE.`in`(Units.Radians)) + override fun openStopper() { + stopperMotor.set(ControlMode.Position, UNLOCK_ANGLE.`in`(Units.Radians)) } From 92d5edeb3e84a87e7b815b6b79ab92880821c842 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:10:54 +0200 Subject: [PATCH 195/253] Rename lock motor tto stepper motor --- src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt index 194b9e8dd..6e3a21a24 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberPorts.kt @@ -5,4 +5,4 @@ const val AUX_MOTOR_ID = 1 const val LATCH_SERVO_ID = 2 const val FOLLOW_LATCH_SERVO_ID = 3 const val SENSOR_ID = 4 -const val LOCK_MOTOR_ID = 5 +const val STOPPER_MOTOR_ID = 5 From 1598fee69340848cc4c244b83c19d74f6785a5c8 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:13:01 +0200 Subject: [PATCH 196/253] Rename positionVoltage to positionControl --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 23d768c16..924f7beaa 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -44,7 +44,7 @@ class ClimberIOReal:ClimberIO { } override fun setAngle(angle: Angle) { - mainMotor.setControl(positionVoltage.withPosition(angle)) + mainMotor.setControl(positionControl.withPosition(angle)) } override fun closeStopper() { From 0fc69048c8c2f8d758356501ad8e6579714add1f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:13:39 +0200 Subject: [PATCH 197/253] Change StrictFollower to Follower --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 924f7beaa..ddaad3cb1 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -2,7 +2,7 @@ package frc.robot.subsystems.climber import com.ctre.phoenix.motorcontrol.ControlMode import com.ctre.phoenix.motorcontrol.can.TalonSRX -import com.ctre.phoenix6.controls.DutyCycleOut +import com.ctre.phoenix6.controls.Follower import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.VoltageOut import com.ctre.phoenix6.hardware.TalonFX @@ -31,8 +31,8 @@ class ClimberIOReal:ClimberIO { private val distanceFilter = MedianFilter(3) init { - auxMotor.setControl(StrictFollower(mainMotor.deviceID)) listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } + auxMotor.setControl(Follower(mainMotor.deviceID, true)) } override fun setLatchPosition(position: Distance) { From f1887d27e9c8af4c4ec83ab2a6b50d805af85e58 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:13:56 +0200 Subject: [PATCH 198/253] Remove line separator --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index ddaad3cb1..019d20f7f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -65,7 +65,6 @@ class ClimberIOReal:ClimberIO { if (calculatedDistance < 0) { calculatedDistance = 80.0 } - inputs.sensorDistance = Units.Meters.of(calculatedDistance) } } \ No newline at end of file From e9611c713b9a5e11daccbb4022bc15e89fa21df3 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:15:02 +0200 Subject: [PATCH 199/253] Initialize Gains before MOTOR_CONFIG --- .../subsystems/climber/ClimberConstants.kt | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 9203579db..636de089a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -23,6 +23,15 @@ val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +var Gains = selectGainsBasedOnMode( + Gains( + 0.0, + 0.0, + ), Gains( + 0.0, + 0.0, + ) +) var MOTOR_CONFIG = TalonFXConfiguration().apply { MotorOutput.apply { NeutralMode = NeutralModeValue.Brake @@ -38,12 +47,3 @@ var MOTOR_CONFIG = TalonFXConfiguration().apply { kI = Gains.kI } } -var Gains = selectGainsBasedOnMode( - Gains( - 0.0, - 0.0, - ), Gains( - 0.0, - 0.0, - ) -) From 02bc33cd037e97e05e8c783275a9af0979b36a4a Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:15:30 +0200 Subject: [PATCH 200/253] Rename Gains to GAINS --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 636de089a..569039c5e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -23,7 +23,7 @@ val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var Gains = selectGainsBasedOnMode( +var GAINS = selectGainsBasedOnMode( Gains( 0.0, 0.0, @@ -42,8 +42,8 @@ var MOTOR_CONFIG = TalonFXConfiguration().apply { SupplyCurrentLimitEnable = false } Slot0.apply { - kD = Gains.kD - kP = Gains.kP - kI = Gains.kI + kD = GAINS.kD + kP = GAINS.kP + kI = GAINS.kI } } From a429982759598b7ffbbf08b9439a4489d2b9adb1 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:16:01 +0200 Subject: [PATCH 201/253] Move kD under kP and kI --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 569039c5e..1dc72d9bf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -42,8 +42,8 @@ var MOTOR_CONFIG = TalonFXConfiguration().apply { SupplyCurrentLimitEnable = false } Slot0.apply { - kD = GAINS.kD kP = GAINS.kP kI = GAINS.kI + kD = GAINS.kD } } From 82348d5d4928c6e4d1edd74a3fbed35638a34ebc Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:16:49 +0200 Subject: [PATCH 202/253] Apply spotless --- .../subsystems/climber/ClimberConstants.kt | 47 ++++++++++--------- .../robot/subsystems/climber/ClimberIOReal.kt | 15 +++--- 2 files changed, 33 insertions(+), 29 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 1dc72d9bf..65af542e7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -23,27 +23,30 @@ val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) -var GAINS = selectGainsBasedOnMode( - Gains( - 0.0, - 0.0, - ), Gains( - 0.0, - 0.0, +var GAINS = + selectGainsBasedOnMode( + Gains( + 0.0, + 0.0, + ), + Gains( + 0.0, + 0.0, + ) ) -) -var MOTOR_CONFIG = TalonFXConfiguration().apply { - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake - Inverted = InvertedValue.Clockwise_Positive +var MOTOR_CONFIG = + TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + Inverted = InvertedValue.Clockwise_Positive + } + CurrentLimits.apply { + StatorCurrentLimitEnable = false + SupplyCurrentLimitEnable = false + } + Slot0.apply { + kP = GAINS.kP + kI = GAINS.kI + kD = GAINS.kD + } } - CurrentLimits.apply { - StatorCurrentLimitEnable = false - SupplyCurrentLimitEnable = false - } - Slot0.apply { - kP = GAINS.kP - kI = GAINS.kI - kD = GAINS.kD - } -} diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 019d20f7f..3fcaf5431 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -14,8 +14,7 @@ import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj.AnalogInput import frc.robot.lib.motors.LinearServo - -class ClimberIOReal:ClimberIO { +class ClimberIOReal : ClimberIO { override var inputs: LoggedClimberInputs = LoggedClimberInputs() private val mainMotor = TalonFX(MAIN_MOTOR_ID) @@ -31,12 +30,14 @@ class ClimberIOReal:ClimberIO { private val distanceFilter = MedianFilter(3) init { - listOf(auxMotor, mainMotor).forEach { it.apply {MOTOR_CONFIG} } + listOf(auxMotor, mainMotor).forEach { it.apply { MOTOR_CONFIG } } auxMotor.setControl(Follower(mainMotor.deviceID, true)) } override fun setLatchPosition(position: Distance) { - listOf(servo2, servo1).forEach { it.position = position.`in`(Units.Millimeters) } + listOf(servo2, servo1).forEach { + it.position = position.`in`(Units.Millimeters) + } } override fun setVoltage(voltage: Voltage) { @@ -53,7 +54,6 @@ class ClimberIOReal:ClimberIO { override fun openStopper() { stopperMotor.set(ControlMode.Position, UNLOCK_ANGLE.`in`(Units.Radians)) - } override fun updateInput() { @@ -61,10 +61,11 @@ class ClimberIOReal:ClimberIO { inputs.appliedVoltage = mainMotor.supplyVoltage.value inputs.latchPosition = Units.Millimeters.of(servo1.position) - var calculatedDistance = distanceFilter.calculate(4800 / (200 * sensor.voltage - 20.0)) + var calculatedDistance = + distanceFilter.calculate(4800 / (200 * sensor.voltage - 20.0)) if (calculatedDistance < 0) { calculatedDistance = 80.0 } inputs.sensorDistance = Units.Meters.of(calculatedDistance) } -} \ No newline at end of file +} From 6c83e2e1f2f5870d202f51d84d8e34461f70857f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:26:53 +0200 Subject: [PATCH 203/253] Update Linear servos initialization --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 3fcaf5431..c3b89a583 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -19,8 +19,8 @@ class ClimberIOReal : ClimberIO { private val mainMotor = TalonFX(MAIN_MOTOR_ID) private val auxMotor = TalonFX(AUX_MOTOR_ID) - private val servo1 = LinearServo(LATCH_SERVO_ID, 1, 1) - private val servo2 = LinearServo(FOLLOW_LATCH_SERVO_ID, 1, 1) + private val servo1 = LinearServo(LATCH_SERVO_ID, 1, 1, LATCH_TOLERANCE.`in`(Units.Millimeters)) + private val servo2 = LinearServo(FOLLOW_LATCH_SERVO_ID, 1, 1, LATCH_TOLERANCE.`in`(Units.Millimeters)) private val stopperMotor = TalonSRX(STOPPER_MOTOR_ID) private val voltageControl = VoltageOut(0.0) From 1d5db83642bebcf203696c85367e43962168689f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:28:07 +0200 Subject: [PATCH 204/253] Apply spotless --- .../robot/subsystems/climber/ClimberIOReal.kt | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index c3b89a583..2b8422d13 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -19,8 +19,20 @@ class ClimberIOReal : ClimberIO { private val mainMotor = TalonFX(MAIN_MOTOR_ID) private val auxMotor = TalonFX(AUX_MOTOR_ID) - private val servo1 = LinearServo(LATCH_SERVO_ID, 1, 1, LATCH_TOLERANCE.`in`(Units.Millimeters)) - private val servo2 = LinearServo(FOLLOW_LATCH_SERVO_ID, 1, 1, LATCH_TOLERANCE.`in`(Units.Millimeters)) + private val servo1 = + LinearServo( + LATCH_SERVO_ID, + 1, + 1, + LATCH_TOLERANCE.`in`(Units.Millimeters) + ) + private val servo2 = + LinearServo( + FOLLOW_LATCH_SERVO_ID, + 1, + 1, + LATCH_TOLERANCE.`in`(Units.Millimeters) + ) private val stopperMotor = TalonSRX(STOPPER_MOTOR_ID) private val voltageControl = VoltageOut(0.0) From 3fb70f3a5c6bdae35b5376462dbcde3861303351 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:37:30 +0200 Subject: [PATCH 205/253] Fix merge problems --- .../robot/subsystems/climber/ClimberIOSim.kt | 23 ++++++++++--------- .../frc/robot/subsystems/climber/Port.kt | 8 ------- 2 files changed, 12 insertions(+), 19 deletions(-) delete mode 100644 src/main/kotlin/frc/robot/subsystems/climber/Port.kt diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index ea4df1180..88d41ec89 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -1,12 +1,13 @@ package frc.robot.subsystems.climber -import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage +import com.ctre.phoenix6.controls.VoltageOut import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.math.system.plant.LinearSystemId import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle -import edu.wpi.first.units.measure.Dimensionless +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.simulation.DCMotorSim @@ -15,12 +16,12 @@ import frc.robot.lib.motors.TalonFXSim import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { - override var inputs: LoggedInputClimber = LoggedInputClimber() + override var inputs: LoggedClimberInputs = LoggedClimberInputs() var motor = TalonFXSim( 2, GEAR_RATIO, - MOMENT_OF_INERTIA_MAIN.`in`(Units.KilogramSquareMeters), + MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, TalonType.KRAKEN ) @@ -35,10 +36,10 @@ class ClimberIOSim : ClimberIO { ), DCMotor.getBag(1) ) - var dutyCycle = DutyCycleOut(0.0) + var voltageControl = VoltageOut(0.0) var positionControler = PositionVoltage(0.0) - override fun setLatchPosition(position: Dimensionless) { + override fun setLatchPosition(position: Distance) { if (Double.equals(OPEN_LATCH_POSITION)) { listOf(servo2, servo1).forEach { it.output = true } } else { @@ -46,16 +47,16 @@ class ClimberIOSim : ClimberIO { } } - override fun lock() { + override fun closeStopper() { lockMotor.setAngle(LOCK_ANGLE.`in`(Units.Radians)) } - override fun unlock() { + override fun openStopper() { lockMotor.setAngle(UNLOCK_ANGLE.`in`(Units.Radians)) } - override fun setPower(power: Double) { - motor.setControl(dutyCycle.withOutput(power)) + override fun setVoltage(voltage: Voltage) { + motor.setControl(voltageControl.withOutput(voltage)) } override fun setAngle(angle: Angle) { @@ -66,7 +67,7 @@ class ClimberIOSim : ClimberIO { motor.update(Timer.getFPGATimestamp()) lockMotor.update(Timer.getFPGATimestamp()) inputs.angle = Units.Rotations.of(motor.position) - inputs.appliedVoltage = Units.Volt.of(motor.appliedVoltage) + inputs.appliedVoltage = motor.appliedVoltage if (servo1.output) { inputs.latchPosition = OPEN_LATCH_POSITION } else { diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt b/src/main/kotlin/frc/robot/subsystems/climber/Port.kt deleted file mode 100644 index 194b9e8dd..000000000 --- a/src/main/kotlin/frc/robot/subsystems/climber/Port.kt +++ /dev/null @@ -1,8 +0,0 @@ -package frc.robot.subsystems.climber - -const val MAIN_MOTOR_ID = 0 -const val AUX_MOTOR_ID = 1 -const val LATCH_SERVO_ID = 2 -const val FOLLOW_LATCH_SERVO_ID = 3 -const val SENSOR_ID = 4 -const val LOCK_MOTOR_ID = 5 From d2ed0872eea3d819f2da63ccc35f68b6e77cb461 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:40:27 +0200 Subject: [PATCH 206/253] Make variables private --- .../frc/robot/subsystems/climber/ClimberIOSim.kt | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 88d41ec89..afcd1db3a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -17,7 +17,8 @@ import frc.robot.lib.motors.TalonType class ClimberIOSim : ClimberIO { override var inputs: LoggedClimberInputs = LoggedClimberInputs() - var motor = + + private var motor = TalonFXSim( 2, GEAR_RATIO, @@ -25,9 +26,9 @@ class ClimberIOSim : ClimberIO { 1.0, TalonType.KRAKEN ) - var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) - var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) - val lockMotor = + private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + private val lockMotor = DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getBag(1), @@ -36,8 +37,8 @@ class ClimberIOSim : ClimberIO { ), DCMotor.getBag(1) ) - var voltageControl = VoltageOut(0.0) - var positionControler = PositionVoltage(0.0) + private var voltageControl = VoltageOut(0.0) + private var positionControler = PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { if (Double.equals(OPEN_LATCH_POSITION)) { From 19877b64268759913f4e835a6b9c23b8303bb339 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:40:59 +0200 Subject: [PATCH 207/253] Use ports as solenoid sim channels --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index afcd1db3a..b8e4a56f0 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -26,8 +26,8 @@ class ClimberIOSim : ClimberIO { 1.0, TalonType.KRAKEN ) - private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, 0) - private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, 0) + private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) + private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) private val lockMotor = DCMotorSim( LinearSystemId.createDCMotorSystem( From c977ee077a532f6444cf3509aec88dc850b95093 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:41:18 +0200 Subject: [PATCH 208/253] Fix typo --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index b8e4a56f0..ff880eaaf 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -38,7 +38,7 @@ class ClimberIOSim : ClimberIO { DCMotor.getBag(1) ) private var voltageControl = VoltageOut(0.0) - private var positionControler = PositionVoltage(0.0) + private var positionController = PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { if (Double.equals(OPEN_LATCH_POSITION)) { @@ -61,7 +61,7 @@ class ClimberIOSim : ClimberIO { } override fun setAngle(angle: Angle) { - motor.setControl(positionControler.withPosition(angle)) + motor.setControl(positionController.withPosition(angle)) } override fun updateInput() { From 192090f1d9f4d29dc720cf9c676e70a40609df8f Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:42:04 +0200 Subject: [PATCH 209/253] Use kraken foc --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index ff880eaaf..3df455416 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -24,7 +24,7 @@ class ClimberIOSim : ClimberIO { GEAR_RATIO, MOMENT_OF_INERTIA.`in`(Units.KilogramSquareMeters), 1.0, - TalonType.KRAKEN + TalonType.KRAKEN_FOC ) private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) From 41337ac2826056895cf696a85bd77e64b300727d Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:42:28 +0200 Subject: [PATCH 210/253] Rename lockMotor to stopperMotor --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 3df455416..cb0d73b28 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -28,7 +28,7 @@ class ClimberIOSim : ClimberIO { ) private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) - private val lockMotor = + private val stopperMotor = DCMotorSim( LinearSystemId.createDCMotorSystem( DCMotor.getBag(1), @@ -49,11 +49,11 @@ class ClimberIOSim : ClimberIO { } override fun closeStopper() { - lockMotor.setAngle(LOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.setAngle(LOCK_ANGLE.`in`(Units.Radians)) } override fun openStopper() { - lockMotor.setAngle(UNLOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.setAngle(UNLOCK_ANGLE.`in`(Units.Radians)) } override fun setVoltage(voltage: Voltage) { @@ -66,7 +66,7 @@ class ClimberIOSim : ClimberIO { override fun updateInput() { motor.update(Timer.getFPGATimestamp()) - lockMotor.update(Timer.getFPGATimestamp()) + stopperMotor.update(Timer.getFPGATimestamp()) inputs.angle = Units.Rotations.of(motor.position) inputs.appliedVoltage = motor.appliedVoltage if (servo1.output) { From 36e8ebfe2a19bccfca50e025339ee0823376714d Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 14:49:34 +0200 Subject: [PATCH 211/253] Reorder constants and add STOPPER_GEAR_RATIO --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e230724bd..80495ea9f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -6,6 +6,8 @@ import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia import edu.wpi.first.units.measure.Voltage +const val GEAR_RATIO = 1.0 +const val STOPPER_GEAR_RATIO = 1.0 val UNLOCK_VOLTAGE: Voltage = Units.Volts.of(0.0) val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) @@ -16,7 +18,5 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) - -const val GEAR_RATIO = 1.0 val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) From 0803705315fb26400e9fdca7185f8fd888a88e44 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:10:20 +0200 Subject: [PATCH 212/253] Add stopperMotorCurrent input --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index fa440603f..5cf1b27ea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Current import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import org.team9432.annotation.Logged @@ -18,6 +19,7 @@ interface ClimberIO { @Logged open class ClimberInputs { var appliedVoltage: Voltage = Units.Volts.zero() + var stopperMotorCurrent: Current = Units.Amps.zero() var angle: Angle = Units.Degree.zero() var latchPosition: Distance = Units.Millimeters.zero() var sensorDistance: Distance = Units.Centimeter.zero() From eb3ec20783e191f441bb581955f765c23f6ac27d Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:10:37 +0200 Subject: [PATCH 213/253] Add STOPPER_CURRENT_THRESHOLD constant --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 7168a78d7..1903f1ec9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.climber import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle +import edu.wpi.first.units.measure.Current import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia import edu.wpi.first.units.measure.Voltage @@ -15,3 +16,4 @@ val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) +val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) From 799ccb84324a477eb48ed7df51f444a777e8704b Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:10:55 +0200 Subject: [PATCH 214/253] Add isStopperStuck trigger --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 8bc2b80d5..a25a8575e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -18,11 +18,17 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private var isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } + @AutoLogOutput private var isLatchClosed = Trigger { inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } + @AutoLogOutput + private var isStopperStuck = Trigger { + inputs.stopperMotorCurrent.abs(Units.Amps) >= STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) + } + @AutoLogOutput private var isAttached = isLatchClosed.and(isTouching) From d536cfc96e2a9246362bce4f85e82a45b35cb8fc Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:11:59 +0200 Subject: [PATCH 215/253] Add stopStopper IO function --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index 5cf1b27ea..ac8c72dc7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -14,6 +14,7 @@ interface ClimberIO { fun setAngle(angle: Angle) {} fun closeStopper() {} fun openStopper() {} + fun stopStopper() {} fun updateInput() {} @Logged From 1422ff89805a37b304a4e1193105c6234de2f8f3 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:20:17 +0200 Subject: [PATCH 216/253] Use isStopperStuck in lock and unlock commands --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index a25a8575e..13e969861 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -53,10 +53,11 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun openLatch(): Command = setLatchPose(OPEN_LATCH_POSITION) - fun lock(): Command = runOnce { io.closeStopper() } + fun lock(): Command = + run(io::closeStopper).until(isStopperStuck).andThen(io::stopStopper) fun unlock(): Command = - setVoltage(UNLOCK_VOLTAGE).withTimeout(0.15).andThen(io::openStopper) + run(io::openStopper).until(isStopperStuck).andThen(io::stopStopper) fun unfold() = setAngle(UNFOLDED_ANGLE) From 2067dc27b4fa5b233ebd360b6601b702da8c48bf Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:20:32 +0200 Subject: [PATCH 217/253] Delete unnecessary constant --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 1903f1ec9..08f542abe 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -5,9 +5,7 @@ import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Current import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia -import edu.wpi.first.units.measure.Voltage -val UNLOCK_VOLTAGE: Voltage = Units.Volts.of(0.0) val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) From aa1371d270f1e2dcc243104242892c1823910129 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:20:45 +0200 Subject: [PATCH 218/253] Apply spotless --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 13e969861..c68ff483a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -26,7 +26,8 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var isStopperStuck = Trigger { - inputs.stopperMotorCurrent.abs(Units.Amps) >= STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) + inputs.stopperMotorCurrent.abs(Units.Amps) >= + STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) } @AutoLogOutput From f81289ce07f4c88e9844116f87451d67b4711762 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:32:47 +0200 Subject: [PATCH 219/253] Use percent output for the stopper motor --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 5 ++--- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 4 ++-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 28555ca39..2f580078a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -8,15 +8,14 @@ import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Current import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.MomentOfInertia -import edu.wpi.first.units.measure.Voltage import frc.robot.lib.Gains import frc.robot.lib.selectGainsBasedOnMode val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) -val LOCK_ANGLE: Angle = Units.Degrees.of(90.0) -val UNLOCK_ANGLE: Angle = Units.Degrees.of(10.0) +val LOCK_POWER = 0.0 +val UNLOCK_POWER = 0.0 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 2b8422d13..41e4d39ea 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -61,11 +61,11 @@ class ClimberIOReal : ClimberIO { } override fun closeStopper() { - stopperMotor.set(ControlMode.Position, LOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.set(ControlMode.PercentOutput, LOCK_POWER) } override fun openStopper() { - stopperMotor.set(ControlMode.Position, UNLOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.set(ControlMode.PercentOutput, UNLOCK_POWER) } override fun updateInput() { From a2b92c39ee170b2e206618cc9eb959ddaef573d3 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:41:28 +0200 Subject: [PATCH 220/253] Use TalonFXSim for stopper motor --- .../robot/subsystems/climber/ClimberIOSim.kt | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index cb0d73b28..5a0303263 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -3,14 +3,12 @@ package frc.robot.subsystems.climber import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.VoltageOut import edu.wpi.first.math.system.plant.DCMotor -import edu.wpi.first.math.system.plant.LinearSystemId import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj.PneumaticsModuleType import edu.wpi.first.wpilibj.Timer -import edu.wpi.first.wpilibj.simulation.DCMotorSim import edu.wpi.first.wpilibj.simulation.SolenoidSim import frc.robot.lib.motors.TalonFXSim import frc.robot.lib.motors.TalonType @@ -29,13 +27,11 @@ class ClimberIOSim : ClimberIO { private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) private val stopperMotor = - DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getBag(1), - MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), - 1.0 - ), - DCMotor.getBag(1) + TalonFXSim( + DCMotor.getBag(1), + STOPPER_GEAR_RATIO, + MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), + 1.0 ) private var voltageControl = VoltageOut(0.0) private var positionController = PositionVoltage(0.0) @@ -49,11 +45,11 @@ class ClimberIOSim : ClimberIO { } override fun closeStopper() { - stopperMotor.setAngle(LOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.setControl(voltageControl.withOutput(LOCK_POWER * 12)) } override fun openStopper() { - stopperMotor.setAngle(UNLOCK_ANGLE.`in`(Units.Radians)) + stopperMotor.setControl(voltageControl.withOutput(UNLOCK_POWER * 12)) } override fun setVoltage(voltage: Voltage) { From b31a2383024fe73089c28c4b321f8d3aa9dd6299 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 15:42:00 +0200 Subject: [PATCH 221/253] Apply spotless --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 5a0303263..a29b864b7 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -25,7 +25,8 @@ class ClimberIOSim : ClimberIO { TalonType.KRAKEN_FOC ) private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) - private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) + private var servo2 = + SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) private val stopperMotor = TalonFXSim( DCMotor.getBag(1), From defbd3b1a9b3c3d5fe41201dba289604e8358172 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Mon, 20 Jan 2025 16:16:27 +0200 Subject: [PATCH 222/253] Add kg Trigger --- .../subsystems/elevator/ElevatorConstants.kt | 3 ++- .../subsystems/elevator/ElevatorIOReal.kt | 22 ++++++++++++++----- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt index 5f21ea554..f4c122e78 100644 --- a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt @@ -12,6 +12,7 @@ const val GEAR_RATIO = (1.0 / 12.0) * (42.0 / 48.0) const val FIRST_STAGE_RATIO = 2.0 const val ENCODER_OFFSET = 0.0 const val ADJUSTED_GEAR_RATIO = FIRST_STAGE_RATIO * GEAR_RATIO +val MIN_KG_HEIGHT: Distance = Units.Centimeters.of(3.0) val SPROCKET_RADIUS: Distance = Units.Millimeters.of(36.4 / 2) private val ROTATIONS_TO_CENTIMETERS_RATIO = GEAR_RATIO * @@ -22,7 +23,7 @@ val ROTATIONS_TO_CENTIMETER: Measure> = val CENTIMETERS_TO_ROTATIONS: Measure> = Units.Rotations.per(Units.Centimeter).of(1 / ROTATIONS_TO_CENTIMETERS_RATIO) -val GAINS = selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0), Gains()) +val GAINS = selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0, kG = 1.0), Gains()) enum class Positions(val value: Distance) { L1(Units.Centimeters.of(25.0)), diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt index 80736a6a7..09e979c59 100644 --- a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt @@ -8,8 +8,12 @@ import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.lib.toAngle import frc.robot.lib.toDistance +import org.littletonrobotics.junction.AutoLogOutput class ElevatorIOReal : ElevatorIO { override val inputs = LoggedElevatorInputs() @@ -17,6 +21,15 @@ class ElevatorIOReal : ElevatorIO { private val auxMotor = TalonFX(AUX_ID) private val encoder = CANcoder(ENCODER_ID) private val mainMotorPositionRequest = MotionMagicTorqueCurrentFOC(0.0) + private val slot0Configs = Slot0Configs().apply { + kP = GAINS.kP + kI = GAINS.kI + kD = GAINS.kD + kG = 0.0 + } + + @AutoLogOutput + private val kgTrigger = Trigger { inputs.height > MIN_KG_HEIGHT }.onTrue(setKG(GAINS.kG)).onFalse(setKG(0.0)) init { val motorConfig = @@ -27,12 +40,7 @@ class ElevatorIOReal : ElevatorIO { Inverted = InvertedValue.Clockwise_Positive } Feedback = FeedbackConfigs().apply { RotorToSensorRatio = 1.0 } - Slot0 = - Slot0Configs().apply { - kP = GAINS.kP - kI = GAINS.kI - kD = GAINS.kD - } + Slot0 = slot0Configs CurrentLimits = CurrentLimitsConfigs().apply { StatorCurrentLimitEnable = true @@ -55,6 +63,8 @@ class ElevatorIOReal : ElevatorIO { encoder.configurator.apply(encoderConfig) } + private fun setKG(kg: Double): Command = runOnce({ mainMotor.configurator.apply(slot0Configs.withKG(kg)) }) + override fun setHeight(height: Distance) { mainMotor.setControl( mainMotorPositionRequest.withPosition( From 3dd48662c31420e022e0c81f725c26276c1984e3 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 20:50:28 +0200 Subject: [PATCH 223/253] Add isUnfolded trigger --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index c68ff483a..eb8c9c557 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -38,6 +38,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(FOLDED_ANGLE, FOLDED_TOLERANCE) } + @AutoLogOutput + private val isUnfolded = Trigger { + inputs.angle.isNear(UNFOLDED_ANGLE, FOLDED_TOLERANCE) + } private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) } private fun setVoltage(voltage: Voltage): Command = From e07679e9adb04b52545bf4d99110be350a9bb925 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 20:51:02 +0200 Subject: [PATCH 224/253] Add wait commands in climb and declimb commands --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index eb8c9c557..c16341ceb 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -42,6 +42,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { private val isUnfolded = Trigger { inputs.angle.isNear(UNFOLDED_ANGLE, FOLDED_TOLERANCE) } + + @AutoLogOutput + private var setpoint = + private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) } private fun setVoltage(voltage: Voltage): Command = @@ -68,9 +72,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun fold() = setAngle(FOLDED_ANGLE) - fun climb(): Command = Commands.sequence(closeLatch(), fold(), lock()) + fun climb(): Command = + Commands.sequence(closeLatch(), Commands.waitUntil(isLatchClosed), fold(), Commands.waitUntil(isFolded), lock()) - fun declimb(): Command = Commands.sequence(unlock(), unfold(), openLatch()) + fun declimb(): Command = Commands.sequence(unlock(), unfold(), Commands.waitUntil(isUnfolded), openLatch()) override fun periodic() { io.updateInput() From 1ce01630ad2a16b8608fc219373cc58daa9ae72d Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:03:51 +0200 Subject: [PATCH 225/253] Change triggers to values --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index c16341ceb..119e166cc 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,23 +15,23 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private var isTouching = Trigger { + private val isTouching = Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD } @AutoLogOutput - private var isLatchClosed = Trigger { + private val isLatchClosed = Trigger { inputs.latchPosition.isNear(CLOSE_LATCH_POSITION, LATCH_TOLERANCE) } @AutoLogOutput - private var isStopperStuck = Trigger { + private val isStopperStuck = Trigger { inputs.stopperMotorCurrent.abs(Units.Amps) >= STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) } @AutoLogOutput - private var isAttached = isLatchClosed.and(isTouching) + private val isAttached = isLatchClosed.and(isTouching) @AutoLogOutput private val isFolded = Trigger { From 2caea5e3ae57b658c30501f0bd073612aea95634 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:04:27 +0200 Subject: [PATCH 226/253] Add setpoint variable --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 119e166cc..c9d9bc55c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -44,9 +44,13 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } @AutoLogOutput - private var setpoint = + private var setpoint = Units.Rotations.zero() - private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) } + private fun setAngle(angle: Angle): Command = + runOnce { + io.setAngle(angle) + setpoint = angle + } private fun setVoltage(voltage: Voltage): Command = Commands.startEnd( From c9142dc067fba0dc5f3f2c0c0f30c2e3b2204436 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:04:40 +0200 Subject: [PATCH 227/253] Add atSetpoint trigger --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index c9d9bc55c..e2a053f1f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -43,6 +43,11 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(UNFOLDED_ANGLE, FOLDED_TOLERANCE) } + @AutoLogOutput + private val atSetpoint = Trigger { + inputs.angle.isNear(setpoint, FOLDED_TOLERANCE) + } + @AutoLogOutput private var setpoint = Units.Rotations.zero() From db54b0b5dd14cb6bf965eef77ab2bea795957df1 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:08:43 +0200 Subject: [PATCH 228/253] Specify parameter name when initializing gains --- .../frc/robot/subsystems/climber/ClimberConstants.kt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 2f580078a..e7bb468ad 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -26,12 +26,14 @@ val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) var GAINS = selectGainsBasedOnMode( Gains( - 0.0, - 0.0, + kP = 0.0, + kI = 0.0, + kD = 0.0 ), Gains( - 0.0, - 0.0, + kP = 0.0, + kI = 0.0, + kD = 0.0 ) ) var MOTOR_CONFIG = From 90c8b3a2ed6fa90091de606c0c3ffca5b163ee23 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:08:54 +0200 Subject: [PATCH 229/253] Add current limits --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index e7bb468ad..7bea0c32f 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -43,8 +43,10 @@ var MOTOR_CONFIG = Inverted = InvertedValue.Clockwise_Positive } CurrentLimits.apply { - StatorCurrentLimitEnable = false - SupplyCurrentLimitEnable = false + StatorCurrentLimitEnable = true + SupplyCurrentLimitEnable = true + StatorCurrentLimit = 160.0 + SupplyCurrentLimit = 80.0 } Slot0.apply { kP = GAINS.kP From c19b280b5a9a81f12fa00acc2ac6ac3ccbf09ade Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:11:02 +0200 Subject: [PATCH 230/253] Apply spotless --- .../frc/robot/subsystems/climber/Climber.kt | 25 +++++++++++++------ .../subsystems/climber/ClimberConstants.kt | 12 ++------- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index e2a053f1f..db00d6bc0 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -51,11 +51,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var setpoint = Units.Rotations.zero() - private fun setAngle(angle: Angle): Command = - runOnce { - io.setAngle(angle) - setpoint = angle - } + private fun setAngle(angle: Angle): Command = runOnce { + io.setAngle(angle) + setpoint = angle + } private fun setVoltage(voltage: Voltage): Command = Commands.startEnd( @@ -82,9 +81,21 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun fold() = setAngle(FOLDED_ANGLE) fun climb(): Command = - Commands.sequence(closeLatch(), Commands.waitUntil(isLatchClosed), fold(), Commands.waitUntil(isFolded), lock()) + Commands.sequence( + closeLatch(), + Commands.waitUntil(isLatchClosed), + fold(), + Commands.waitUntil(isFolded), + lock() + ) - fun declimb(): Command = Commands.sequence(unlock(), unfold(), Commands.waitUntil(isUnfolded), openLatch()) + fun declimb(): Command = + Commands.sequence( + unlock(), + unfold(), + Commands.waitUntil(isUnfolded), + openLatch() + ) override fun periodic() { io.updateInput() diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 7bea0c32f..ace4df5ef 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -25,16 +25,8 @@ val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) var GAINS = selectGainsBasedOnMode( - Gains( - kP = 0.0, - kI = 0.0, - kD = 0.0 - ), - Gains( - kP = 0.0, - kI = 0.0, - kD = 0.0 - ) + Gains(kP = 0.0, kI = 0.0, kD = 0.0), + Gains(kP = 0.0, kI = 0.0, kD = 0.0) ) var MOTOR_CONFIG = TalonFXConfiguration().apply { From 25486fad09ee929a075111c7b2d78d1530147365 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:31:16 +0200 Subject: [PATCH 231/253] Replace IO stopper functions with setStopperPower --- .../frc/robot/subsystems/climber/Climber.kt | 16 ++++++++++++++-- .../robot/subsystems/climber/ClimberConstants.kt | 2 ++ .../frc/robot/subsystems/climber/ClimberIO.kt | 4 +--- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index e2a053f1f..40e000fe1 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -67,15 +67,27 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { io.setLatchPosition(latchPose) } + private fun setStopperPower(power: Double): Command = runOnce { + io.setStopperPower(power) + } + fun closeLatch(): Command = setLatchPose(CLOSE_LATCH_POSITION) fun openLatch(): Command = setLatchPose(OPEN_LATCH_POSITION) fun lock(): Command = - run(io::closeStopper).until(isStopperStuck).andThen(io::stopStopper) + Commands.sequence( + setStopperPower(LOCK_POWER), + Commands.waitUntil(isStopperStuck), + setStopperPower(0.0) + ) fun unlock(): Command = - run(io::openStopper).until(isStopperStuck).andThen(io::stopStopper) + Commands.sequence( + setStopperPower(UNLOCK_POWER), + Commands.waitUntil(isStopperStuck), + setStopperPower(0.0) + ) fun unfold() = setAngle(UNFOLDED_ANGLE) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 08f542abe..3eb681479 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -9,6 +9,8 @@ import edu.wpi.first.units.measure.MomentOfInertia val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) +val LOCK_POWER = 0.0 +val UNLOCK_POWER = 0.0 val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt index ac8c72dc7..bec291848 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIO.kt @@ -12,9 +12,7 @@ interface ClimberIO { fun setLatchPosition(position: Distance) {} fun setVoltage(voltage: Voltage) {} fun setAngle(angle: Angle) {} - fun closeStopper() {} - fun openStopper() {} - fun stopStopper() {} + fun setStopperPower(power: Double) {} fun updateInput() {} @Logged From f10c60f8d091d62f3afbd610502476c69715e82a Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:31:40 +0200 Subject: [PATCH 232/253] Apply spotless --- .../frc/robot/subsystems/climber/Climber.kt | 25 +++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 40e000fe1..04ead1180 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -51,11 +51,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var setpoint = Units.Rotations.zero() - private fun setAngle(angle: Angle): Command = - runOnce { - io.setAngle(angle) - setpoint = angle - } + private fun setAngle(angle: Angle): Command = runOnce { + io.setAngle(angle) + setpoint = angle + } private fun setVoltage(voltage: Voltage): Command = Commands.startEnd( @@ -94,9 +93,21 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { fun fold() = setAngle(FOLDED_ANGLE) fun climb(): Command = - Commands.sequence(closeLatch(), Commands.waitUntil(isLatchClosed), fold(), Commands.waitUntil(isFolded), lock()) + Commands.sequence( + closeLatch(), + Commands.waitUntil(isLatchClosed), + fold(), + Commands.waitUntil(isFolded), + lock() + ) - fun declimb(): Command = Commands.sequence(unlock(), unfold(), Commands.waitUntil(isUnfolded), openLatch()) + fun declimb(): Command = + Commands.sequence( + unlock(), + unfold(), + Commands.waitUntil(isUnfolded), + openLatch() + ) override fun periodic() { io.updateInput() From 0158e42d690062e05ecf20c0ebb86832f153bfb2 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:33:10 +0200 Subject: [PATCH 233/253] Replace IO stopper functions with setStopperPower --- .../kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt index 41e4d39ea..7c2574227 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOReal.kt @@ -60,12 +60,8 @@ class ClimberIOReal : ClimberIO { mainMotor.setControl(positionControl.withPosition(angle)) } - override fun closeStopper() { - stopperMotor.set(ControlMode.PercentOutput, LOCK_POWER) - } - - override fun openStopper() { - stopperMotor.set(ControlMode.PercentOutput, UNLOCK_POWER) + override fun setStopperPower(power: Double) { + stopperMotor.set(ControlMode.PercentOutput, power) } override fun updateInput() { From f03b9b03553740e63d03f082fdeb071bc8588142 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:44:30 +0200 Subject: [PATCH 234/253] Replace IO stopper functions with setStopperPower --- .../frc/robot/subsystems/climber/ClimberIOSim.kt | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index a29b864b7..a55cd47e9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -1,5 +1,6 @@ package frc.robot.subsystems.climber +import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.VoltageOut import edu.wpi.first.math.system.plant.DCMotor @@ -35,6 +36,7 @@ class ClimberIOSim : ClimberIO { 1.0 ) private var voltageControl = VoltageOut(0.0) + private var powerControl = DutyCycleOut(0.0) private var positionController = PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { @@ -45,12 +47,8 @@ class ClimberIOSim : ClimberIO { } } - override fun closeStopper() { - stopperMotor.setControl(voltageControl.withOutput(LOCK_POWER * 12)) - } - - override fun openStopper() { - stopperMotor.setControl(voltageControl.withOutput(UNLOCK_POWER * 12)) + override fun setStopperPower(power: Double) { + stopperMotor.setControl(powerControl.withOutput(power)) } override fun setVoltage(voltage: Voltage) { From 1f4b20c5c98444ba759fec7049adf3984d4fdf37 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:47:26 +0200 Subject: [PATCH 235/253] Fix check in setLatchPosition --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index a55cd47e9..37f64b4ff 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -40,7 +40,7 @@ class ClimberIOSim : ClimberIO { private var positionController = PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { - if (Double.equals(OPEN_LATCH_POSITION)) { + if (position == OPEN_LATCH_POSITION) { listOf(servo2, servo1).forEach { it.output = true } } else { listOf(servo2, servo1).forEach { it.output = false } From d41d76da235bac7d8aed75f1c316000b95214e64 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:47:41 +0200 Subject: [PATCH 236/253] Add line separators --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 37f64b4ff..c540cba45 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -25,9 +25,11 @@ class ClimberIOSim : ClimberIO { 1.0, TalonType.KRAKEN_FOC ) + private var servo1 = SolenoidSim(PneumaticsModuleType.REVPH, LATCH_SERVO_ID) private var servo2 = SolenoidSim(PneumaticsModuleType.REVPH, FOLLOW_LATCH_SERVO_ID) + private val stopperMotor = TalonFXSim( DCMotor.getBag(1), @@ -35,6 +37,7 @@ class ClimberIOSim : ClimberIO { MOMENT_OF_INERTIA_LOCK.`in`(Units.KilogramSquareMeters), 1.0 ) + private var voltageControl = VoltageOut(0.0) private var powerControl = DutyCycleOut(0.0) private var positionController = PositionVoltage(0.0) From e613d321a4d58fd7fe7d535ff82a61af396a3fbc Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:50:05 +0200 Subject: [PATCH 237/253] Make latch position update more compact --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index c540cba45..422698270 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -67,10 +67,7 @@ class ClimberIOSim : ClimberIO { stopperMotor.update(Timer.getFPGATimestamp()) inputs.angle = Units.Rotations.of(motor.position) inputs.appliedVoltage = motor.appliedVoltage - if (servo1.output) { - inputs.latchPosition = OPEN_LATCH_POSITION - } else { - inputs.latchPosition = CLOSE_LATCH_POSITION - } + inputs.latchPosition = + if (servo1.output) OPEN_LATCH_POSITION else CLOSE_LATCH_POSITION } } From 8ac031f514728e9940abcd426a990e6722725455 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:54:51 +0200 Subject: [PATCH 238/253] Make setLatchPosition more compact --- .../kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 422698270..93a053245 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -43,10 +43,8 @@ class ClimberIOSim : ClimberIO { private var positionController = PositionVoltage(0.0) override fun setLatchPosition(position: Distance) { - if (position == OPEN_LATCH_POSITION) { - listOf(servo2, servo1).forEach { it.output = true } - } else { - listOf(servo2, servo1).forEach { it.output = false } + listOf(servo1, servo2).forEach { + it.output = position == OPEN_LATCH_POSITION } } From 9162763c0aec8cd827907e3213c31f1023e5d47e Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 21:58:27 +0200 Subject: [PATCH 239/253] Debounce isTouching trigger --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 04ead1180..d9587e099 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -15,9 +15,8 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @AutoLogOutput - private val isTouching = Trigger { - inputs.sensorDistance < DISTANCE_THRESHOLD - } + private val isTouching = + Trigger { inputs.sensorDistance < DISTANCE_THRESHOLD }.debounce(1.0) @AutoLogOutput private val isLatchClosed = Trigger { From 8430c00e51187ebbeb775f849bc2e84e2461505d Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 23:13:44 +0200 Subject: [PATCH 240/253] Add mechanism2d --- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index d9587e099..af9ee9f7c 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -10,6 +10,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase import edu.wpi.first.wpilibj2.command.button.Trigger import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger +import org.littletonrobotics.junction.mechanism.LoggedMechanism2d +import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d class Climber(private val io: ClimberIO) : SubsystemBase() { var inputs = io.inputs @@ -50,6 +52,12 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private var setpoint = Units.Rotations.zero() + @AutoLogOutput + private var mechanism = LoggedMechanism2d(3.0, 2.0) + private var root = mechanism.getRoot("Climber", 1.0, 1.0) + private val ligament = + root.append(LoggedMechanismLigament2d("ClimberLigament", 0.27003, 90.0)) + private fun setAngle(angle: Angle): Command = runOnce { io.setAngle(angle) setpoint = angle @@ -111,5 +119,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { override fun periodic() { io.updateInput() Logger.processInputs(this::class.simpleName, io.inputs) + + ligament.angle = inputs.angle.`in`(Units.Degrees) } } From e28f3c5701821e27201cffdb3390a0fd3afb9f4e Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Mon, 20 Jan 2025 23:44:17 +0200 Subject: [PATCH 241/253] Use subsystem's startEnd --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index af9ee9f7c..4e138a644 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -64,7 +64,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { } private fun setVoltage(voltage: Voltage): Command = - Commands.startEnd( + startEnd( { io.setVoltage(voltage) }, { io.setVoltage(Units.Volts.zero()) } ) From cdec4ad26d75778c160712f04c53320118b95a66 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 00:07:44 +0200 Subject: [PATCH 242/253] Adjust constants --- .../robot/subsystems/climber/ClimberConstants.kt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 8a5833697..166b156b5 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -11,25 +11,25 @@ import edu.wpi.first.units.measure.MomentOfInertia import frc.robot.lib.Gains import frc.robot.lib.selectGainsBasedOnMode -const val GEAR_RATIO = 1.0 -const val STOPPER_GEAR_RATIO = 1.0 +const val GEAR_RATIO = (1.0 / 12.0) * (30.0 / 66.0) * (12.0 / 36.0) +const val STOPPER_GEAR_RATIO = (8.0 / 72.0) * (1.0 / 15.0) val OPEN_LATCH_POSITION: Distance = Units.Millimeters.of(0.8) val CLOSE_LATCH_POSITION: Distance = Units.Millimeters.of(0.2) val LATCH_TOLERANCE: Distance = Units.Millimeters.of(1.0) val LOCK_POWER = 0.0 val UNLOCK_POWER = 0.0 -val UNFOLDED_ANGLE: Angle = Units.Degree.of(60.0) -val FOLDED_ANGLE: Angle = Units.Degree.of(30.0) +val UNFOLDED_ANGLE: Angle = Units.Degree.of(0.0) +val FOLDED_ANGLE: Angle = Units.Degree.of(90.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) -val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) -val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.0) +val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.003) +val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.003) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) var GAINS = selectGainsBasedOnMode( Gains(kP = 0.0, kI = 0.0, kD = 0.0), - Gains(kP = 0.0, kI = 0.0, kD = 0.0) + Gains(kP = 0.5, kI = 0.0, kD = 0.08) ) var MOTOR_CONFIG = TalonFXConfiguration().apply { From a5b08c24bca2b15abe74dd83ff50716c647b4389 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 00:18:18 +0200 Subject: [PATCH 243/253] Add PID controller to sim motor --- src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt index 93a053245..b828f53d8 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberIOSim.kt @@ -3,6 +3,7 @@ package frc.robot.subsystems.climber import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.controls.VoltageOut +import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Angle @@ -42,6 +43,10 @@ class ClimberIOSim : ClimberIO { private var powerControl = DutyCycleOut(0.0) private var positionController = PositionVoltage(0.0) + init { + motor.setController(PIDController(GAINS.kP, GAINS.kI, GAINS.kD)) + } + override fun setLatchPosition(position: Distance) { listOf(servo1, servo2).forEach { it.output = position == OPEN_LATCH_POSITION From 37ae8cfda2257649744dd5d77eacf58b7043f0e4 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 00:29:59 +0200 Subject: [PATCH 244/253] Log mechanism and setpoint --- src/main/kotlin/frc/robot/subsystems/climber/Climber.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index 4e138a644..a9845ae42 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -121,5 +121,7 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { Logger.processInputs(this::class.simpleName, io.inputs) ligament.angle = inputs.angle.`in`(Units.Degrees) + Logger.recordOutput("Climber/Mechanism2d", mechanism) + Logger.recordOutput("Climber/setpoint", setpoint) } } From 67955e5b0f8b83a8d75abf588d1124d95196f646 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 00:30:18 +0200 Subject: [PATCH 245/253] Calibrate PID --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index 166b156b5..aaedd8b4a 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -29,7 +29,7 @@ val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) var GAINS = selectGainsBasedOnMode( Gains(kP = 0.0, kI = 0.0, kD = 0.0), - Gains(kP = 0.5, kI = 0.0, kD = 0.08) + Gains(kP = 0.015, kI = 0.0, kD = 0.045) ) var MOTOR_CONFIG = TalonFXConfiguration().apply { From b4b4eef4f2a09024b044269b30decce18f9cb347 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 07:55:08 +0200 Subject: [PATCH 246/253] Apply spotless --- .../kotlin/frc/robot/subsystems/climber/ClimberConstants.kt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt index aaedd8b4a..08b190c83 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/ClimberConstants.kt @@ -22,7 +22,8 @@ val UNFOLDED_ANGLE: Angle = Units.Degree.of(0.0) val FOLDED_ANGLE: Angle = Units.Degree.of(90.0) val FOLDED_TOLERANCE: Angle = Units.Degree.of(1.0) val MOMENT_OF_INERTIA: MomentOfInertia = Units.KilogramSquareMeters.of(0.003) -val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = Units.KilogramSquareMeters.of(0.003) +val MOMENT_OF_INERTIA_LOCK: MomentOfInertia = + Units.KilogramSquareMeters.of(0.003) val DISTANCE_THRESHOLD: Distance = Units.Centimeter.of(0.4) val STOPPER_CURRENT_THRESHOLD: Current = Units.Amps.of(0.0) From 95474d46de83a6430b6d02b51fdefa2cf2dbfe00 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 09:22:23 +0200 Subject: [PATCH 247/253] Initialize subsystems --- src/main/kotlin/frc/robot/Initializer.kt | 104 +++++++++++++++++++- src/main/kotlin/frc/robot/RobotContainer.kt | 7 ++ 2 files changed, 110 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/Initializer.kt b/src/main/kotlin/frc/robot/Initializer.kt index 6f1d3690e..95c43d92c 100644 --- a/src/main/kotlin/frc/robot/Initializer.kt +++ b/src/main/kotlin/frc/robot/Initializer.kt @@ -1,10 +1,46 @@ package frc.robot -import frc.robot.subsystems.drive.* +import frc.robot.subsystems.climber.Climber +import frc.robot.subsystems.climber.ClimberIO +import frc.robot.subsystems.climber.ClimberIOReal +import frc.robot.subsystems.climber.ClimberIOSim +import frc.robot.subsystems.climber.LoggedClimberInputs +import frc.robot.subsystems.drive.Drive +import frc.robot.subsystems.drive.GyroIO +import frc.robot.subsystems.drive.GyroIONavX +import frc.robot.subsystems.drive.ModuleIO +import frc.robot.subsystems.drive.ModuleIOSim +import frc.robot.subsystems.drive.ModuleIOTalonFX +import frc.robot.subsystems.drive.TunerConstants +import frc.robot.subsystems.elevator.Elevator +import frc.robot.subsystems.elevator.ElevatorIO +import frc.robot.subsystems.elevator.ElevatorIOReal +import frc.robot.subsystems.elevator.ElevatorIOSim +import frc.robot.subsystems.elevator.LoggedElevatorInputs +import frc.robot.subsystems.gripper.Gripper +import frc.robot.subsystems.gripper.GripperIO +import frc.robot.subsystems.gripper.GripperIOReal +import frc.robot.subsystems.gripper.GripperIOSim +import frc.robot.subsystems.gripper.LoggedGripperInputs +import frc.robot.subsystems.intake.extender.Extender +import frc.robot.subsystems.intake.extender.ExtenderIO +import frc.robot.subsystems.intake.extender.ExtenderIOReal +import frc.robot.subsystems.intake.extender.ExtenderIOSim +import frc.robot.subsystems.intake.extender.LoggedExtenderInputs +import frc.robot.subsystems.intake.roller.LoggedRollerInputs +import frc.robot.subsystems.intake.roller.Roller +import frc.robot.subsystems.intake.roller.RollerIO +import frc.robot.subsystems.intake.roller.RollerIOReal +import frc.robot.subsystems.intake.roller.RollerIOSim import frc.robot.subsystems.vision.Vision import frc.robot.subsystems.vision.VisionConstants import frc.robot.subsystems.vision.VisionIOPhotonVision import frc.robot.subsystems.vision.VisionIOPhotonVisionSim +import frc.robot.subsystems.wrist.LoggedWristInputs +import frc.robot.subsystems.wrist.Wrist +import frc.robot.subsystems.wrist.WristIO +import frc.robot.subsystems.wrist.WristIOReal +import frc.robot.subsystems.wrist.WristIOSim private val swerveModuleIOs = arrayOf( @@ -44,3 +80,69 @@ private val visionIOs = }.toTypedArray() val vision = Vision(swerveDrive::addVisionMeasurement, *visionIOs) + +val subsystems = + when (CURRENT_MODE) { + Mode.REAL -> { + mapOf( + "Climber" to Climber(ClimberIOReal()), + "Elevator" to Elevator(ElevatorIOReal()), + "Gripper" to Gripper(GripperIOReal()), + "Extender" to Extender(ExtenderIOReal()), + "Roller" to Roller(RollerIOReal()), + "Wrist" to Wrist(WristIOReal()) + ) + } + + Mode.SIM -> { + mapOf( + "Climber" to Climber(ClimberIOSim()), + "Elevator" to Elevator(ElevatorIOSim()), + "Gripper" to Gripper(GripperIOSim()), + "Extender" to Extender(ExtenderIOSim()), + "Roller" to Roller(RollerIOSim()), + "Wrist" to Wrist(WristIOSim()) + ) + } + + Mode.REPLAY -> { + mapOf( + "Climber" to + Climber( + object : ClimberIO { + override var inputs = LoggedClimberInputs() + } + ), + "Elevator" to + Elevator( + object : ElevatorIO { + override var inputs = LoggedElevatorInputs() + } + ), + "Gripper" to + Gripper( + object : GripperIO { + override var inputs = LoggedGripperInputs() + } + ), + "Extender" to + Extender( + object : ExtenderIO { + override var inputs = LoggedExtenderInputs() + } + ), + "Roller" to + Roller( + object : RollerIO { + override var inputs = LoggedRollerInputs() + } + ), + "Wrist" to + Wrist( + object : WristIO { + override var inputs = LoggedWristInputs() + } + ) + ) + } + } diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 7c00f9b1b..3be99e824 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -21,6 +21,13 @@ object RobotContainer { private val testController = CommandXboxController(2) private val swerveDrive = frc.robot.swerveDrive + private val vision = frc.robot.vision + private val Climber = subsystems["CLimber"] + private val Elevator = subsystems["Elevator"] + private val Gripper = subsystems["Gripper"] + private val Extender = subsystems["Extender"] + private val Roller = subsystems["Roller"] + private val Wrist = subsystems["Wrist"] init { registerAutoCommands() From 37969ef6bdc6cc64a64230d94e51c918a587da5c Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 11:23:19 +0200 Subject: [PATCH 248/253] Fix typo --- src/main/kotlin/frc/robot/RobotContainer.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 3be99e824..8e0248797 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -22,7 +22,7 @@ object RobotContainer { private val swerveDrive = frc.robot.swerveDrive private val vision = frc.robot.vision - private val Climber = subsystems["CLimber"] + private val Climber = subsystems["Climber"] private val Elevator = subsystems["Elevator"] private val Gripper = subsystems["Gripper"] private val Extender = subsystems["Extender"] From 61307e05c23b9268e00f3a94d2247915eaafb335 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 11:57:37 +0200 Subject: [PATCH 249/253] Improve Initializer --- src/main/kotlin/frc/robot/Initializer.kt | 110 +++++++++----------- src/main/kotlin/frc/robot/RobotContainer.kt | 12 +-- 2 files changed, 58 insertions(+), 64 deletions(-) diff --git a/src/main/kotlin/frc/robot/Initializer.kt b/src/main/kotlin/frc/robot/Initializer.kt index 95c43d92c..54a951bf8 100644 --- a/src/main/kotlin/frc/robot/Initializer.kt +++ b/src/main/kotlin/frc/robot/Initializer.kt @@ -81,68 +81,62 @@ private val visionIOs = val vision = Vision(swerveDrive::addVisionMeasurement, *visionIOs) -val subsystems = +val climber = Climber( when (CURRENT_MODE) { - Mode.REAL -> { - mapOf( - "Climber" to Climber(ClimberIOReal()), - "Elevator" to Elevator(ElevatorIOReal()), - "Gripper" to Gripper(GripperIOReal()), - "Extender" to Extender(ExtenderIOReal()), - "Roller" to Roller(RollerIOReal()), - "Wrist" to Wrist(WristIOReal()) - ) + Mode.REAL -> ClimberIOReal() + Mode.SIM -> ClimberIOSim() + Mode.REPLAY -> object : ClimberIO { + override var inputs = LoggedClimberInputs() } + } +) + +val elevator = Elevator( + when (CURRENT_MODE) { + Mode.REAL -> ElevatorIOReal() + Mode.SIM -> ElevatorIOSim() + Mode.REPLAY -> object : ElevatorIO { + override var inputs = LoggedElevatorInputs() + } + } +) + +val gripper = Gripper( + when (CURRENT_MODE) { + Mode.REAL -> GripperIOReal() + Mode.SIM -> GripperIOSim() + Mode.REPLAY -> object : GripperIO { + override var inputs = LoggedGripperInputs() + } + } +) + +val extender = Extender( + when (CURRENT_MODE) { + Mode.REAL -> ExtenderIOReal() + Mode.SIM -> ExtenderIOSim() + Mode.REPLAY -> object : ExtenderIO { + override var inputs = LoggedExtenderInputs() + } + } +) - Mode.SIM -> { - mapOf( - "Climber" to Climber(ClimberIOSim()), - "Elevator" to Elevator(ElevatorIOSim()), - "Gripper" to Gripper(GripperIOSim()), - "Extender" to Extender(ExtenderIOSim()), - "Roller" to Roller(RollerIOSim()), - "Wrist" to Wrist(WristIOSim()) - ) +val roller = Roller( + when (CURRENT_MODE) { + Mode.REAL -> RollerIOReal() + Mode.SIM -> RollerIOSim() + Mode.REPLAY -> object : RollerIO { + override var inputs = LoggedRollerInputs() } + } +) - Mode.REPLAY -> { - mapOf( - "Climber" to - Climber( - object : ClimberIO { - override var inputs = LoggedClimberInputs() - } - ), - "Elevator" to - Elevator( - object : ElevatorIO { - override var inputs = LoggedElevatorInputs() - } - ), - "Gripper" to - Gripper( - object : GripperIO { - override var inputs = LoggedGripperInputs() - } - ), - "Extender" to - Extender( - object : ExtenderIO { - override var inputs = LoggedExtenderInputs() - } - ), - "Roller" to - Roller( - object : RollerIO { - override var inputs = LoggedRollerInputs() - } - ), - "Wrist" to - Wrist( - object : WristIO { - override var inputs = LoggedWristInputs() - } - ) - ) +val wrist = Wrist( + when (CURRENT_MODE) { + Mode.REAL -> WristIOReal() + Mode.SIM -> WristIOSim() + Mode.REPLAY -> object : WristIO { + override var inputs = LoggedWristInputs() } } +) \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 8e0248797..1805fac81 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -22,12 +22,12 @@ object RobotContainer { private val swerveDrive = frc.robot.swerveDrive private val vision = frc.robot.vision - private val Climber = subsystems["Climber"] - private val Elevator = subsystems["Elevator"] - private val Gripper = subsystems["Gripper"] - private val Extender = subsystems["Extender"] - private val Roller = subsystems["Roller"] - private val Wrist = subsystems["Wrist"] + private val climber = frc.robot.climber + private val elevator = frc.robot.elevator + private val gripper = frc.robot.gripper + private val extender = frc.robot.extender + private val roller = frc.robot.roller + private val wrist = frc.robot.wrist init { registerAutoCommands() From cb65764d763bcb0ee34eae0d9eb709849df3b387 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 11:59:50 +0200 Subject: [PATCH 250/253] Apply spotless --- src/main/kotlin/frc/robot/Initializer.kt | 108 +++++++++++++---------- 1 file changed, 60 insertions(+), 48 deletions(-) diff --git a/src/main/kotlin/frc/robot/Initializer.kt b/src/main/kotlin/frc/robot/Initializer.kt index 54a951bf8..03e48d2d5 100644 --- a/src/main/kotlin/frc/robot/Initializer.kt +++ b/src/main/kotlin/frc/robot/Initializer.kt @@ -81,62 +81,74 @@ private val visionIOs = val vision = Vision(swerveDrive::addVisionMeasurement, *visionIOs) -val climber = Climber( - when (CURRENT_MODE) { - Mode.REAL -> ClimberIOReal() - Mode.SIM -> ClimberIOSim() - Mode.REPLAY -> object : ClimberIO { - override var inputs = LoggedClimberInputs() +val climber = + Climber( + when (CURRENT_MODE) { + Mode.REAL -> ClimberIOReal() + Mode.SIM -> ClimberIOSim() + Mode.REPLAY -> + object : ClimberIO { + override var inputs = LoggedClimberInputs() + } } - } -) + ) -val elevator = Elevator( - when (CURRENT_MODE) { - Mode.REAL -> ElevatorIOReal() - Mode.SIM -> ElevatorIOSim() - Mode.REPLAY -> object : ElevatorIO { - override var inputs = LoggedElevatorInputs() +val elevator = + Elevator( + when (CURRENT_MODE) { + Mode.REAL -> ElevatorIOReal() + Mode.SIM -> ElevatorIOSim() + Mode.REPLAY -> + object : ElevatorIO { + override var inputs = LoggedElevatorInputs() + } } - } -) + ) -val gripper = Gripper( - when (CURRENT_MODE) { - Mode.REAL -> GripperIOReal() - Mode.SIM -> GripperIOSim() - Mode.REPLAY -> object : GripperIO { - override var inputs = LoggedGripperInputs() +val gripper = + Gripper( + when (CURRENT_MODE) { + Mode.REAL -> GripperIOReal() + Mode.SIM -> GripperIOSim() + Mode.REPLAY -> + object : GripperIO { + override var inputs = LoggedGripperInputs() + } } - } -) + ) -val extender = Extender( - when (CURRENT_MODE) { - Mode.REAL -> ExtenderIOReal() - Mode.SIM -> ExtenderIOSim() - Mode.REPLAY -> object : ExtenderIO { - override var inputs = LoggedExtenderInputs() +val extender = + Extender( + when (CURRENT_MODE) { + Mode.REAL -> ExtenderIOReal() + Mode.SIM -> ExtenderIOSim() + Mode.REPLAY -> + object : ExtenderIO { + override var inputs = LoggedExtenderInputs() + } } - } -) + ) -val roller = Roller( - when (CURRENT_MODE) { - Mode.REAL -> RollerIOReal() - Mode.SIM -> RollerIOSim() - Mode.REPLAY -> object : RollerIO { - override var inputs = LoggedRollerInputs() +val roller = + Roller( + when (CURRENT_MODE) { + Mode.REAL -> RollerIOReal() + Mode.SIM -> RollerIOSim() + Mode.REPLAY -> + object : RollerIO { + override var inputs = LoggedRollerInputs() + } } - } -) + ) -val wrist = Wrist( - when (CURRENT_MODE) { - Mode.REAL -> WristIOReal() - Mode.SIM -> WristIOSim() - Mode.REPLAY -> object : WristIO { - override var inputs = LoggedWristInputs() +val wrist = + Wrist( + when (CURRENT_MODE) { + Mode.REAL -> WristIOReal() + Mode.SIM -> WristIOSim() + Mode.REPLAY -> + object : WristIO { + override var inputs = LoggedWristInputs() + } } - } -) \ No newline at end of file + ) From 080ef0a303019343affd7e95b26f8288e531cb29 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 12:11:56 +0200 Subject: [PATCH 251/253] Delete old tuner constants file --- src/main/kotlin/frc/robot/generated/TunerConstants.java | 1 - 1 file changed, 1 deletion(-) delete mode 100644 src/main/kotlin/frc/robot/generated/TunerConstants.java diff --git a/src/main/kotlin/frc/robot/generated/TunerConstants.java b/src/main/kotlin/frc/robot/generated/TunerConstants.java deleted file mode 100644 index 8b1378917..000000000 --- a/src/main/kotlin/frc/robot/generated/TunerConstants.java +++ /dev/null @@ -1 +0,0 @@ - From 643f6a944d9fb313784d342228c4b742aff73cfc Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 13:21:48 +0200 Subject: [PATCH 252/253] Apply spotless --- .../subsystems/elevator/ElevatorConstants.kt | 10 +++++-- .../subsystems/elevator/ElevatorIOReal.kt | 28 +++++++++++++------ 2 files changed, 26 insertions(+), 12 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt index f4c122e78..b925d0545 100644 --- a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorConstants.kt @@ -1,11 +1,14 @@ package frc.robot.subsystems.elevator -import edu.wpi.first.units.* +import edu.wpi.first.units.AngleUnit +import edu.wpi.first.units.DistanceUnit +import edu.wpi.first.units.Measure +import edu.wpi.first.units.PerUnit +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Distance import frc.robot.lib.Gains import frc.robot.lib.selectGainsBasedOnMode import kotlin.math.PI -import kotlin.time.times val MAX_HEIGHT: Distance = Units.Meters.of(1.3) const val GEAR_RATIO = (1.0 / 12.0) * (42.0 / 48.0) @@ -23,7 +26,8 @@ val ROTATIONS_TO_CENTIMETER: Measure> = val CENTIMETERS_TO_ROTATIONS: Measure> = Units.Rotations.per(Units.Centimeter).of(1 / ROTATIONS_TO_CENTIMETERS_RATIO) -val GAINS = selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0, kG = 1.0), Gains()) +val GAINS = + selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0, kG = 1.0), Gains()) enum class Positions(val value: Distance) { L1(Units.Centimeters.of(25.0)), diff --git a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt index 09e979c59..b35682d6d 100644 --- a/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt +++ b/src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt @@ -1,6 +1,11 @@ package frc.robot.subsystems.elevator -import com.ctre.phoenix6.configs.* +import com.ctre.phoenix6.configs.CANcoderConfiguration +import com.ctre.phoenix6.configs.CurrentLimitsConfigs +import com.ctre.phoenix6.configs.FeedbackConfigs +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs +import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.Follower import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.hardware.CANcoder @@ -21,15 +26,19 @@ class ElevatorIOReal : ElevatorIO { private val auxMotor = TalonFX(AUX_ID) private val encoder = CANcoder(ENCODER_ID) private val mainMotorPositionRequest = MotionMagicTorqueCurrentFOC(0.0) - private val slot0Configs = Slot0Configs().apply { - kP = GAINS.kP - kI = GAINS.kI - kD = GAINS.kD - kG = 0.0 - } + private val slot0Configs = + Slot0Configs().apply { + kP = GAINS.kP + kI = GAINS.kI + kD = GAINS.kD + kG = 0.0 + } @AutoLogOutput - private val kgTrigger = Trigger { inputs.height > MIN_KG_HEIGHT }.onTrue(setKG(GAINS.kG)).onFalse(setKG(0.0)) + private val kgTrigger = + Trigger { inputs.height > MIN_KG_HEIGHT } + .onTrue(setKG(GAINS.kG)) + .onFalse(setKG(0.0)) init { val motorConfig = @@ -63,7 +72,8 @@ class ElevatorIOReal : ElevatorIO { encoder.configurator.apply(encoderConfig) } - private fun setKG(kg: Double): Command = runOnce({ mainMotor.configurator.apply(slot0Configs.withKG(kg)) }) + private fun setKG(kg: Double): Command = + runOnce({ mainMotor.configurator.apply(slot0Configs.withKG(kg)) }) override fun setHeight(height: Distance) { mainMotor.setControl( From 5cb7bfe8353ba5efbb7f73e8c523884c81012134 Mon Sep 17 00:00:00 2001 From: Emma Livne Date: Tue, 21 Jan 2025 14:23:51 +0200 Subject: [PATCH 253/253] Apply spotless --- src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java | 4 ++-- .../kotlin/frc/robot/subsystems/climber/Climber.kt | 11 ++++------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java index f77546b57..8ae928c41 100644 --- a/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java +++ b/src/main/kotlin/frc/robot/lib/motors/TalonFXSim.java @@ -74,8 +74,8 @@ public void setControl(VelocityVoltage request) { voltageRequest = () -> controller.calculate( - getVelocity().in(Units.RotationsPerSecond), - request.Velocity) + getVelocity().in(Units.RotationsPerSecond), + request.Velocity) + request.FeedForward; } diff --git a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt index a9845ae42..bc536b338 100644 --- a/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt +++ b/src/main/kotlin/frc/robot/subsystems/climber/Climber.kt @@ -28,11 +28,10 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { @AutoLogOutput private val isStopperStuck = Trigger { inputs.stopperMotorCurrent.abs(Units.Amps) >= - STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) + STOPPER_CURRENT_THRESHOLD.`in`(Units.Amps) } - @AutoLogOutput - private val isAttached = isLatchClosed.and(isTouching) + @AutoLogOutput private val isAttached = isLatchClosed.and(isTouching) @AutoLogOutput private val isFolded = Trigger { @@ -49,11 +48,9 @@ class Climber(private val io: ClimberIO) : SubsystemBase() { inputs.angle.isNear(setpoint, FOLDED_TOLERANCE) } - @AutoLogOutput - private var setpoint = Units.Rotations.zero() + @AutoLogOutput private var setpoint = Units.Rotations.zero() - @AutoLogOutput - private var mechanism = LoggedMechanism2d(3.0, 2.0) + @AutoLogOutput private var mechanism = LoggedMechanism2d(3.0, 2.0) private var root = mechanism.getRoot("Climber", 1.0, 1.0) private val ligament = root.append(LoggedMechanismLigament2d("ClimberLigament", 0.27003, 90.0))