Reordered DriverStation functions in wpilibc and wpilibj to match wpilibc header (#166)

wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -67,7 +67,7 @@ public class DriverStation implements RobotState.Interface {
     }
 
     public void run() {
-      m_ds.task();
+      m_ds.run();
     }
   } /* DriverStationTask */
 
@@ -116,7 +116,7 @@ public class DriverStation implements RobotState.Interface {
    * <p>The single DriverStation instance is created statically with the instance static member
    * variable.
    */
-  protected DriverStation() {
+  private DriverStation() {
     m_dataSem = new Object();
     m_joystickMutex = new Object();
     m_newControlDataMutex = new Object();
@@ -144,128 +144,46 @@ public class DriverStation implements RobotState.Interface {
   }
 
   /**
-   * Provides the service routine for the DS polling m_thread.
-   */
-  private void task() {
-    int safetyCounter = 0;
-    while (m_threadKeepAlive) {
-      HAL.waitForDSData();
-      getData();
-      synchronized (m_dataSem) {
-        m_updatedControlLoopData = true;
-        m_dataSem.notifyAll();
-      }
-      if (++safetyCounter >= 4) {
-        MotorSafetyHelper.checkMotors();
-        safetyCounter = 0;
-      }
-      if (m_userInDisabled) {
-        HAL.observeUserProgramDisabled();
-      }
-      if (m_userInAutonomous) {
-        HAL.observeUserProgramAutonomous();
-      }
-      if (m_userInTeleop) {
-        HAL.observeUserProgramTeleop();
-      }
-      if (m_userInTest) {
-        HAL.observeUserProgramTest();
-      }
-    }
-  }
-
-  /**
-   * Wait for new data from the driver station.
-   */
-  public void waitForData() {
-    waitForData(0);
-  }
-
-  /**
-   * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
-   * only.
+   * Report error to Driver Station. Also prints error to System.err Optionally appends Stack trace
+   * to error message.
    *
-   * @param timeout The maximum time in milliseconds to wait.
+   * @param printTrace If true, append stack trace to error string
    */
-  public void waitForData(long timeout) {
-    synchronized (m_dataSem) {
-      try {
-        while (!m_updatedControlLoopData) {
-          m_dataSem.wait(timeout);
-        }
-        m_updatedControlLoopData = false;
-      } catch (InterruptedException ex) {
-        Thread.currentThread().interrupt();
+  public static void reportError(String error, boolean printTrace) {
+    reportErrorImpl(true, 1, error, printTrace);
+  }
+
+  /**
+   * Report warning to Driver Station. Also prints error to System.err Optionally appends Stack
+   * trace to warning message.
+   *
+   * @param printTrace If true, append stack trace to warning string
+   */
+  public static void reportWarning(String error, boolean printTrace) {
+    reportErrorImpl(false, 1, error, printTrace);
+  }
+
+  private static void reportErrorImpl(boolean isError, int code, String error, boolean
+      printTrace) {
+    StackTraceElement[] traces = Thread.currentThread().getStackTrace();
+    String locString;
+    if (traces.length > 3) {
+      locString = traces[3].toString();
+    } else {
+      locString = new String();
+    }
+    boolean haveLoc = false;
+    String traceString = " at ";
+    for (int i = 3; i < traces.length; i++) {
+      String loc = traces[i].toString();
+      traceString += loc + "\n";
+      // get first user function
+      if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
+        locString = loc;
+        haveLoc = true;
       }
     }
-  }
-
-  /**
-   * Copy data from the DS task for the user. If no new data exists, it will just be returned,
-   * otherwise the data will be copied from the DS polling loop.
-   */
-  protected void getData() {
-    // Get the status of all of the joysticks
-    for (byte stick = 0; stick < kJoystickPorts; stick++) {
-      m_joystickAxesCache[stick].m_count =
-          HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
-      m_joystickPOVsCache[stick].m_count =
-          HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
-      m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
-      m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
-    }
-    // lock joystick mutex to swap cache data
-    synchronized (m_joystickMutex) {
-      // move cache to actual data
-      HALJoystickAxes[] currentAxes = m_joystickAxes;
-      m_joystickAxes = m_joystickAxesCache;
-      m_joystickAxesCache = currentAxes;
-
-      HALJoystickButtons[] currentButtons = m_joystickButtons;
-      m_joystickButtons = m_joystickButtonsCache;
-      m_joystickButtonsCache = currentButtons;
-
-      HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
-      m_joystickPOVs = m_joystickPOVsCache;
-      m_joystickPOVsCache = currentPOVs;
-    }
-    //Lock new control data mutex and set new control data.
-    synchronized (m_newControlDataMutex) {
-      m_newControlData = true;
-    }
-  }
-
-  /**
-   * Read the battery voltage.
-   *
-   * @return The battery voltage in Volts.
-   */
-  public double getBatteryVoltage() {
-    return PowerJNI.getVinVoltage();
-  }
-
-  /**
-   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
-   * the DS.
-   */
-  private void reportJoystickUnpluggedError(String message) {
-    double currentTime = Timer.getFPGATimestamp();
-    if (currentTime > m_nextMessageTime) {
-      reportError(message, false);
-      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
-    }
-  }
-
-  /**
-   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
-   * the DS.
-   */
-  private void reportJoystickUnpluggedWarning(String message) {
-    double currentTime = Timer.getFPGATimestamp();
-    if (currentTime > m_nextMessageTime) {
-      reportWarning(message, false);
-      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
-    }
+    HAL.sendError(isError, code, false, error, locString, printTrace ? traceString : "", true);
   }
 
   /**
@@ -302,21 +220,6 @@ public class DriverStation implements RobotState.Interface {
     return retVal;
   }
 
-  /**
-   * Returns the number of axes on a given joystick port.
-   *
-   * @param stick The joystick port number
-   * @return The number of axes on the indicated joystick
-   */
-  public int getStickAxisCount(int stick) {
-    if (stick < 0 || stick >= kJoystickPorts) {
-      throw new RuntimeException("Joystick index is out of range, should be 0-5");
-    }
-    synchronized (m_joystickMutex) {
-      return m_joystickAxes[stick].m_count;
-    }
-  }
-
   /**
    * Get the state of a POV on the joystick.
    *
@@ -346,21 +249,6 @@ public class DriverStation implements RobotState.Interface {
     return retVal;
   }
 
-  /**
-   * Returns the number of POVs on a given joystick port.
-   *
-   * @param stick The joystick port number
-   * @return The number of POVs on the indicated joystick
-   */
-  public int getStickPOVCount(int stick) {
-    if (stick < 0 || stick >= kJoystickPorts) {
-      throw new RuntimeException("Joystick index is out of range, should be 0-5");
-    }
-    synchronized (m_joystickMutex) {
-      return m_joystickPOVs[stick].m_count;
-    }
-  }
-
   /**
    * The state of the buttons on the joystick.
    *
@@ -408,6 +296,36 @@ public class DriverStation implements RobotState.Interface {
     return retVal;
   }
 
+  /**
+   * Returns the number of axes on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The number of axes on the indicated joystick
+   */
+  public int getStickAxisCount(int stick) {
+    if (stick < 0 || stick >= kJoystickPorts) {
+      throw new RuntimeException("Joystick index is out of range, should be 0-5");
+    }
+    synchronized (m_joystickMutex) {
+      return m_joystickAxes[stick].m_count;
+    }
+  }
+
+  /**
+   * Returns the number of POVs on a given joystick port.
+   *
+   * @param stick The joystick port number
+   * @return The number of POVs on the indicated joystick
+   */
+  public int getStickPOVCount(int stick) {
+    if (stick < 0 || stick >= kJoystickPorts) {
+      throw new RuntimeException("Joystick index is out of range, should be 0-5");
+    }
+    synchronized (m_joystickMutex) {
+      return m_joystickPOVs[stick].m_count;
+    }
+  }
+
   /**
    * Gets the number of buttons on a joystick.
    *
@@ -540,6 +458,16 @@ public class DriverStation implements RobotState.Interface {
     return controlWord.getAutonomous();
   }
 
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controlled mode.
+   *
+   * @return True if operator-controlled mode should be enabled, false otherwise.
+   */
+  public boolean isOperatorControl() {
+    return !(isAutonomous() || isTest());
+  }
+
   /**
    * Gets a value indicating whether the Driver Station requires the robot to be running in test
    * mode.
@@ -552,13 +480,38 @@ public class DriverStation implements RobotState.Interface {
   }
 
   /**
-   * Gets a value indicating whether the Driver Station requires the robot to be running in
-   * operator-controlled mode.
+   * Gets a value indicating whether the Driver Station is attached.
    *
-   * @return True if operator-controlled mode should be enabled, false otherwise.
+   * @return True if Driver Station is attached, false otherwise.
    */
-  public boolean isOperatorControl() {
-    return !(isAutonomous() || isTest());
+  public boolean isDSAttached() {
+    ControlWord controlWord = HAL.getControlWord();
+    return controlWord.getDSAttached();
+  }
+
+  /**
+   * Has a new control packet from the driver station arrived since the last time this function was
+   * called?
+   *
+   * @return True if the control data has been updated since the last call.
+   */
+  public boolean isNewControlData() {
+    synchronized (m_newControlDataMutex) {
+      boolean result = m_newControlData;
+      m_newControlData = false;
+      return result;
+    }
+  }
+
+  /**
+   * Is the driver station attached to a Field Management System? Note: This does not work with the
+   * Blue DS.
+   *
+   * @return True if the robot is competing on a field being controlled by a Field Management System
+   */
+  public boolean isFMSAttached() {
+    ControlWord controlWord = HAL.getControlWord();
+    return controlWord.getFMSAttached();
   }
 
   /**
@@ -580,20 +533,6 @@ public class DriverStation implements RobotState.Interface {
     return HAL.getBrownedOut();
   }
 
-  /**
-   * Has a new control packet from the driver station arrived since the last time this function was
-   * called?
-   *
-   * @return True if the control data has been updated since the last call.
-   */
-  public boolean isNewControlData() {
-    synchronized (m_newControlDataMutex) {
-      boolean result = m_newControlData;
-      m_newControlData = false;
-      return result;
-    }
-  }
-
   /**
    * Get the current alliance from the FMS.
    *
@@ -650,19 +589,29 @@ public class DriverStation implements RobotState.Interface {
   }
 
   /**
-   * Is the driver station attached to a Field Management System? Note: This does not work with the
-   * Blue DS.
-   *
-   * @return True if the robot is competing on a field being controlled by a Field Management System
+   * Wait for new data from the driver station.
    */
-  public boolean isFMSAttached() {
-    ControlWord controlWord = HAL.getControlWord();
-    return controlWord.getFMSAttached();
+  public void waitForData() {
+    waitForData(0);
   }
 
-  public boolean isDSAttached() {
-    ControlWord controlWord = HAL.getControlWord();
-    return controlWord.getDSAttached();
+  /**
+   * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
+   * only.
+   *
+   * @param timeout The maximum time in milliseconds to wait.
+   */
+  public void waitForData(long timeout) {
+    synchronized (m_dataSem) {
+      try {
+        while (!m_updatedControlLoopData) {
+          m_dataSem.wait(timeout);
+        }
+        m_updatedControlLoopData = false;
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
+    }
   }
 
   /**
@@ -679,46 +628,12 @@ public class DriverStation implements RobotState.Interface {
   }
 
   /**
-   * Report error to Driver Station. Also prints error to System.err Optionally appends Stack trace
-   * to error message.
+   * Read the battery voltage.
    *
-   * @param printTrace If true, append stack trace to error string
+   * @return The battery voltage in Volts.
    */
-  public static void reportError(String error, boolean printTrace) {
-    reportErrorImpl(true, 1, error, printTrace);
-  }
-
-  /**
-   * Report warning to Driver Station. Also prints error to System.err Optionally appends Stack
-   * trace to warning message.
-   *
-   * @param printTrace If true, append stack trace to warning string
-   */
-  public static void reportWarning(String error, boolean printTrace) {
-    reportErrorImpl(false, 1, error, printTrace);
-  }
-
-  private static void reportErrorImpl(boolean isError, int code, String error, boolean
-      printTrace) {
-    StackTraceElement[] traces = Thread.currentThread().getStackTrace();
-    String locString;
-    if (traces.length > 3) {
-      locString = traces[3].toString();
-    } else {
-      locString = new String();
-    }
-    boolean haveLoc = false;
-    String traceString = " at ";
-    for (int i = 3; i < traces.length; i++) {
-      String loc = traces[i].toString();
-      traceString += loc + "\n";
-      // get first user function
-      if (!haveLoc && !loc.startsWith("edu.wpi.first.wpilibj")) {
-        locString = loc;
-        haveLoc = true;
-      }
-    }
-    HAL.sendError(isError, code, false, error, locString, printTrace ? traceString : "", true);
+  public double getBatteryVoltage() {
+    return PowerJNI.getVinVoltage();
   }
 
   /**
@@ -764,4 +679,94 @@ public class DriverStation implements RobotState.Interface {
   public void InTest(boolean entering) {
     m_userInTest = entering;
   }
+
+  /**
+   * Copy data from the DS task for the user. If no new data exists, it will just be returned,
+   * otherwise the data will be copied from the DS polling loop.
+   */
+  protected void getData() {
+    // Get the status of all of the joysticks
+    for (byte stick = 0; stick < kJoystickPorts; stick++) {
+      m_joystickAxesCache[stick].m_count =
+          HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
+      m_joystickPOVsCache[stick].m_count =
+          HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
+      m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
+      m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
+    }
+    // lock joystick mutex to swap cache data
+    synchronized (m_joystickMutex) {
+      // move cache to actual data
+      HALJoystickAxes[] currentAxes = m_joystickAxes;
+      m_joystickAxes = m_joystickAxesCache;
+      m_joystickAxesCache = currentAxes;
+
+      HALJoystickButtons[] currentButtons = m_joystickButtons;
+      m_joystickButtons = m_joystickButtonsCache;
+      m_joystickButtonsCache = currentButtons;
+
+      HALJoystickPOVs[] currentPOVs = m_joystickPOVs;
+      m_joystickPOVs = m_joystickPOVsCache;
+      m_joystickPOVsCache = currentPOVs;
+    }
+    //Lock new control data mutex and set new control data.
+    synchronized (m_newControlDataMutex) {
+      m_newControlData = true;
+    }
+  }
+
+  /**
+   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+   * the DS.
+   */
+  private void reportJoystickUnpluggedError(String message) {
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      reportError(message, false);
+      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    }
+  }
+
+  /**
+   * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
+   * the DS.
+   */
+  private void reportJoystickUnpluggedWarning(String message) {
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      reportWarning(message, false);
+      m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
+    }
+  }
+
+  /**
+   * Provides the service routine for the DS polling m_thread.
+   */
+  private void run() {
+    int safetyCounter = 0;
+    while (m_threadKeepAlive) {
+      HAL.waitForDSData();
+      getData();
+      synchronized (m_dataSem) {
+        m_updatedControlLoopData = true;
+        m_dataSem.notifyAll();
+      }
+      if (++safetyCounter >= 4) {
+        MotorSafetyHelper.checkMotors();
+        safetyCounter = 0;
+      }
+      if (m_userInDisabled) {
+        HAL.observeUserProgramDisabled();
+      }
+      if (m_userInAutonomous) {
+        HAL.observeUserProgramAutonomous();
+      }
+      if (m_userInTeleop) {
+        HAL.observeUserProgramTeleop();
+      }
+      if (m_userInTest) {
+        HAL.observeUserProgramTest();
+      }
+    }
+  }
 }