FRC Swerve Module Simulation CI Test - Need Help simulating while testing

Technical Programming
, , , , ,
1

I am trying to create a unit test for my FRC swerve modules using the FRC simulation library, but my test keeps failing. The swerve module code works fine when using the full Java robot simulator, but when I try to use the simulation library, the motors do not move and their positions do not change. I think the problem is with the simulation, but I am not sure what part of it is causing the issue.

Here is my GitHub repo on the branch with my testing
To reproduce the issue, you can run the SwerveModuleTest in the test package. The test currently fails with the following error message:

expected: <1.0> but was: <0.0> 
Expected :1.0 Actual :0.0 
<Click to see difference>

I have tried printing out various debug information from the swerve module, including the position, velocity, angle, drive and steer motor values, and power and current of the drive motor. However, this information has not helped me find the root cause of the issue.

I would greatly appreciate any help or suggestions on how to debug this issue. Thank you in advance!

here is the test

    var swerveModule: SwerveModule? = null
    var simEncoder: EncoderSim? = null
    @BeforeEach
    fun setUp() {
        // initialize the HAL to start the simulation.
        // pass in 500 as the timeout in milliseconds and 0 as the device ID
        assert(HAL.initialize(500, 0))
        SimHooks.setHALRuntimeType(HALUtil.getHALRuntimeType())
        // create our simulation encoder at the correct CAN index
        simEncoder = EncoderSim.createForIndex(Constants.FrontLeftEncoder)
        // create our swerve module using the appropriate motor and encoder IDs and a Translation2d object
        swerveModule = SwerveModule(
            "fl",
            Constants.FrontLeftDriveMotor,
            Constants.FrontLeftSteerMotor,
            Constants.FrontLeftEncoder,
            Translation2d(1.0, 1.0)
        )
    }

@Test
    fun testMove() {
        // before running the test, initialize the HAL and observe the user program teleop
        HAL.initialize(500, 0)
        HAL.observeUserProgramTeleop()
        // run the simulation and the swerve module's move method 100 times
        for (i in 0..100) {
            // run the command scheduler and physics simulation
            CommandScheduler.getInstance().run()
            PhysicsSim.instance.run()
            // run the move method on the swerve module, passing in a drive value of 1.0 and an angle value of 1.0
            swerveModule!!.move(1.0, 1.0)
        }
        // assert that the velocity and angle of the swerve module are as expected
        assertEquals(1.0, swerveModule!!.velocity, DELTA)
        assertEquals(1.0, swerveModule!!.angle, DELTA)
    }
2

What you linked to doesnā€™t have a src/test, but most likely this is due to the fact that youā€™re not stepping time appropriately, so thereā€™s no time for either the set command to get to the Talons (CTRE models the commanding delay through CAN bus) or the motors to actually move. What does PhysicsSim.instance.run() do?

3

whoops! I fixed the link!

src/test/kotlin/frc/robot
PhysicsSim.instance.run() just advances the simulation provided by CTRE

1 Like
4

Where is frc.robot.sim.PhysicsSim located?

5

I believe thereā€™s no good way to step time in simulation to account Talon CAN delays aside from using Time.sleep()

see CTRE Talon simulation - #7 by TytanRock

6

sorry, turns out I broke my git somehow but it is fixed now
frc.robot.sim.PhysicsSim

7

that feels really haphazard, are you sure there arenā€™t other ways of doing this?

2 Likes
8

Iā€™d love to know if there are :frowning:

9

this ended up not workingā€¦ I donā€™t know if I am coding it wrong or what.
hereā€™s the test case

@Test
    fun move() {
        swerveModule!!.move(SwerveModuleState(1.1, Rotation2d(1.2)))
        for (i in 0..100) {
            // run the command scheduler and physics simulation
            CommandScheduler.getInstance().run()
            PhysicsSim.instance.run()
            Thread.sleep(20)
            // run the move method on the swerve module, passing in a drive value of 1.0 and an angle value of 1.0
            swerveModule!!.move(SwerveModuleState(1.1, Rotation2d(1.2)))
        }
        println("swerveModule!!.angle: " + swerveModule!!.angle)
        println("swerveModule!!.velocity: " + swerveModule!!.velocity)
        println("swerveModule!!.translation2d: " + swerveModule!!.translation2d)
        println(swerveModule!!.driveMotor.motorOutputPercent)
        assertEquals(1.0, swerveModule!!.velocity, DELTA)
        assertEquals(1.0, swerveModule!!.angle, DELTA)
    }

