Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add maple_sim #38

Merged
merged 18 commits into from
Jan 15, 2025
Merged
Changes from 16 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .idea/runConfigurations/Build_Robot.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

4 changes: 2 additions & 2 deletions .idea/runConfigurations/Build___Deploy_Robot.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

4 changes: 2 additions & 2 deletions .idea/runConfigurations/Build___Run_Simulate_Java.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

2 changes: 1 addition & 1 deletion .idea/runConfigurations/Clean.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

4 changes: 2 additions & 2 deletions .idea/runConfigurations/Clean_Build_Robot.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

4 changes: 2 additions & 2 deletions .idea/runConfigurations/Clean_Build___Deploy_Robot.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

4 changes: 2 additions & 2 deletions .idea/runConfigurations/Clean_Build___Run_Simulate_Java.xml

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

18 changes: 18 additions & 0 deletions src/main/kotlin/com/frcteam3636/frc2025/Robot.kt
Original file line number Diff line number Diff line change
@@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler
import edu.wpi.first.wpilibj2.command.Commands
import edu.wpi.first.wpilibj2.command.button.CommandXboxController
import edu.wpi.first.wpilibj2.command.button.JoystickButton
import org.ironmaple.simulation.SimulatedArena
import org.littletonrobotics.junction.LogFileUtil
import org.littletonrobotics.junction.LoggedRobot
import org.littletonrobotics.junction.Logger
@@ -156,10 +157,27 @@ object Robot : LoggedRobot() {
Dashboard.showTeleopTab(Shuffleboard.getTab("Teleoperated"))
}

override fun disabledInit() {
if (Robot.model == Model.SIMULATION) {
SimulatedArena.getInstance().resetFieldForAuto()
}
}

