Add Game Piece Visualization via Maple Sim

src/main/kotlin/frc/robot/RobotContainer.kt

@@ -27,20 +27,20 @@
  * 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)
     private val testController = CommandXboxController(2)
 
     private val swerveDrive = frc.robot.swerveDrive
     private val vision = frc.robot.vision
     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
     val visualizer: Visualizer
     val voltage = Units.Volts.of(1.0)
 
@@ -58,7 +58,7 @@
     }
 
     @AutoLogOutput(key = "MapleSimPose")
     private fun getMapleSimPose(): Pose2d? =
         driveSimulation?.simulatedDriveTrainPose
 
     private fun getDriveCommandReal(): Command =
@@ -124,7 +124,7 @@
         DriveCommands.wheelRadiusCharacterization(swerveDrive)
 
     private fun registerAutoCommands() {
         fun register(name: String, command: Command) =
             NamedCommands.registerCommand(name, command)
     }
 }

src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt

@@ -8,63 +8,65 @@
 import edu.wpi.first.units.measure.Distance
 import frc.robot.IS_RED
 
 fun Translation2d.getRotationToTranslation(other: Translation2d): Rotation2d =
     (this - other).angle
 
 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 =
     Pose2d(translation, this.rotation)
 
 fun Pose2d.withRotation(rotation: Rotation2d): Pose2d =
     Pose2d(this.translation, rotation)
 
 fun Pose3d.withRotation(rotation: Rotation3d): Pose3d =
     Pose3d(this.translation, rotation)
 
 fun Pose3d.withRotation(
     roll: Double = 0.0,
     pitch: Double = 0.0,
     yaw: Double = 0.0
 ): Pose3d = Pose3d(this.translation, Rotation3d(roll, pitch, yaw))
 
 fun Pose3d.withRotation(
     roll: Angle = Rotations.zero(),
     pitch: Angle = Rotations.zero(),
     yaw: Angle = Rotations.zero(),
 ): Pose3d = Pose3d(this.translation, Rotation3d(roll, pitch, yaw))
 
 fun Pose2d.toTransform(): Transform2d =
     Transform2d(this.translation, this.rotation)
 
 fun Pose2d.distanceFromPoint(translationMeters: Translation2d): Distance =
     Units.Meters.of(this.translation.getDistance(translation))
 
 fun Pose3d.toTransform(): Transform3d =
     Transform3d(this.translation, this.rotation)
 
 fun Translation2d.flip(): Translation2d = FlippingUtil.flipFieldPosition(this)
 
 fun Translation2d.flipIfNeeded(): Translation2d =
     if (IS_RED) this.flip() else this
 
 fun Translation2d.toTransform(): Transform2d = Transform2d(this, Rotation2d())
 
 fun Translation2d.toPose(): Pose2d = Pose2d(this, Rotation2d())
 
 fun Translation2d.rotationToPoint(targetPoint: Translation2d): Rotation2d =
     Rotation2d(targetPoint.x - this.x, targetPoint.y - this.y)
 
 fun Rotation2d.flip(): Rotation2d = FlippingUtil.flipFieldRotation(this)
 
 fun Rotation2d.flipIfNeeded(): Rotation2d = if (IS_RED) this.flip() else this
 
 fun Rotation2d.toTransform(): Transform2d = Transform2d(Translation2d(), this)
 
 fun Rotation2d.toPose(): Pose2d = Pose2d(Translation2d(), this)
 
 fun Transform2d.toPose(): Pose2d = Pose2d(this.translation, this.rotation)
 
 fun Transform3d.toPose(): Pose3d = Pose3d(this.translation, this.rotation)

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
@@ -14,13 +17,13 @@
 
 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)
 
 private fun visualizeCoralOuttake(): Command =
     runOnce({
         if (!gripper.hasCoral.asBoolean) return@runOnce
 
         val arena = SimulatedArena.getInstance()
@@ -49,15 +52,15 @@
         )
     })
 
+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()
     )
 
@@ -109,4 +112,4 @@
         gripper.intake().until(gripper.hasCoral).andThen(moveDefaultPosition())
     )
 
 fun retract(): Command = parallel(elevator.zero(), wrist.retract())

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
@@ -53,6 +52,10 @@
 
     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<Pose3d, Pose3d> {
 
         val secondStageHeight = elevator.height.invoke().`in`(Meters)
@@ -71,6 +74,34 @@
         return Pair(firstStagePose, secondStagePose)
     }
 
