From 69571e835cef8e0e0425332c134916875b2882d6 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Thu, 6 Feb 2025 17:05:38 +0200 Subject: [PATCH 01/14] Add extension function --- src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt index 1e1935cb0..2b8877f66 100644 --- a/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt +++ b/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt @@ -13,6 +13,8 @@ fun Translation2d.getRotationToTranslation(other: Translation2d): Rotation2d = fun Pose2d.flip(): Pose2d = FlippingUtil.flipFieldPose(this) +fun Pose2d.toPose3d(): Pose3d = Pose3d(this) + fun Pose2d.flipIfNeeded(): Pose2d = if (IS_RED) this.flip() else this fun Pose2d.withTranslation(translation: Translation2d): Pose2d = From 9c5c72a32c1079005a73a9ad07dd6daf4c57037d Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Thu, 6 Feb 2025 17:05:55 +0200 Subject: [PATCH 02/14] Add Algae visualizatio --- .../kotlin/frc/robot/subsystems/Visualizer.kt | 16 ++++++++++------ .../frc/robot/subsystems/intake/roller/Roller.kt | 8 ++++++++ .../robot/subsystems/intake/roller/RollerIO.kt | 3 +++ .../subsystems/intake/roller/RollerIOSim.kt | 5 +++++ 4 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index e40e2a885..a59a05092 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -1,13 +1,12 @@ package frc.robot.subsystems -import edu.wpi.first.math.geometry.Pose3d -import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.math.geometry.Translation3d +import edu.wpi.first.math.geometry.* import edu.wpi.first.units.Units.* import edu.wpi.first.units.measure.Angle -import frc.robot.lib.getPose3d -import frc.robot.lib.getRotation3d -import frc.robot.lib.getTranslation3d +import frc.robot.CURRENT_MODE +import frc.robot.Mode +import frc.robot.driveSimulation +import frc.robot.lib.* import frc.robot.roller import frc.robot.subsystems.drive.Drive import kotlin.math.cos @@ -71,6 +70,11 @@ class Visualizer { return Pair(firstStagePose, secondStagePose) } + private val ALGAE_OFFSET = Transform3d(0.4, 0.0, 0.35, getRotation3d(0.0)) + + @AutoLogOutput(key = "Visualizer/AlgaePoseInRobot") + private fun getAlgaePose(): Pose3d? = if (CURRENT_MODE != Mode.REAL && roller.shouldVisualizeAlgaeInSim()) driveSimulation?.simulatedDriveTrainPose?.toPose3d()?.plus(ALGAE_OFFSET) else Pose3d() + private fun getSwerveModulePoseTurn( moduleX: Double, moduleY: Double, diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt index 6d777e0d6..74ae57442 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt @@ -5,6 +5,8 @@ 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.SubsystemBase +import org.ironmaple.simulation.IntakeSimulation +import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.networktables.LoggedNetworkNumber @@ -27,6 +29,12 @@ class Roller(private val io: RollerIO) : SubsystemBase() { fun getVoltage(): Double { return io.inputs.appliedVoltage.baseUnitMagnitude() } + + + @AutoLogOutput(key = "VisualizeAlgaeInSim") + // THIS SHOULD ONLY BE USED IN THE SIMULATION!! + fun shouldVisualizeAlgaeInSim(): Boolean = io.getIntakeSimulation()?.gamePiecesAmount != 0 + fun intake() = setVoltage(INTAKE_VOLTAGE).withName("Roller/intake") fun outtake() = setVoltage(OUTTAKE_VOLTAGE).withName("Roller/outtake") diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt index b1c4355c4..4d6b32f22 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt @@ -2,6 +2,7 @@ package frc.robot.subsystems.intake.roller import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Voltage +import org.ironmaple.simulation.IntakeSimulation import org.team9432.annotation.Logged interface RollerIO { @@ -9,6 +10,8 @@ interface RollerIO { fun setVoltage(voltage: Voltage) {} + fun getIntakeSimulation(): IntakeSimulation? = null + fun updateInputs() {} @Logged diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt index 2cfd3e836..201eaf753 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt @@ -24,6 +24,11 @@ class RollerIOSim(driveTrainSimulation: AbstractDriveTrainSimulation) : IntakeSimulation.IntakeSide.FRONT, 1 ) + + override fun getIntakeSimulation(): IntakeSimulation? { + return intakeSimulation + } + private val motor = TalonFXSim( 1, From abc6e57689d267c76bebc83c22f839496f63ca13 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 12:02:12 +0200 Subject: [PATCH 03/14] Rename coral to algae in intake simulation --- src/main/kotlin/frc/robot/RobotContainer.kt | 2 +- .../frc/robot/subsystems/intake/roller/RollerConstants.kt | 6 +++--- .../frc/robot/subsystems/intake/roller/RollerIOSim.kt | 6 +++--- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index b772a8c61..46dfeab09 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -117,7 +117,7 @@ object RobotContainer { driverController.a().onTrue(l1(driverController.a().negate())) driverController.x().onTrue(l2(driverController.x().negate())) driverController.b().onTrue(l3(driverController.b().negate())) - driverController.y().onTrue(l4(driverController.y().negate())) +// driverController.y().onTrue(l4(driverController.y().negate())) driverController .start() .onTrue(feeder(driverController.start().negate())) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerConstants.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerConstants.kt index 45a457c5f..f2f74b3b2 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerConstants.kt @@ -9,9 +9,9 @@ val INTAKE_VOLTAGE: Voltage = Units.Volts.of(-0.7 * 12.0) val OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(0.8 * 12.0) val CORAL_OUTTAKE_TRANSLATION: Translation2d = getTranslation2d(Units.Meters.of(0.3)) -val CORAL_OUTTAKE_HEIGHT: Distance = Units.Meter.of(0.4) -val CORAL_OUTTAKE_VELOCITY: LinearVelocity = Units.MetersPerSecond.of(3.0) -val CORAL_OUTTAKE_ANGLE: Angle = Units.Degrees.zero() +val ALGAE_OUTTAKE_HEIGHT: Distance = Units.Meter.of(0.4) +val ALGAE_OUTTAKE_VELOCITY: LinearVelocity = Units.MetersPerSecond.of(3.0) +val ALGAE_OUTTAKE_ANGLE: Angle = Units.Degrees.zero() val INTAKE_WIDTH: Distance = Units.Meters.of(0.64) val INTAKE_LENGTH_EXTENDED: Distance = Units.Meters.of(0.35) val FAR_OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(0.7 * 12.0) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt index 201eaf753..f1c8596e7 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt @@ -51,9 +51,9 @@ class RollerIOSim(driveTrainSimulation: AbstractDriveTrainSimulation) : driveSimulation .driveTrainSimulatedChassisSpeedsFieldRelative, driveSimulation.simulatedDriveTrainPose.rotation, - CORAL_OUTTAKE_HEIGHT, - CORAL_OUTTAKE_VELOCITY, - CORAL_OUTTAKE_ANGLE + ALGAE_OUTTAKE_HEIGHT, + ALGAE_OUTTAKE_VELOCITY, + ALGAE_OUTTAKE_ANGLE ) ) } From 15876e6c1128aac4e81c70791201b6a162ea1829 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 13:47:40 +0200 Subject: [PATCH 04/14] Add coral visualization --- .../kotlin/frc/robot/subsystems/Visualizer.kt | 27 +++++++++++++++++-- .../subsystems/gripper/GripperConstants.kt | 1 + .../robot/subsystems/gripper/GripperIOSim.kt | 11 +++++--- 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index a59a05092..b27c507e1 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -70,10 +70,33 @@ class Visualizer { return Pair(firstStagePose, secondStagePose) } - private val ALGAE_OFFSET = Transform3d(0.4, 0.0, 0.35, getRotation3d(0.0)) + private val ALGAE_OFFSET = Transform3d(0.4, 0.0, 0.35, Rotation3d()) + private val CORAL_HEIGHT_OFFSET = getPose3d(z = 0.07) @AutoLogOutput(key = "Visualizer/AlgaePoseInRobot") - private fun getAlgaePose(): Pose3d? = if (CURRENT_MODE != Mode.REAL && roller.shouldVisualizeAlgaeInSim()) driveSimulation?.simulatedDriveTrainPose?.toPose3d()?.plus(ALGAE_OFFSET) else Pose3d() + private fun getAlgaePose(): Pose3d? = + if (CURRENT_MODE != Mode.REAL && roller.shouldVisualizeAlgaeInSim()) + driveSimulation + ?.simulatedDriveTrainPose + ?.toPose3d() + ?.plus(ALGAE_OFFSET) + else Pose3d() + + @AutoLogOutput(key = "Visualizer/CoralPoseInRobot") + private fun getCoralPose(): Pose3d? = + if (CURRENT_MODE != Mode.REAL && gripper.hasCoral.asBoolean) + driveSimulation + ?.simulatedDriveTrainPose + ?.toPose3d() + ?.plus( + getGripperRollerPose( + CORAL_ROLLER_UP_C2C[0], + CORAL_ROLLER_UP_C2C[1] + ) + .minus(CORAL_HEIGHT_OFFSET) + ) + ?.withRotation(pitch = -wrist.angle.invoke() + Degrees.of(25.0)) + else Pose3d() private fun getSwerveModulePoseTurn( moduleX: Double, diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt index 06621acd8..2aa0c0172 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt @@ -8,6 +8,7 @@ import edu.wpi.first.units.measure.Voltage const val GEAR_RATIO = 1.0 // TODO: Replace with real value +val DEFAULT_SENSOR_SIMULATION_DISTANCE: Distance = Units.Centimeters.of(20.0) val DISTANCE_THRESHOLD: Distance = Units.Centimeters.of(14.2) val INTAKE_VOLTAGE: Voltage = Units.Volts.of(5.0) val OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(-9.0) diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt index ad14d6d58..25c8f9cda 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt @@ -11,7 +11,7 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber class GripperIOSim : GripperIO { override val inputs = LoggedGripperInputs() private var sensorDistance = - LoggedNetworkNumber("/Tuning/Gripper/SensorDistance", 20.0) + LoggedNetworkNumber("/Tuning/Gripper/SensorDistance", DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters)) private val motor = TalonFXSim( @@ -29,9 +29,12 @@ class GripperIOSim : GripperIO { override fun updateInputs() { motor.update(Timer.getFPGATimestamp()) - inputs.sensorDistance.mut_replace( - Units.Centimeters.of(sensorDistance.get()) - ) + inputs.sensorDistance.mut_replace(Units.Centimeters.of(sensorDistance.get())) + + if (inputs.appliedVoltage < Units.Volts.zero()) { + sensorDistance.set(DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters)) + } + inputs.appliedVoltage.mut_replace(motor.appliedVoltage) } } From c38b075cb0e4ac306032026f73ba14b012ef5e8c Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 13:48:03 +0200 Subject: [PATCH 05/14] Spotless --- src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt index 74ae57442..055a1a492 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt @@ -5,7 +5,6 @@ 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.SubsystemBase -import org.ironmaple.simulation.IntakeSimulation import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.networktables.LoggedNetworkNumber @@ -30,10 +29,10 @@ class Roller(private val io: RollerIO) : SubsystemBase() { return io.inputs.appliedVoltage.baseUnitMagnitude() } - @AutoLogOutput(key = "VisualizeAlgaeInSim") // THIS SHOULD ONLY BE USED IN THE SIMULATION!! - fun shouldVisualizeAlgaeInSim(): Boolean = io.getIntakeSimulation()?.gamePiecesAmount != 0 + fun shouldVisualizeAlgaeInSim(): Boolean = + io.getIntakeSimulation()?.gamePiecesAmount != 0 fun intake() = setVoltage(INTAKE_VOLTAGE).withName("Roller/intake") From 67f282758182b8c59788bd5977eef70f0220da7e Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 14:09:35 +0200 Subject: [PATCH 06/14] Fix GRIPPER_HEIGHT in ReefCommands visualization --- src/main/kotlin/frc/robot/subsystems/ReefCommands.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index db5724525..2e2cf0fcb 100644 --- a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt +++ b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt @@ -14,7 +14,7 @@ private val CORAL_OUTTAKE_TIMEOUT = Units.Seconds.of(0.5) private val CORAL_SHOOT_OFFSET = getTranslation2d(Units.Meters.of(0.40), Units.Meters.of(0.0)) -private val GRIPPER_HEIGHT = Units.Meters.of(0.50) +private val GRIPPER_HEIGHT = Units.Meters.of(0.9) private val CORAL_SHOOT_SPEED = Units.MetersPerSecond.of(3.0) private val CORAL_L4_SHOOT_ANGLE = Units.Degrees.of(-80.0) private val WRIST_ANGLE_OFFSET = Units.Degrees.of(35.0) From d01503eb856eed09ae6622b6fa84f41eae294ab3 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 14:13:03 +0200 Subject: [PATCH 07/14] Refactor Constants --- src/main/kotlin/frc/robot/subsystems/Visualizer.kt | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index b27c507e1..aa6b78c34 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -52,6 +52,10 @@ class Visualizer { private val gripper = frc.robot.gripper + private val ALGAE_OFFSET = Transform3d(0.4, 0.0, 0.35, Rotation3d()) + private val CORAL_HEIGHT_OFFSET = getPose3d(z = 0.07) + private val CORAL_ANGLE_OFFSET = Degrees.of(25.0) + private fun getElevatorPoses(): Pair { val secondStageHeight = elevator.height.invoke().`in`(Meters) @@ -70,9 +74,6 @@ class Visualizer { return Pair(firstStagePose, secondStagePose) } - private val ALGAE_OFFSET = Transform3d(0.4, 0.0, 0.35, Rotation3d()) - private val CORAL_HEIGHT_OFFSET = getPose3d(z = 0.07) - @AutoLogOutput(key = "Visualizer/AlgaePoseInRobot") private fun getAlgaePose(): Pose3d? = if (CURRENT_MODE != Mode.REAL && roller.shouldVisualizeAlgaeInSim()) @@ -95,7 +96,7 @@ class Visualizer { ) .minus(CORAL_HEIGHT_OFFSET) ) - ?.withRotation(pitch = -wrist.angle.invoke() + Degrees.of(25.0)) + ?.withRotation(pitch = -wrist.angle.invoke() + CORAL_ANGLE_OFFSET) else Pose3d() private fun getSwerveModulePoseTurn( From fefad78acd3524067a91a487754747a06e43160d Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 19:10:05 +0200 Subject: [PATCH 08/14] Some fixes & Calibrations --- src/main/kotlin/frc/robot/RobotContainer.kt | 2 +- .../kotlin/frc/robot/subsystems/ReefCommands.kt | 5 ++++- src/main/kotlin/frc/robot/subsystems/Visualizer.kt | 5 ++++- .../kotlin/frc/robot/subsystems/gripper/Gripper.kt | 2 +- .../frc/robot/subsystems/gripper/GripperIOSim.kt | 13 ++++++++++--- .../robot/subsystems/intake/roller/RollerIOSim.kt | 4 ++-- 6 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 548fd83ec..962258c30 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -27,7 +27,7 @@ import org.littletonrobotics.junction.AutoLogOutput * calls). Instead, the structure of the robot (including subsystems, commands, * and trigger mappings) should be declared here. */ -object RobotContainer { +object RobotContainer { private val driverController = CommandPS5Controller(0) private val operatorController = CommandXboxController(1) diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index 2e2cf0fcb..5294ffbcb 100644 --- a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt +++ b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt @@ -1,10 +1,13 @@ package frc.robot.subsystems +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.units.Units import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands.* import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.* +import frc.robot.lib.getRotation2d +import frc.robot.lib.getRotation3d import frc.robot.lib.getTranslation2d import frc.robot.subsystems.elevator.Positions import org.ironmaple.simulation.SimulatedArena @@ -16,7 +19,7 @@ private val CORAL_SHOOT_OFFSET = getTranslation2d(Units.Meters.of(0.40), Units.Meters.of(0.0)) private val GRIPPER_HEIGHT = Units.Meters.of(0.9) private val CORAL_SHOOT_SPEED = Units.MetersPerSecond.of(3.0) -private val CORAL_L4_SHOOT_ANGLE = Units.Degrees.of(-80.0) +private val CORAL_L4_SHOOT_ANGLE = Units.Degrees.of(-85.0) // private val WRIST_ANGLE_OFFSET = Units.Degrees.of(35.0) private fun visualizeCoralOuttake(): Command = diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index aa6b78c34..f36fd04c9 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -96,7 +96,10 @@ class Visualizer { ) .minus(CORAL_HEIGHT_OFFSET) ) - ?.withRotation(pitch = -wrist.angle.invoke() + CORAL_ANGLE_OFFSET) + ?.withRotation( + pitch = -wrist.angle.invoke() + CORAL_ANGLE_OFFSET, + yaw = driveSimulation.simulatedDriveTrainPose.rotation.measure!! // + ) else Pose3d() private fun getSwerveModulePoseTurn( diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index fdb9ee7d7..9904f9334 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -50,7 +50,7 @@ class Gripper(private val io: GripperIO) : SubsystemBase() { setVoltage(REMOVE_ALGAE_VOLTAGE).withName("Gripper/RemoveAlgae") override fun periodic() { - rollerAngle += Units.Rotations.of(getVoltage() * kV * 0.02) + rollerAngle += Units.Rotations.of(getVoltage() * kV * -0.02) // io.updateInputs() Logger.processInputs(this::class.simpleName, io.inputs) } diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt index 25c8f9cda..de7b22acd 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt @@ -11,7 +11,10 @@ import org.littletonrobotics.junction.networktables.LoggedNetworkNumber class GripperIOSim : GripperIO { override val inputs = LoggedGripperInputs() private var sensorDistance = - LoggedNetworkNumber("/Tuning/Gripper/SensorDistance", DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters)) + LoggedNetworkNumber( + "/Tuning/Gripper/SensorDistance", + DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters) + ) private val motor = TalonFXSim( @@ -29,10 +32,14 @@ class GripperIOSim : GripperIO { override fun updateInputs() { motor.update(Timer.getFPGATimestamp()) - inputs.sensorDistance.mut_replace(Units.Centimeters.of(sensorDistance.get())) + inputs.sensorDistance.mut_replace( + Units.Centimeters.of(sensorDistance.get()) + ) if (inputs.appliedVoltage < Units.Volts.zero()) { - sensorDistance.set(DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters)) + sensorDistance.set( + DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters) + ) } inputs.appliedVoltage.mut_replace(motor.appliedVoltage) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt index f1c8596e7..1a6cd2363 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt @@ -60,9 +60,9 @@ class RollerIOSim(driveTrainSimulation: AbstractDriveTrainSimulation) : override fun setVoltage(voltage: Voltage) { motor.setControl(controlRequest.withOutput(voltage)) - if (voltage != Units.Volts.zero()) { + if (voltage < Units.Volts.zero()) { intakeSimulation.startIntake() - } else { + } else if (voltage > Units.Volts.zero()) { intakeSimulation.stopIntake() visualizeOuttakeGamePieceIfNeeded() } From 1ce9ab31d120741287b0e5275133a05be54ac895 Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 20:12:01 +0200 Subject: [PATCH 09/14] Add bumper size --- src/main/kotlin/frc/robot/subsystems/drive/Drive.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index 0111b4231..01d34fe5d 100644 --- a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java +++ b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java @@ -102,6 +102,7 @@ public class Drive extends SubsystemBase { DriveTrainSimulationConfig.Default() .withRobotMass(Kilograms.of(ROBOT_MASS_KG)) .withCustomModuleTranslations(getModuleTranslations()) + .withBumperSize(Meters.of(0.851), Meters.of(0.851)) .withGyro(COTS.ofNav2X()) .withSwerveModule( new SwerveModuleSimulationConfig( From 343e4bedb79aef3f47b7ea7161afd3be8c50a49e Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 20:13:04 +0200 Subject: [PATCH 10/14] Adjust L4 Angle --- src/main/kotlin/frc/robot/subsystems/ReefCommands.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index 5294ffbcb..1cb9c5e5f 100644 --- a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt +++ b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt @@ -19,7 +19,7 @@ private val CORAL_SHOOT_OFFSET = getTranslation2d(Units.Meters.of(0.40), Units.Meters.of(0.0)) private val GRIPPER_HEIGHT = Units.Meters.of(0.9) private val CORAL_SHOOT_SPEED = Units.MetersPerSecond.of(3.0) -private val CORAL_L4_SHOOT_ANGLE = Units.Degrees.of(-85.0) // +private val CORAL_L4_SHOOT_ANGLE = Units.Degrees.of(-80.0) private val WRIST_ANGLE_OFFSET = Units.Degrees.of(35.0) private fun visualizeCoralOuttake(): Command = From 823d7ba483a07f2ebd0446e9860915518eea1948 Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 20:25:13 +0200 Subject: [PATCH 11/14] Add coral visualization in gripper outtake commands --- src/main/kotlin/frc/robot/subsystems/ReefCommands.kt | 6 +++--- src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt | 9 ++++++--- .../frc/robot/subsystems/gripper/GripperConstants.kt | 1 + 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index 1cb9c5e5f..191b6fca0 100644 --- a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt +++ b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt @@ -52,15 +52,15 @@ private fun visualizeCoralOuttake(): Command = ) }) +fun visualizeCoralOuttakeIfNeeded(): Command = visualizeCoralOuttake().onlyIf { CURRENT_MODE != Mode.REAL } + private fun scoreCoral(endTrigger: Trigger): Command = sequence( waitUntil(endTrigger), gripper .outtake() .withTimeout(CORAL_OUTTAKE_TIMEOUT) - .alongWith( - visualizeCoralOuttake().onlyIf { CURRENT_MODE != Mode.REAL } - ), + .alongWith(visualizeCoralOuttakeIfNeeded()), moveDefaultPosition() ) diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index 9904f9334..743755bb1 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -7,6 +7,7 @@ import edu.wpi.first.units.measure.Voltage 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.subsystems.visualizeCoralOuttakeIfNeeded import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.networktables.LoggedNetworkNumber @@ -38,13 +39,15 @@ class Gripper(private val io: GripperIO) : SubsystemBase() { fun intake(): Command = setVoltage(INTAKE_VOLTAGE).withName("Gripper/Intake") + private fun outtakeCommand(voltage: Voltage): Command = setVoltage(voltage).alongWith(visualizeCoralOuttakeIfNeeded()) + fun slowOuttake(): Command = - setVoltage(SLOW_OUTTAKE_VOLTAGE).withName("Gripper/SlowOuttake") + outtakeCommand(SLOW_OUTTAKE_VOLTAGE).withName("Gripper/SlowOuttake") fun outtake(): Command = - setVoltage(OUTTAKE_VOLTAGE).withName("Gripper/Outtake") + outtakeCommand(OUTTAKE_VOLTAGE).withName("Gripper/Outtake") - fun fastOuttake(): Command = setVoltage(Units.Volts.of(-10.0)) + fun fastOuttake(): Command = outtakeCommand(FAST_OUTTAKE_VOLTAGE).withName("Gripper/FastOuttake") fun removeAlgae(): Command = setVoltage(REMOVE_ALGAE_VOLTAGE).withName("Gripper/RemoveAlgae") diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt index c134c39b5..7a083c38d 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt @@ -13,6 +13,7 @@ val DISTANCE_THRESHOLD: Distance = Units.Centimeters.of(14.2) val INTAKE_VOLTAGE: Voltage = Units.Volts.of(7.5) val OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(-9.0) val SLOW_OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(-1.5) +val FAST_OUTTAKE_VOLTAGE: Voltage = Units.Volts.of(-10.0) val REMOVE_ALGAE_VOLTAGE: Voltage = Units.Volts.of(12.0) // TODO: Calibrate val STOP_VOLTAGE: Voltage = Units.Volts.zero() val MOMENT_OF_INERTIA: MomentOfInertia = From 8bf3e6d6691119134d093164ef9e11a89b77a84c Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 21:33:51 +0200 Subject: [PATCH 12/14] Remove redundant comment --- src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index 743755bb1..366833ac7 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -53,7 +53,7 @@ class Gripper(private val io: GripperIO) : SubsystemBase() { setVoltage(REMOVE_ALGAE_VOLTAGE).withName("Gripper/RemoveAlgae") override fun periodic() { - rollerAngle += Units.Rotations.of(getVoltage() * kV * -0.02) // + rollerAngle += Units.Rotations.of(getVoltage() * kV * -0.02) io.updateInputs() Logger.processInputs(this::class.simpleName, io.inputs) } From 65fcfe68bfe68d3bc4ad887250478f5841030a0c Mon Sep 17 00:00:00 2001 From: Barak Bornstein <68773850+rakrakon@users.noreply.github.com> Date: Sun, 9 Feb 2025 21:37:03 +0200 Subject: [PATCH 13/14] Update src/main/kotlin/frc/robot/subsystems/Visualizer.kt Co-authored-by: Dan Katzuv <31829093+katzuv@users.noreply.github.com> --- src/main/kotlin/frc/robot/subsystems/Visualizer.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index f36fd04c9..616ef9ee1 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -98,7 +98,7 @@ class Visualizer { ) ?.withRotation( pitch = -wrist.angle.invoke() + CORAL_ANGLE_OFFSET, - yaw = driveSimulation.simulatedDriveTrainPose.rotation.measure!! // + yaw = driveSimulation.simulatedDriveTrainPose.rotation.measure!! ) else Pose3d() From 46af44a199726e853e0dea6f2cf275c248d9ea86 Mon Sep 17 00:00:00 2001 From: Rakrakon Date: Sun, 9 Feb 2025 21:37:26 +0200 Subject: [PATCH 14/14] Spotless --- src/main/kotlin/frc/robot/RobotContainer.kt | 2 +- src/main/kotlin/frc/robot/subsystems/ReefCommands.kt | 6 ++---- src/main/kotlin/frc/robot/subsystems/Visualizer.kt | 4 +++- src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt | 6 ++++-- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 962258c30..548fd83ec 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -27,7 +27,7 @@ import org.littletonrobotics.junction.AutoLogOutput * calls). Instead, the structure of the robot (including subsystems, commands, * and trigger mappings) should be declared here. */ -object RobotContainer { +object RobotContainer { private val driverController = CommandPS5Controller(0) private val operatorController = CommandXboxController(1) diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index 191b6fca0..c1f97fa6e 100644 --- a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt +++ b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt @@ -1,13 +1,10 @@ package frc.robot.subsystems -import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.units.Units import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands.* import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.* -import frc.robot.lib.getRotation2d -import frc.robot.lib.getRotation3d import frc.robot.lib.getTranslation2d import frc.robot.subsystems.elevator.Positions import org.ironmaple.simulation.SimulatedArena @@ -52,7 +49,8 @@ private fun visualizeCoralOuttake(): Command = ) }) -fun visualizeCoralOuttakeIfNeeded(): Command = visualizeCoralOuttake().onlyIf { CURRENT_MODE != Mode.REAL } +fun visualizeCoralOuttakeIfNeeded(): Command = + visualizeCoralOuttake().onlyIf { CURRENT_MODE != Mode.REAL } private fun scoreCoral(endTrigger: Trigger): Command = sequence( diff --git a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index f36fd04c9..000184ece 100644 --- a/src/main/kotlin/frc/robot/subsystems/Visualizer.kt +++ b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt @@ -98,7 +98,9 @@ class Visualizer { ) ?.withRotation( pitch = -wrist.angle.invoke() + CORAL_ANGLE_OFFSET, - yaw = driveSimulation.simulatedDriveTrainPose.rotation.measure!! // + yaw = + driveSimulation.simulatedDriveTrainPose.rotation + .measure!! // ) else Pose3d() diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index 366833ac7..8a2f7283f 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -39,7 +39,8 @@ class Gripper(private val io: GripperIO) : SubsystemBase() { fun intake(): Command = setVoltage(INTAKE_VOLTAGE).withName("Gripper/Intake") - private fun outtakeCommand(voltage: Voltage): Command = setVoltage(voltage).alongWith(visualizeCoralOuttakeIfNeeded()) + private fun outtakeCommand(voltage: Voltage): Command = + setVoltage(voltage).alongWith(visualizeCoralOuttakeIfNeeded()) fun slowOuttake(): Command = outtakeCommand(SLOW_OUTTAKE_VOLTAGE).withName("Gripper/SlowOuttake") @@ -47,7 +48,8 @@ class Gripper(private val io: GripperIO) : SubsystemBase() { fun outtake(): Command = outtakeCommand(OUTTAKE_VOLTAGE).withName("Gripper/Outtake") - fun fastOuttake(): Command = outtakeCommand(FAST_OUTTAKE_VOLTAGE).withName("Gripper/FastOuttake") + fun fastOuttake(): Command = + outtakeCommand(FAST_OUTTAKE_VOLTAGE).withName("Gripper/FastOuttake") fun removeAlgae(): Command = setVoltage(REMOVE_ALGAE_VOLTAGE).withName("Gripper/RemoveAlgae")