and the result

swerveModule!!.angle: 0.0
swerveModule!!.velocity: 0.0
swerveModule!!.translation2d: Translation2d(X: 1.00, Y: 1.00)
0.0

expected: <1.0> but was: <0.0>
Expected :1.0
Actual   :0.0

and the test setup

    val DELTA = 1e-2 // acceptable deviation range

    var swerveModule: SwerveModule? = null
    var simEncoder: EncoderSim? = null
    @BeforeEach
    fun setUp() {
        // initialize the HAL to start the simulation.
        // pass in 500 as the timeout in milliseconds and 0 as the device ID
        assert(HAL.initialize(500, 0))
        SimHooks.setHALRuntimeType(HALUtil.getHALRuntimeType())
        // create our simulation encoder at the correct CAN index
        simEncoder = EncoderSim.createForIndex(Constants.FrontLeftEncoder)
        // create our swerve module using the appropriate motor and encoder IDs and a Translation2d object
        swerveModule = SwerveModule(
            "fl",
            Constants.FrontLeftDriveMotor,
            Constants.FrontLeftSteerMotor,
            Constants.FrontLeftEncoder,
            Translation2d(1.0, 1.0)
        )
    }

and physics sim

package frc.robot.sim

import com.ctre.phoenix.motorcontrol.can.TalonFX
import com.ctre.phoenix.motorcontrol.can.TalonSRX
import com.ctre.phoenix.motorcontrol.can.VictorSPX
import java.lang.Math.random
import kotlin.math.IEEErem
import kotlin.math.sin

/**
 * Manages physics simulation for CTRE products.
 */
class PhysicsSim {
    /**
     * Adds a TalonSRX controller to the simulator.
     *
     * @param talon
     * The TalonSRX device
     * @param accelToFullTime
     * The time the motor takes to accelerate from 0 to full, in seconds
     * @param fullVel
     * The maximum motor velocity, in ticks per 100ms
     * @param sensorPhase
     * The phase of the TalonSRX sensors
     */
    @JvmOverloads
    fun addTalonSRX(talon: TalonSRX?, accelToFullTime: Double, fullVel: Double, sensorPhase: Boolean = false) {
        if (talon != null) {
            val simTalon = TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase)
            simProfiles.add(simTalon)
        }
    }
    /**
     * Adds a TalonFX controller to the simulator.
     *
     * @param falcon
     * The TalonFX device
     * @param accelToFullTime
     * The time the motor takes to accelerate from 0 to full, in seconds
     * @param fullVel
     * The maximum motor velocity, in ticks per 100ms
     * @param sensorPhase
     * The phase of the TalonFX sensors
     */
    @JvmOverloads
    fun addTalonFX(falcon: TalonFX?, accelToFullTime: Double, fullVel: Double, sensorPhase: Boolean = false) {
        if (falcon != null) {
            val simFalcon = TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase)
            simProfiles.add(simFalcon)
        }
    }

    /**
     * Adds a VictorSPX controller to the simulator.
     *
     * @param victor
     * The VictorSPX device
     */
    fun addVictorSPX(victor: VictorSPX?) {
        if (victor != null) {
            val simVictor = VictorSPXSimProfile(victor)
            simProfiles.add(simVictor)
        }
    }

    /**
     * Runs the simulator:
     * - enable the robot
     * - simulate sensors
     */
    fun run() {
        // Simulate devices
        for (simProfile in simProfiles) {
            simProfile.run()
        }
    }

    private val simProfiles = ArrayList<SimProfile>()

    fun reset() {
        simProfiles.clear()
    }

    /**
     * Holds information about a simulated device.
     */
    internal open class SimProfile {
        private var _lastTime: Long = 0
        private var _running = false

        /**
         * Runs the simulation profile.
         * Implemented by device-specific profiles.
         */
        open fun run() {}
        /**
         * The time since last call, in milliseconds.
         */
        protected val period: Double
            get() {
                // set the start time if not yet running
                if (!_running) {
                    _lastTime = System.nanoTime()
                    _running = true
                }
                val now = System.nanoTime()
                val period = (now - _lastTime) / 1000000.0
                _lastTime = now
                return period
            }
    }

    companion object {
        /**
         * Gets the robot simulator instance.
         */
        val instance = PhysicsSim()

        /* scales a random domain of [0, 2pi] to [min, max] while prioritizing the peaks */
        fun random(min: Double, max: Double): Double {
            return (max - min) / 2 * sin(random().IEEErem(2 * 3.14159)) + (max + min) / 2
        }

        /**
         * random(max) is equivalent to random(0, max)
         */
        fun random(max: Double): Double {
            return random(0.0, max)
        }
    }
}

