Simulation getOutputMotorVoltage() returns zero in unit testing

[Piphi5]

Calling getOutputMotorVoltage() in JUnit simulation seems to return zero and ultimately interfere with the entire system simulation. More information here.

Steps to reproduce:

• Clone: https://github.com/Piphi5/CTRESimTest
• Right click on build.gradle and Test Robot Code

Expected Behaviour:

When running the unit tests, the CTRE test should pass the unit test with a non-zero speed and voltage printed out to the console (see the WPILib Test as a reference).

[JCaporuscio]

Looking at your test, the issue is with the test setup itself: https://github.com/Piphi5/CTRESimTest/blob/8782b0d20f955ecc01e7154ed342e5501e851c3f/src/test/java/frc/robot/CTRETest.java#L24

The Phoenix simulation elements don't support manually stepping the timing - they operate in real time. So your objects don't have enough time to react and you're actually reading back the initial conditions.

This can be worked around by inserting a delay with the step timing. Keep in mind you also need to account for the time it would normally take for new data to come over the CAN bus - so if the data you need is sent every 20 ms you need to account for that in addition to the time you need for the system to react. https://docs.ctre-phoenix.com/en/stable/ch18_CommonAPI.html

[Piphi5]

This can be worked around by inserting a delay with the step timing. Keep in mind you also need to account for the time it would normally take for new data to come over the CAN bus - so if the data you need is sent every 20 ms you need to account for that in addition to the time you need for the system to react.

By delay do you mean a Thread.sleep()? Also, for the data delay, if I wanted to set and get the voltage of the device I would have to pause for 30 ms (20ms for robot loop + 10ms for Status 1)?

[JCaporuscio]

Yes, a sleep would be sufficient.

The periodic timings aren't cumulative, they happen in parallel. So you only need to wait for the longest one (in your case Status 2 (20ms) for sensor velocity). So you should sleep for SimProcessTime + CAN data time.

[Piphi5]

So I tried replacing the step timing with a sleep but the problem still remains. Is 20ms the wrong timing?

[JCaporuscio]

20ms will not be enough. You need to allow the simulated Talon to process (1ms), and then you need to allow for enough time for a new set of data. When you sample any periodic data, I recommend waiting double to guarantee enough time for a new set of data.

So I would wait 40 or even 50ms just to be safe.

[Piphi5]

I increased it to 50ms but it seems like the problem still remains.

[JCaporuscio]

Ok. I'll do some testing with it today when I have minute and check it out. There may be something else going on that's not obvious.

[JCaporuscio]

Ok, so I've got it working. There were a couple things going on that are specific to unit tests (normally handled by the full robot project):

Init

I had to add the following lines to your setup routine before the notifier creation:

assert HAL.initialize(500, 1);
SimHooks.pauseTiming();
SimHooks.stepTiming(0);

These are necessary to initialize some of the WPI-specific items that Phoenix relies on for sim hooks as well as initializing the timing steps.

Enable

When you run full simulation code, Phoenix registers periodic callbacks to facilitate enabling the simulated devices (among other things) based on the DS's enable state. This isn't the case with unit testing so you have to call it manually. You can also just enable with the Unmanaged function if you want. I recommend using the PeriodicBefore() callbacks and then adding another call to feedEnable if your unit test will take longer than 100ms (the periodic callback that enables sim Talons only enables in 100ms periods).

So in my testing I added the following lines at the beginning of the flywheelMovementTest() function:

DriverStationSim.setEnabled(true);
HAL.simPeriodicBefore();
Unmanaged.feedEnable(1000);  //Use 1000 initially, can make smaller later

Thread Sleep

So as I mentioned before, you have to sleep to wait for updated data from the simulated Talon. For output voltage this means waiting for the percent output from Status 2 (20ms). Sleeping for just 20ms is sufficient (at least when I did this test).

After the sleep then you can step the timing of the simhooks, which runs your flywheel periodic code. After this you have to sleep again to wait for the updated velocity value. This is a bit odd for the version of Phoenix you're using - there was a bug initially where the velocity was being filtered incorrectly, the effect of which was a delay in the reported velocity from Talon. You'll need to sleep a few hundred milliseconds (the lowest I was able to use in my test was 260ms).

The good news is that there's a development build with this bug fixed. You can find it here: http://devsite.ctr-electronics.com/maven/development/com/ctre/phoenix/frcjson/5.19.5-rc-2/frcjson-5.19.5-rc-2.json If you want to use it, remove the existing Phoenix json from your vendordeps and do an online install with the above URL.

So the full CTRETest.java then looks like this:

package frc.robot;

import static org.junit.Assert.*;

import com.ctre.phoenix.unmanaged.Unmanaged;

import org.junit.*;

import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.Notifier;
import edu.wpi.first.wpilibj.simulation.DriverStationSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;

public class CTRETest {
  private CTREFlywheel mFlywheel = new CTREFlywheel(); 
  private Notifier mNotifier;
  @Before
  public void setup() {
    assert HAL.initialize(500, 1);
    SimHooks.pauseTiming();
    SimHooks.stepTiming(0);
    System.out.println("started");

    mNotifier = new Notifier(() -> {
      mFlywheel.simulationPeriodic();
      System.out.println(mFlywheel.getMotorOutputVoltage());
    });

    mNotifier.startPeriodic(0.02);
  }

  @Test
  public void flywheelMovementTest() throws InterruptedException {
    DriverStationSim.setEnabled(true);
    HAL.simPeriodicBefore();
    Unmanaged.feedEnable(1000);  //Use 1000 initially, can make smaller later

    mFlywheel.setPower(1.0);
    Thread.sleep(20);
    SimHooks.stepTiming(0.2);
    Thread.sleep(260);  //You can lower this to ~20-30ms with latest dev Phoenix

    System.out.println("Speed: " + mFlywheel.getSpeed());
    System.out.println("Voltage: " + mFlywheel.getMotorOutputVoltage());
    assertTrue(mFlywheel.getSpeed() > 0);
  }
}

Other Thoughts

There is a way to bypass the waiting. We report some values to the WPILib sim GUI, and these values are reported directly via the registered callbacks (so there's no delay). You can access these by creating a SimDeviceSim object in java. For example, to get Percent Output from a Victor with ID 8, you would create the following:

var simOutput = new SimDeviceSim("CANMotor:Victor SPX[8]").getDouble("percentOutput");

And then you get the value by calling simOutput.get()

Percent Output, Bus Voltage are all available this way. For the exact strings needed you can look at Network Tables to see the entries. But you only need to worry about that if you want to get rid of the sleeps (though you'd still need like a ~1ms sleep to let Talon process). Otherwise you can keep your tests as above.

One final note, the conversion for RPM to native units per 100ms should be dividing by 600, not multiplying.

[Piphi5]

It worked for me. Thank you so much!