Dual Ultrasonic error

[NWChen]

Our robot has two VEX ultrasonic rangefinders, each connected to separate DIO ports on the digital sidecar. We're using the command-based template.

In a subsystem they're declared as:

private Ultrasonic wallSonar, ballSonar;

and in the constructor of the subsystem,

ballSonar = new Ultrasonic(Map.ARM_SONAR_INPUT, Map.ARM_SONAR_OUTPUT);
ballSonar.setEnabled(true);
ballSonar.setAutomaticMode(true);
wallSonar = new Ultrasonic(Map.LAUNCHER_SONAR_INPUT, Map.LAUNCHER_SONAR_OUTPUT);
wallSonar.setEnabled(true);
wallSonar.setAutomaticMode(true);

Ultrasonic.java in WPILib appears to be designed to handle >1 Ultrasonic instance, but when we try to run this code it throws an IllegalThreadStateException.

Things we tried so far:
- Changing the order of assignment (wallSonar before ballSonar, ballSonar before wallSonar); still throws IllegalThreadStateException
- Moving one of the Ultrasonic sensors to a different subsystem; still throws IllegalThreadStateException.
- Declaring only one of the Ultrasonic sensors; having only one Ultrasonic sensor throws no errors, and the sensor works as expected.
- Unit testing with SimpleRobot, like this. Both sensors worked as expected, with no errors thrown.

Any idea why we can't declare/use two Ultrasonic sensors?

Thanks to Jeanne Boyarsky from team 694 at NYC today for helping us try to fix this.

[pblankenbaker]

In looking through the Ultrasonic sensor source code (thanks for pasting in the link), it shows that the m_task variable associated with the background thread is static (a single instance for all Ultrasonic objects).

In looking at the comments for the setAutomaticMode() method, it looks like this method manages all of the configured sensors (to keep them time sequenced so they don't interfere with each other).

So, my best guess is that you only need to call setAutomaticMode(true) one time (even though it is not a static method).

Can you try changing your code to:

ballSonar = new Ultrasonic(Map.ARM_SONAR_INPUT, Map.ARM_SONAR_OUTPUT);
ballSonar.setEnabled(true);

wallSonar = new Ultrasonic(Map.LAUNCHER_SONAR_INPUT, Map.LAUNCHER_SONAR_OUTPUT);
wallSonar.setEnabled(true);

// Enable automatic pinging on all sensors (not sure why this method
// wasn't declared static in Ultrasonic class)
wallSonar.setAutomaticMode(true);

[NWChen]

Originally Posted by pblankenbaker

So, my best guess is that you only need to call setAutomaticMode(true) one time (even though it is not a static method).

Can you try changing your code to:

ballSonar = new Ultrasonic(Map.ARM_SONAR_INPUT, Map.ARM_SONAR_OUTPUT);
ballSonar.setEnabled(true);

wallSonar = new Ultrasonic(Map.LAUNCHER_SONAR_INPUT, Map.LAUNCHER_SONAR_OUTPUT);
wallSonar.setEnabled(true);

// Enable automatic pinging on all sensors (not sure why this method
// wasn't declared static in Ultrasonic class)
wallSonar.setAutomaticMode(true);

That makes sense to me. We didn't try this yet, thanks for the suggestion.

Having said that (and having not checked CD this morning, oops), we tried something else. Calling each constructor consecutively rather than after each setEnabled() and setAutomaticMode() method appeared to resolve the issue:

ballSonar = new Ultrasonic(Map.ARM_SONAR_INPUT, Map.ARM_SONAR_OUTPUT);
wallSonar = new Ultrasonic(Map.LAUNCHER_SONAR_INPUT, Map.LAUNCHER_SONAR_OUTPUT);
ballSonar.setEnabled(true);
ballSonar.setAutomaticMode(true);
wallSonar.setEnabled(true);
wallSonar.setAutomaticMode(true);

Both sensors returned accurate values from getRangeInches() after reordering these assignments in the constructor of the subsystem they were declared in:

private Ultrasonic wallSonar, ballSonar;

public Arms(){
    ballSonar = new Ultrasonic(Map.ARM_SONAR_INPUT, Map.ARM_SONAR_OUTPUT);
    wallSonar = new Ultrasonic(Map.LAUNCHER_SONAR_INPUT, Map.LAUNCHER_SONAR_OUTPUT);
    ballSonar.setEnabled(true);
    ballSonar.setAutomaticMode(true);
    wallSonar.setEnabled(true);
    wallSonar.setAutomaticMode(true);
...
}