From 741485886164c90058ec5a2e0bddaff11fdf4676 Mon Sep 17 00:00:00 2001 From: taimorioka Date: Tue, 14 Jan 2025 17:41:58 -0800 Subject: [PATCH] working --- .../kotlin-compiler-18277150595480624353.salive | 0 .../kotlin/com/frcteam3636/frc2025/Robot.kt | 7 ++++--- .../frc2025/subsystems/drivetrain/Drivetrain.kt | 4 ++++ .../subsystems/drivetrain/DrivetrainIO.kt | 17 ++++++++++++++--- .../frc2025/subsystems/drivetrain/Module.kt | 2 +- 5 files changed, 23 insertions(+), 7 deletions(-) create mode 100644 .kotlin/sessions/kotlin-compiler-18277150595480624353.salive diff --git a/.kotlin/sessions/kotlin-compiler-18277150595480624353.salive b/.kotlin/sessions/kotlin-compiler-18277150595480624353.salive new file mode 100644 index 0000000..e69de29 diff --git a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt index 575e6fe..95be164 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/Robot.kt @@ -82,7 +82,6 @@ object Robot : LoggedRobot() { configureAutos() configureBindings() configureDashboard() - simulationPeriodic() } /** Start logging or pull replay logs from a file */ @@ -171,8 +170,10 @@ object Robot : LoggedRobot() { Dashboard.showTeleopTab(Shuffleboard.getTab("Teleoperated")) } - override fun simulationInit() { - SimulatedArena.getInstance().resetFieldForAuto() + override fun disabledInit() { + if (Robot.model == Model.SIMULATION) { + SimulatedArena.getInstance().resetFieldForAuto() + } } override fun simulationPeriodic() { diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt index 8a23641..3099933 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt @@ -118,6 +118,10 @@ object Drivetrain : Subsystem, Sendable { this ) + if (io is DrivetrainIOSim){ + poseEstimator.resetPose(io.swerveDriveSimulation.simulatedDriveTrainPose) + } + } override fun periodic() { diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt index dde0cd6..340ab77 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt @@ -5,6 +5,7 @@ import com.frcteam3636.frc2025.Pigeon2 import com.frcteam3636.frc2025.Robot import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.BUMPER_LENGTH import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.BUMPER_WIDTH +import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.MODULE_POSITIONS import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.TRACK_WIDTH import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.WHEEL_BASE import com.frcteam3636.frc2025.utils.swerve.PerCorner @@ -24,6 +25,7 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation import org.ironmaple.simulation.drivesims.SwerveModuleSimulation import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig +import org.littletonrobotics.junction.Logger import org.team9432.annotation.Logged @@ -41,7 +43,7 @@ abstract class DrivetrainIO { abstract val modules: PerCorner - fun updateInputs(inputs: DrivetrainInputs) { + open fun updateInputs(inputs: DrivetrainInputs) { gyro.periodic() modules.forEach(SwerveModule::periodic) @@ -106,7 +108,7 @@ class DrivetrainIOSim : DrivetrainIO() { SwerveModuleSimulationConfig( DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60 DCMotor.getNeo550(1), // Steer motor is a Neo 550 - 3.5, + (45.0 * 22.0) / (14.0 * 15.0), 9424.0 / 203.0, Volts.of(0.1), Volts.of(0.1), @@ -122,6 +124,10 @@ class DrivetrainIOSim : DrivetrainIO() { ) // Configures the bumper size (dimensions of the robot bumper) .withBumperSize(BUMPER_WIDTH, BUMPER_LENGTH) + .withCustomModuleTranslations( + MODULE_POSITIONS.map { it.translation }.toTypedArray() + ) + // Create a swerve drive simulation val swerveDriveSimulation = SwerveDriveSimulation( // Specify Configuration @@ -131,11 +137,16 @@ class DrivetrainIOSim : DrivetrainIO() { ) override val modules = PerCorner.generate { SimSwerveModule(swerveDriveSimulation.modules[it.ordinal]) } - override val gyro = GyroMapleSim(GyroSimulation(.001, .05)) + override val gyro = GyroMapleSim(swerveDriveSimulation.gyroSimulation) init { SimulatedArena.getInstance().addDriveTrainSimulation(swerveDriveSimulation) } + override fun updateInputs(inputs: DrivetrainInputs) { + super.updateInputs(inputs) + Logger.recordOutput("FieldSimulation/RobotPosition", swerveDriveSimulation.simulatedDriveTrainPose) + } + // Register the drivetrain simulation to the default simulation world } diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Module.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Module.kt index d75bf32..5a545e4 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Module.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Module.kt @@ -220,7 +220,7 @@ class SimSwerveModule(val sim: SwerveModuleSimulation) : SwerveModule { // take the known wheel diameter, divide it by two to get the radius, then get the // circumference -internal val WHEEL_RADIUS = Inches.of(3.0).div(2.0) +internal val WHEEL_RADIUS = Inches.of(1.5) internal val WHEEL_CIRCUMFERENCE = WHEEL_RADIUS * TAU internal val NEO_FREE_SPEED = RPM.of(5676.0)