Sticking with the high level sim approach (i.e. not using a FlywheelSim, though for a more robust full simulation solution I’d definitely recommend looking into that) the gyro “simulation” can actually be very simple.
Because you care more about the control structure, for your use case it would be quickest to find where you’re commanding a ChassisSpeeds and just use that to update the gyro. Many teams have one singular “drive” function that takes in a ChassisSpeeds and does the kinematics calculations. If this is the only place your modules would ever be commanded, you could just directly use the omegaRadiansPerSecond
and multiply it by some measured dt
timestep and just add it to a simulated heading variable. Then in your getGyroAngle()
function just have a check RobotBase.isSimulation() ? simHeading : gyro.getYaw()
so you can use the same functions in simulation and real.
If you wanted it to be more “true” to the real robot you could instead take the result of kinematics.toChassisSpeeds(moduleStates).omegaRadiansPerSecond
and multiply by the same timestep although this should return the exact same value as the previous method just with an extra calculation.
This approach assumes zero losses from friction but it also wouldn’t take into account things like the skew from driving and turning noted here, but again judging from your use case, this is likely good enough.