override fun simulationPeriodic() {
SimulatedArena.getInstance().simulationPeriodic()

Logger.recordOutput("FieldSimulation/Algae",
*SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"))
Logger.recordOutput("FieldSimulation/Coral",
*SimulatedArena.getInstance().getGamePiecesArrayByType("Coral"))

}

override fun robotPeriodic() {
Dashboard.update()
Diagnostics.collect(statusSignals).reportAlerts()
CommandScheduler.getInstance().run()

}

override fun autonomousInit() {
Original file line number Diff line number Diff line change
@@ -47,7 +47,7 @@ import kotlin.math.abs
/** A singleton object representing the drivetrain. */
object Drivetrain : Subsystem, Sendable {
private val io = when (Robot.model) {
Robot.Model.SIMULATION -> TODO()
Robot.Model.SIMULATION -> DrivetrainIOSim()
Robot.Model.COMPETITION -> DrivetrainIOReal.fromKrakenSwerve()
Robot.Model.PROTOTYPE -> DrivetrainIOReal.fromNeoSwerve()
}
@@ -118,6 +118,10 @@ object Drivetrain : Subsystem, Sendable {
this
)

if (io is DrivetrainIOSim){
poseEstimator.resetPose(io.swerveDriveSimulation.simulatedDriveTrainPose)
}

}

override fun periodic() {
@@ -329,8 +333,11 @@ object Drivetrain : Subsystem, Sendable {
/** Unit: Rotations per second */
const val ROTATION_SENSITIVITY = 0.4

private val WHEEL_BASE: Double = Units.inchesToMeters(30.0)
private val TRACK_WIDTH: Double = Units.inchesToMeters(28.0)
val WHEEL_BASE = Inches.of(30.0)
val TRACK_WIDTH = Inches.of(28.0)

val BUMPER_WIDTH = Inches.of(33.5)
val BUMPER_LENGTH = Inches.of(35.5)

const val JOYSTICK_DEADBAND = 0.15

Original file line number Diff line number Diff line change
@@ -3,13 +3,31 @@ package com.frcteam3636.frc2025.subsystems.drivetrain
import com.frcteam3636.frc2025.CTREDeviceId
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
import com.studica.frc.AHRS
import edu.wpi.first.math.geometry.Pose2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Rotation3d
import edu.wpi.first.math.kinematics.SwerveModulePosition
import edu.wpi.first.math.kinematics.SwerveModuleState
import edu.wpi.first.math.system.plant.DCMotor
import edu.wpi.first.units.Units.KilogramSquareMeters
import edu.wpi.first.units.Units.Volts
import org.ironmaple.simulation.SimulatedArena
import org.ironmaple.simulation.drivesims.COTS
import org.ironmaple.simulation.drivesims.COTS.WHEELS
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig
import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig
import org.littletonrobotics.junction.Logger
import org.team9432.annotation.Logged


@Logged
open class DrivetrainInputs {
var gyroRotation = Rotation3d()
@@ -24,7 +42,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)

@@ -78,8 +96,56 @@ class DrivetrainIOReal(override val modules: PerCorner<SwerveModule>) : Drivetra
}
}

///** Drivetrain I/O layer that uses simulated swerve modules along with a simulated gyro with an angle based off their movement. */
//class DrivetrainIOSim : DrivetrainIO() {
// override val modules = PerCorner.generate { SimSwerveModule() }
// override val gyro = GyroSim(modules.map { it })
//}
/** Drivetrain I/O layer that uses simulated swerve modules along with a simulated gyro with an angle based off their movement. */
class DrivetrainIOSim : DrivetrainIO() {
// Create and configure a drivetrain simulation configuration
val driveTrainSimulationConfig: DriveTrainSimulationConfig =
DriveTrainSimulationConfig.Default() // Specify gyro type (for realistic gyro drifting and error simulation)
.withGyro(COTS.ofPigeon2()) // Specify swerve module (for realistic swerve dynamics)
.withSwerveModule(
// FIXME: Calculate values
SwerveModuleSimulationConfig(
DCMotor.getKrakenX60(1), // Drive motor is a Kraken X60
DCMotor.getNeo550(1), // Steer motor is a Neo 550
(45.0 * 22.0) / (14.0 * 15.0),
9424.0 / 203.0,
Volts.of(0.1),
Volts.of(0.1),
WHEEL_RADIUS,
KilogramSquareMeters.of(0.02),
WHEELS.SLS_PRINTED_WHEELS.cof
)
)
// Configures the track length and track width (spacing between swerve modules)
.withTrackLengthTrackWidth(
WHEEL_BASE,
TRACK_WIDTH
) // 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
driveTrainSimulationConfig,
// Specify starting pose
Pose2d(3.0, 3.0, Rotation2d())
)

override val modules = PerCorner.generate { SimSwerveModule(swerveDriveSimulation.modules[it.ordinal]) }
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
}
Original file line number Diff line number Diff line change
@@ -8,6 +8,7 @@ import com.frcteam3636.frc2025.utils.swerve.translation2dPerSecond
import com.studica.frc.AHRS
import edu.wpi.first.math.geometry.Rotation3d
import edu.wpi.first.math.geometry.Translation2d
import org.ironmaple.simulation.drivesims.GyroSimulation
import org.littletonrobotics.junction.Logger
import kotlin.math.sign

@@ -49,7 +50,7 @@ class GyroPigeon(private val pigeon: Pigeon2) : Gyro {

init {
Logger.recordOutput("PigeonGyro/Offset", offset)
BaseStatusSignal.setUpdateFrequencyForAll(100.0, pigeon.yaw, pigeon.pitch, pigeon.roll);
BaseStatusSignal.setUpdateFrequencyForAll(100.0, pigeon.yaw, pigeon.pitch, pigeon.roll)
}

override var rotation: Rotation3d
@@ -81,3 +82,13 @@ class GyroSim(private val modules: PerCorner<SwerveModule>) : Gyro {
rotation += Rotation3d(0.0, 0.0, yawVelocity) * Robot.period
}
}

class GyroMapleSim(val gyroSimulation : GyroSimulation): Gyro {
override var rotation: Rotation3d
get() = Rotation3d(gyroSimulation.gyroReading)
set(value) {
gyroSimulation.setRotation(value.toRotation2d())
}
override val connected: Boolean
get() = true
}
Loading