also i probably should have included that when I do gradle simulateJava, I can control the robot and the modules work just fine.

10

Seems like youā€™ve misunderstood what multiple classes/functions/parameters do; here is the case thatā€™s likely causing the tests to fail. EncoderSim is for quadrature encoders connected to the RoboRIO DIO and read using the Encoder class; the ā€œindexā€ mentioned is a HAL index used by the native HAL code and shouldnā€™t be used from team code (the similar createForChannel factory method accepts a DIO channel number).

Encoders connected to CTRE (or other) CAN devices are a whole other story, and use the relevant vendorā€™s simulation stuff. I recommend reading up on CTREā€™s SimCollection docs, in your case.

11

Thank you for responding @Starlight220,

I am still learning how to simulate encoders and appreciate your explanation. While I understand your suggestion, I believe the issue may lie elsewhere because the simulation is able to get to assertEquals(1.0, swerveModule!!.velocity, DELTA), which is connected to the drive motor and has nothing to do with the simEncoder I declared at the beginning of the test class. In addition, the universal encoder is used to keep the modules aligned after powering off, rather than for the motors themselves.

Thank you for your help and input, I really appreciate it.

12

Are you adding the motors to your PhysicsSim at any point?

13

Yep! I do it in the SwerveModule class.

open class SwerveModule(
    val moduleName: String,
    val driveId: Int,
    val steerId: Int,
    val encId: Int,
    val translation2d: Translation2d,
): SubsystemBase() {

    val driveMotor = WPI_TalonFX(driveId).apply {
        configFactoryDefault()
        PhysicsSim.instance.addTalonFX(
            this,
            DRIVE_MOTOR_ACCELERATION,
            DRIVE_MOTOR_TOP_SPEED
        )
    }

    val steerMotor = WPI_TalonFX(steerId).apply {
        configFactoryDefault()
        PhysicsSim.instance.addTalonFX(
            this,
            TURN_MOTOR_ACCELERATION,
            TURN_MOTOR_TOP_SPEED
        )
    }
// the rest of the module code
}
14

The motors move and everything works as expected when I use gradle simulateJava. Is it possible that the lack of a robot class or the standard FRC main class to initialize everything properly is stopping the simulation from working?

One thing I have noticed is that on JUnit 4 (and possibly JUnit 5, I havenā€™t checked) if the real robot simulation is running (the one from gradle simulateJava), the Drivetrain and Swerve module tests completely fail to execute (they do not appear in the test list) and in the logs it says something about a port already in use. Once the real simulation is closed, the tests run as they did before, except for those that rely on the simulation working to succeed.

The tests also do not appear to run in the ā€˜realā€™ simulation on my desktop as described in the WPILIB documentation. No windows are opened or anything. (on both junit 4 and 5, with or without changes to the gradle.build file)

Tests will always be run in simulation on your desktop. For prerequisites and more info, see the simulation introduction.

I would appreciate any suggestions or advice on how to troubleshoot and resolve these issues.

15

Two things are getting conflated here. Projects have two source sets: main and test. main is where most code goes. The test source set depends on main, and contains only test-specific code (namely, unit tests).

On the robot, the main source set is run by calling the main() method and all that.

gradle simulateJava runs the main source set, same as it is on the robot ā€“ just in a simulated environment rather than a real robot one. This is regular simulation, what youā€™ve called ā€œreal simulationā€ for some reason.

As opposed to those two, gradle test runs the test source set in a simulated environment. main() is not called, and JUnit runs the unit tests.

Thereā€™s no reason whatsoever to run both gradle test and gradle simulateJava at the same time, theyā€™re completely separate things: each runs in a separate process with a separate version of the code. The error message you saw is likely because each of those processes tried to create a NT server, and each network port can only be used by a single process.

What is meant by this section in the documentation is that tests are run in the simulated environment, as opposed to on a real robot. Tests are not run when running regular code in the simulated environment. If thereā€™s a more clear phrasing, we can change the docs.

16

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.