working

FRC3636 · Jan 15, 2025 · 7414858 · 7414858
1 parent d64e2b4
commit 7414858
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 7 deletions.
diff --git a/.kotlin/sessions/kotlin-compiler-18277150595480624353.salive b/.kotlin/sessions/kotlin-compiler-18277150595480624353.salive
diff --git 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
@@ -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
@@ -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<out SwerveModule>
 
 
-    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
@@ -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)
-Original file line number
+Diff line change
@@ Expand Up / @@ -118,6 +118,10 @@ object Drivetrain : Subsystem, Sendable { @@
                 this
             )
+            if (io is DrivetrainIOSim){
+                poseEstimator.resetPose(io.swerveDriveSimulation.simulatedDriveTrainPose)
+            }
         }
         override fun periodic() {
@@ Expand Down @@