+    @AutoLogOutput(key = "Visualizer/AlgaePoseInRobot")
+    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() + CORAL_ANGLE_OFFSET,
+                    yaw = driveSimulation.simulatedDriveTrainPose.rotation.measure!! //
+                )
+        else Pose3d()
+
     private fun getSwerveModulePoseTurn(
         moduleX: Double,
         moduleY: Double,

src/main/kotlin/frc/robot/subsystems/drive/Drive.java

@@ -102,6 +102,7 @@
             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(
@@ -139,7 +140,7 @@
                 new SwerveModulePosition(),
                 new SwerveModulePosition()
             };
     private SwerveDrivePoseEstimator poseEstimator =
             new SwerveDrivePoseEstimator(
                     kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
 

src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt

@@ -7,12 +7,13 @@
 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
 
 class Gripper(private val io: GripperIO) : SubsystemBase() {
     private val kV = 6
     var rollerAngle: Angle = Units.Rotations.zero()
     private val tuningVoltage =
         LoggedNetworkNumber("/Tuning/Gripper/Voltage", 0.0)
@@ -28,29 +29,31 @@
     private fun setVoltage(voltage: Voltage): Command =
         startEnd({ io.setVoltage(voltage) }, { io.setVoltage(STOP_VOLTAGE) })
 
     private fun getVoltage(): Double {
         return io.inputs.appliedVoltage.`in`(Units.Volts)
     }
     fun tuningVoltage(): Command =
         setVoltage(Units.Volts.of(tuningVoltage.get()))
             .withName("Gripper/Tuning")
 
     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")
 
     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)
     }

src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt

@@ -8,10 +8,12 @@ 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(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 =

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", 20.0)
+        LoggedNetworkNumber(
+            "/Tuning/Gripper/SensorDistance",
+            DEFAULT_SENSOR_SIMULATION_DISTANCE.`in`(Units.Centimeters)
+        )
 
     private val motor =
         TalonFXSim(
@@ -32,6 +35,13 @@ class GripperIOSim : GripperIO {
         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)
     }
 }

src/main/kotlin/frc/robot/subsystems/intake/roller/Roller.kt

@@ -5,11 +5,12 @@
 import edu.wpi.first.units.measure.Voltage
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.SubsystemBase
+import org.littletonrobotics.junction.AutoLogOutput
 import org.littletonrobotics.junction.Logger
 import org.littletonrobotics.junction.networktables.LoggedNetworkNumber
 
 class Roller(private val io: RollerIO) : SubsystemBase() {
     private var kV = 2
     var rollerAngle: Angle = Units.Rotations.zero()
     private val tuningVoltage =
         LoggedNetworkNumber("Tuning/Roller/Voltage", 0.0)
@@ -21,12 +22,18 @@
             )
             .withName("Roller/setVoltage")
 
     fun setTuningVoltage(): Command =
         setVoltage(Units.Volts.of(tuningVoltage.get()))
             .withName("Roller/Voltage")
     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")

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)

src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt

@@ -2,17 +2,20 @@
 
 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 {
     val inputs: LoggedRollerInputs
 
     fun setVoltage(voltage: Voltage) {}
 
+    fun getIntakeSimulation(): IntakeSimulation? = null
+
     fun updateInputs() {}
 
     @Logged
     open class RollerInputs {
         var appliedVoltage: Voltage = Units.Volts.zero()
     }
 }

src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIOSim.kt

@@ -24,6 +24,11 @@
             IntakeSimulation.IntakeSide.FRONT,
             1
         )
+
+    override fun getIntakeSimulation(): IntakeSimulation? {
+        return intakeSimulation
+    }
+
     private val motor =
         TalonFXSim(
             1,
@@ -46,18 +51,18 @@
                     driveSimulation
                         .driveTrainSimulatedChassisSpeedsFieldRelative,
                     driveSimulation.simulatedDriveTrainPose.rotation,
-                    CORAL_OUTTAKE_HEIGHT,
-                    CORAL_OUTTAKE_VELOCITY,
-                    CORAL_OUTTAKE_ANGLE
+                    ALGAE_OUTTAKE_HEIGHT,
+                    ALGAE_OUTTAKE_VELOCITY,
+                    ALGAE_OUTTAKE_ANGLE
                 )
             )
     }
 
     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()
         }