diff --git a/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt b/src/main/kotlin/frc/robot/lib/GeomExtensionFunctions.kt index 1e1935cb..2b8877f6 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 = diff --git a/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt b/src/main/kotlin/frc/robot/subsystems/ReefCommands.kt index db572452..c1f97fa6 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) @@ -49,15 +49,16 @@ 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/Visualizer.kt b/src/main/kotlin/frc/robot/subsystems/Visualizer.kt index e40e2a88..76164658 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 @@ -53,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) @@ -71,6 +74,36 @@ class Visualizer { 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, diff --git a/src/main/kotlin/frc/robot/subsystems/drive/Drive.java b/src/main/kotlin/frc/robot/subsystems/drive/Drive.java index 0111b423..01d34fe5 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( diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index fdb9ee7d..8a2f7283 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,19 +39,23 @@ 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") 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/GripperConstants.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt index 8b81e87c..7a083c38 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/GripperConstants.kt +++ b/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 = diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt b/src/main/kotlin/frc/robot/subsystems/gripper/GripperIOSim.kt index ad14d6d5..de7b22ac 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", 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) } } 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 6d777e0d..055a1a49 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,7 @@ 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.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger import org.littletonrobotics.junction.networktables.LoggedNetworkNumber @@ -27,6 +28,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/RollerConstants.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerConstants.kt index 45a457c5..f2f74b3b 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/RollerIO.kt b/src/main/kotlin/frc/robot/subsystems/intake/roller/RollerIO.kt index b1c4355c..4d6b32f2 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 2cfd3e83..1a6cd236 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, @@ -46,18 +51,18 @@ 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 ) ) } 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() }