Added TimedRobot (#520)

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

@@ -10,285 +10,30 @@ package edu.wpi.first.wpilibj;
 import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
 import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.wpilibj.hal.HAL;
-import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 
 /**
- * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase
- * class.
+ * IterativeRobot implements the IterativeRobotBase robot program framework.
  *
  * <p>The IterativeRobot class is intended to be subclassed by a user creating a robot program.
  *
- * <p>This class is intended to implement the "old style" default code, by providing the following
- * functions which are called by the main loop, startCompetition(), at the appropriate times:
- *
- * <p>robotInit() -- provide for initialization at robot power-on
- *
- * <p>init() functions -- each of the following functions is called once when the appropriate mode
- * is entered: - DisabledInit() -- called only when first disabled - AutonomousInit() -- called each
- * and every time autonomous is entered from another mode - TeleopInit() -- called each and every
- * time teleop is entered from another mode - TestInit() -- called each and every time test mode is
- * entered from anothermode
- *
- * <p>Periodic() functions -- each of these functions is called iteratively at the appropriate
- * periodic rate (aka the "slow loop"). The period of the iterative robot is synced to the driver
- * station control packets, giving a periodic frequency of about 50Hz (50 times per second). -
- * disabledPeriodic() - autonomousPeriodic() - teleopPeriodic() - testPeriodoc()
+ * <p>periodic() functions from the base class are called each time a new packet is received from
+ * the driver station.
  */
-public class IterativeRobot extends RobotBase {
-  private boolean m_disabledInitialized;
-  private boolean m_autonomousInitialized;
-  private boolean m_teleopInitialized;
-  private boolean m_testInitialized;
-
-  /**
-   * Constructor for RobotIterativeBase.
-   *
-   * <p>The constructor initializes the instance variables for the robot to indicate the status of
-   * initialization for disabled, autonomous, and teleop code.
-   */
+public class IterativeRobot extends IterativeRobotBase {
   public IterativeRobot() {
-    // set status for initialization of disabled, autonomous, and teleop code.
-    m_disabledInitialized = false;
-    m_autonomousInitialized = false;
-    m_teleopInitialized = false;
-    m_testInitialized = false;
+    HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
   }
 
   /**
    * Provide an alternate "main loop" via startCompetition().
    */
   public void startCompetition() {
-    HAL.report(tResourceType.kResourceType_Framework,
-                                   tInstances.kFramework_Iterative);
-
-    robotInit();
-
-    // Tell the DS that the robot is ready to be enabled
-    HAL.observeUserProgramStarting();
-
     // loop forever, calling the appropriate mode-dependent function
-    LiveWindow.setEnabled(false);
     while (true) {
       // Wait for new data to arrive
       m_ds.waitForData();
-      // Call the appropriate function depending upon the current robot mode
-      if (isDisabled()) {
-        // call DisabledInit() if we are now just entering disabled mode from
-        // either a different mode or from power-on
-        if (!m_disabledInitialized) {
-          LiveWindow.setEnabled(false);
-          disabledInit();
-          m_disabledInitialized = true;
-          // reset the initialization flags for the other modes
-          m_autonomousInitialized = false;
-          m_teleopInitialized = false;
-          m_testInitialized = false;
-        }
-        HAL.observeUserProgramDisabled();
-        disabledPeriodic();
-      } else if (isTest()) {
-        // call TestInit() if we are now just entering test mode from either
-        // a different mode or from power-on
-        if (!m_testInitialized) {
-          LiveWindow.setEnabled(true);
-          testInit();
-          m_testInitialized = true;
-          m_autonomousInitialized = false;
-          m_teleopInitialized = false;
-          m_disabledInitialized = false;
-        }
-        HAL.observeUserProgramTest();
-        testPeriodic();
-      } else if (isAutonomous()) {
-        // call Autonomous_Init() if this is the first time
-        // we've entered autonomous_mode
-        if (!m_autonomousInitialized) {
-          LiveWindow.setEnabled(false);
-          // KBS NOTE: old code reset all PWMs and relays to "safe values"
-          // whenever entering autonomous mode, before calling
-          // "Autonomous_Init()"
-          autonomousInit();
-          m_autonomousInitialized = true;
-          m_testInitialized = false;
-          m_teleopInitialized = false;
-          m_disabledInitialized = false;
-        }
-        HAL.observeUserProgramAutonomous();
-        autonomousPeriodic();
-      } else {
-        // call Teleop_Init() if this is the first time
-        // we've entered teleop_mode
-        if (!m_teleopInitialized) {
-          LiveWindow.setEnabled(false);
-          teleopInit();
-          m_teleopInitialized = true;
-          m_testInitialized = false;
-          m_autonomousInitialized = false;
-          m_disabledInitialized = false;
-        }
-        HAL.observeUserProgramTeleop();
-        teleopPeriodic();
-      }
-      robotPeriodic();
-    }
-  }
 
-  /* ----------- Overridable initialization code ----------------- */
-
-  /**
-   * Robot-wide initialization code should go here.
-   *
-   * <p>Users should override this method for default Robot-wide initialization which will be called
-   * when the robot is first powered on. It will be called exactly one time.
-   *
-   * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
-   * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
-   * never indicate that the code is ready, causing the robot to be bypassed in a match.
-   */
-  public void robotInit() {
-    System.out.println("Default IterativeRobot.robotInit() method... Overload me!");
-  }
-
-  /**
-   * Initialization code for disabled mode should go here.
-   *
-   * <p>Users should override this method for initialization code which will be called each time the
-   * robot enters disabled mode.
-   */
-  public void disabledInit() {
-    System.out.println("Default IterativeRobot.disabledInit() method... Overload me!");
-  }
-
-  /**
-   * Initialization code for autonomous mode should go here.
-   *
-   * <p>Users should override this method for initialization code which will be called each time the
-   * robot enters autonomous mode.
-   */
-  public void autonomousInit() {
-    System.out.println("Default IterativeRobot.autonomousInit() method... Overload me!");
-  }
-
-  /**
-   * Initialization code for teleop mode should go here.
-   *
-   * <p>Users should override this method for initialization code which will be called each time the
-   * robot enters teleop mode.
-   */
-  public void teleopInit() {
-    System.out.println("Default IterativeRobot.teleopInit() method... Overload me!");
-  }
-
-  /**
-   * Initialization code for test mode should go here.
-   *
-   * <p>Users should override this method for initialization code which will be called each time the
-   * robot enters test mode.
-   */
-  @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
-  public void testInit() {
-    System.out.println("Default IterativeRobot.testInit() method... Overload me!");
-  }
-
-  /* ----------- Overridable periodic code ----------------- */
-
-  private boolean m_rpFirstRun = true;
-
-  /**
-   * Periodic code for all robot modes should go here.
-   *
-   * <p>This function is called each time a new packet is received from the driver station.
-   *
-   * <p>Packets are received approximately every 20ms.  Fixed loop timing is not guaranteed due to
-   * network timing variability and the function may not be called at all if the Driver Station is
-   * disconnected.  For most use cases the variable timing will not be an issue.  If your code does
-   * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
-   */
-  public void robotPeriodic() {
-    if (m_rpFirstRun) {
-      System.out.println("Default IterativeRobot.robotPeriodic() method... Overload me!");
-      m_rpFirstRun = false;
-    }
-  }
-
-  private boolean m_dpFirstRun = true;
-
-  /**
-   * Periodic code for disabled mode should go here.
-   *
-   * <p>Users should override this method for code which will be called each time a new packet is
-   * received from the driver station and the robot is in disabled mode.
-   *
-   * <p>Packets are received approximately every 20ms.  Fixed loop timing is not guaranteed due to
-   * network timing variability and the function may not be called at all if the Driver Station is
-   * disconnected.  For most use cases the variable timing will not be an issue.  If your code does
-   * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
-   */
-  public void disabledPeriodic() {
-    if (m_dpFirstRun) {
-      System.out.println("Default IterativeRobot.disabledPeriodic() method... Overload me!");
-      m_dpFirstRun = false;
-    }
-  }
-
-  private boolean m_apFirstRun = true;
-
-  /**
-   * Periodic code for autonomous mode should go here.
-   *
-   * <p>Users should override this method for code which will be called each time a new packet is
-   * received from the driver station and the robot is in autonomous mode.
-   *
-   * <p>Packets are received approximately every 20ms.  Fixed loop timing is not guaranteed due to
-   * network timing variability and the function may not be called at all if the Driver Station is
-   * disconnected.  For most use cases the variable timing will not be an issue.  If your code does
-   * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
-   */
-  public void autonomousPeriodic() {
-    if (m_apFirstRun) {
-      System.out.println("Default IterativeRobot.autonomousPeriodic() method... Overload me!");
-      m_apFirstRun = false;
-    }
-  }
-
-  private boolean m_tpFirstRun = true;
-
-  /**
-   * Periodic code for teleop mode should go here.
-   *
-   * <p>Users should override this method for code which will be called each time a new packet is
-   * received from the driver station and the robot is in teleop mode.
-   *
-   * <p>Packets are received approximately every 20ms.  Fixed loop timing is not guaranteed due to
-   * network timing variability and the function may not be called at all if the Driver Station is
-   * disconnected.  For most use cases the variable timing will not be an issue.  If your code does
-   * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
-   */
-  public void teleopPeriodic() {
-    if (m_tpFirstRun) {
-      System.out.println("Default IterativeRobot.teleopPeriodic() method... Overload me!");
-      m_tpFirstRun = false;
-    }
-  }
-
-  private boolean m_tmpFirstRun = true;
-
-  /**
-   * Periodic code for test mode should go here.
-   *
-   * <p>Users should override this method for code which will be called each time a new packet is
-   * received from the driver station and the robot is in test mode.
-   *
-   * <p>Packets are received approximately every 20ms.  Fixed loop timing is not guaranteed due to
-   * network timing variability and the function may not be called at all if the Driver Station is
-   * disconnected.  For most use cases the variable timing will not be an issue.  If your code does
-   * require guaranteed fixed periodic timing, consider using Notifier or PIDController instead.
-   */
-  @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
-  public void testPeriodic() {
-    if (m_tmpFirstRun) {
-      System.out.println("Default IterativeRobot.testPeriodic() method... Overload me!");
-      m_tmpFirstRun = false;
+      loopFunc();
     }
   }
 }

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

@@ -0,0 +1,236 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2017. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.hal.HAL;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * IterativeRobotBase implements a specific type of robot program framework, extending the RobotBase
+ * class.
+ *
+ * <p>The IterativeRobotBase class does not implement startCompetition(), so it should not be used
+ * by teams directly.
+ *
+ * <p>This class provides the following functions which are called by the main loop,
+ * startCompetition(), at the appropriate times:
+ *
+ * <p>robotInit() -- provide for initialization at robot power-on
+ *
+ * <p>init() functions -- each of the following functions is called once when the
+ * appropriate mode is entered:
+ *   - disabledInit()   -- called only when first disabled
+ *   - autonomousInit() -- called each and every time autonomous is entered from
+ *                         another mode
+ *   - teleopInit()     -- called each and every time teleop is entered from
+ *                         another mode
+ *   - testInit()       -- called each and every time test is entered from
+ *                         another mode
+ *
+ * <p>periodic() functions -- each of these functions is called on an interval:
+ *   - robotPeriodic()
+ *   - disabledPeriodic()
+ *   - autonomousPeriodic()
+ *   - teleopPeriodic()
+ *   - testPeriodic()
+ */
+public abstract class IterativeRobotBase extends RobotBase {
+  private enum Mode {
+    kNone,
+    kDisabled,
+    kAutonomous,
+    kTeleop,
+    kTest
+  }
+
+  private Mode m_lastMode = Mode.kNone;
+
+  /**
+   * IterativeRobotBase constructor.
+   */
+  public IterativeRobotBase() {
+    robotInit();
+
+    // Tell the DS that the robot is ready to be enabled
+    HAL.observeUserProgramStarting();
+  }
+
+  /**
+   * Provide an alternate "main loop" via startCompetition().
+   */
+  public abstract void startCompetition();
+
+  /* ----------- Overridable initialization code ----------------- */
+
+  /**
+   * Robot-wide initialization code should go here.
+   *
+   * <p>Users should override this method for default Robot-wide initialization which will be called
+   * when the robot is first powered on. It will be called exactly one time.
+   *
+   * <p>Warning: the Driver Station "Robot Code" light and FMS "Robot Ready" indicators will be off
+   * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
+   * never indicate that the code is ready, causing the robot to be bypassed in a match.
+   */
+  public void robotInit() {
+    System.out.println("Default robotInit() method... Overload me!");
+  }
+
+  /**
+   * Initialization code for disabled mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters disabled mode.
+   */
+  public void disabledInit() {
+    System.out.println("Default disabledInit() method... Overload me!");
+  }
+
+  /**
+   * Initialization code for autonomous mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters autonomous mode.
+   */
+  public void autonomousInit() {
+    System.out.println("Default autonomousInit() method... Overload me!");
+  }
+
+  /**
+   * Initialization code for teleop mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters teleop mode.
+   */
+  public void teleopInit() {
+    System.out.println("Default teleopInit() method... Overload me!");
+  }
+
+  /**
+   * Initialization code for test mode should go here.
+   *
+   * <p>Users should override this method for initialization code which will be called each time the
+   * robot enters test mode.
+   */
+  @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+  public void testInit() {
+    System.out.println("Default testInit() method... Overload me!");
+  }
+
+  /* ----------- Overridable periodic code ----------------- */
+
+  private boolean m_rpFirstRun = true;
+
+  /**
+   * Periodic code for all robot modes should go here.
+   */
+  public void robotPeriodic() {
+    if (m_rpFirstRun) {
+      System.out.println("Default robotPeriodic() method... Overload me!");
+      m_rpFirstRun = false;
+    }
+  }
+
+  private boolean m_dpFirstRun = true;
+
+  /**
+   * Periodic code for disabled mode should go here.
+   */
+  public void disabledPeriodic() {
+    if (m_dpFirstRun) {
+      System.out.println("Default disabledPeriodic() method... Overload me!");
+      m_dpFirstRun = false;
+    }
+  }
+
+  private boolean m_apFirstRun = true;
+
+  /**
+   * Periodic code for autonomous mode should go here.
+   */
+  public void autonomousPeriodic() {
+    if (m_apFirstRun) {
+      System.out.println("Default autonomousPeriodic() method... Overload me!");
+      m_apFirstRun = false;
+    }
+  }
+
+  private boolean m_tpFirstRun = true;
+
+  /**
+   * Periodic code for teleop mode should go here.
+   */
+  public void teleopPeriodic() {
+    if (m_tpFirstRun) {
+      System.out.println("Default teleopPeriodic() method... Overload me!");
+      m_tpFirstRun = false;
+    }
+  }
+
+  private boolean m_tmpFirstRun = true;
+
+  /**
+   * Periodic code for test mode should go here.
+   */
+  @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+  public void testPeriodic() {
+    if (m_tmpFirstRun) {
+      System.out.println("Default testPeriodic() method... Overload me!");
+      m_tmpFirstRun = false;
+    }
+  }
+
+  protected void loopFunc() {
+    // Call the appropriate function depending upon the current robot mode
+    if (isDisabled()) {
+      // call DisabledInit() if we are now just entering disabled mode from
+      // either a different mode or from power-on
+      if (m_lastMode != Mode.kDisabled) {
+        LiveWindow.setEnabled(false);
+        disabledInit();
+        m_lastMode = Mode.kDisabled;
+      }
+      HAL.observeUserProgramDisabled();
+      disabledPeriodic();
+    } else if (isAutonomous()) {
+      // call Autonomous_Init() if this is the first time
+      // we've entered autonomous_mode
+      if (m_lastMode != Mode.kAutonomous) {
+        LiveWindow.setEnabled(false);
+        // KBS NOTE: old code reset all PWMs and relays to "safe values"
+        // whenever entering autonomous mode, before calling
+        // "Autonomous_Init()"
+        autonomousInit();
+        m_lastMode = Mode.kAutonomous;
+      }
+      HAL.observeUserProgramAutonomous();
+      autonomousPeriodic();
+    } else if (isOperatorControl()) {
+      // call Teleop_Init() if this is the first time
+      // we've entered teleop_mode
+      if (m_lastMode != Mode.kTeleop) {
+        LiveWindow.setEnabled(false);
+        teleopInit();
+        m_lastMode = Mode.kTeleop;
+      }
+      HAL.observeUserProgramTeleop();
+      teleopPeriodic();
+    } else {
+      // call TestInit() if we are now just entering test mode from either
+      // a different mode or from power-on
+      if (m_lastMode != Mode.kTest) {
+        LiveWindow.setEnabled(true);
+        testInit();
+        m_lastMode = Mode.kTest;
+      }
+      HAL.observeUserProgramTest();
+      testPeriodic();
+    }
+    robotPeriodic();
+  }
+}

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

@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.wpilibj.hal.HAL;
 import edu.wpi.first.wpilibj.internal.HardwareHLUsageReporting;
 import edu.wpi.first.wpilibj.internal.HardwareTimer;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.util.WPILibVersion;
 
@@ -55,16 +56,14 @@ public abstract class RobotBase {
    * to put this code into it's own task that loads on boot so ensure that it runs.
    */
   protected RobotBase() {
-    // TODO: StartCAPI();
-    // TODO: See if the next line is necessary
-    // Resource.RestartProgram();
-
     NetworkTable.setNetworkIdentity("Robot");
     NetworkTable.setPersistentFilename("/home/lvuser/networktables.ini");
     NetworkTable.setServerMode(); // must be before b
     m_ds = DriverStation.getInstance();
     NetworkTable.getTable(""); // forces network tables to initialize
     NetworkTable.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", false);
+
+    LiveWindow.setEnabled(false);
   }
 
   /**
@@ -212,6 +211,8 @@ public abstract class RobotBase {
       }
     }
 
+    System.out.println("********** Robot program starting **********");
+
     RobotBase robot;
     try {
       robot = (RobotBase) Class.forName(robotName).newInstance();
@@ -244,7 +245,6 @@ public abstract class RobotBase {
 
     boolean errorOnExit = false;
     try {
-      System.out.println("********** Robot program starting **********");
       robot.startCompetition();
     } catch (Throwable throwable) {
       DriverStation.reportError(

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

@@ -25,14 +25,18 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow;
  */
 public class SampleRobot extends RobotBase {
 
-  private boolean m_robotMainOverridden;
+  private boolean m_robotMainOverridden = true;
 
   /**
    * Create a new SampleRobot.
    */
   public SampleRobot() {
-    super();
-    m_robotMainOverridden = true;
+    robotInit();
+
+    // Tell the DS that the robot is ready to be enabled
+    HAL.observeUserProgramStarting();
+
+    HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Simple);
   }
 
   /**
@@ -111,19 +115,8 @@ public class SampleRobot extends RobotBase {
    * for the robot to be enabled again.
    */
   public void startCompetition() {
-    HAL.report(tResourceType.kResourceType_Framework,
-                                   tInstances.kFramework_Simple);
-
-    robotInit();
-
-    // Tell the DS that the robot is ready to be enabled
-    HAL.observeUserProgramStarting();
-
     robotMain();
     if (!m_robotMainOverridden) {
-      // first and one-time initialization
-      LiveWindow.setEnabled(false);
-
       while (true) {
         if (isDisabled()) {
           m_ds.InDisabled(true);

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

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2017. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tInstances;
+import edu.wpi.first.wpilibj.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.hal.HAL;
+
+/**
+ * TimedRobot implements the IterativeRobotBase robot program framework.
+ *
+ * <p>The TimedRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>periodic() functions from the base class are called on an interval by a Notifier instance.
+ */
+public class TimedRobot extends IterativeRobotBase {
+  public static final double DEFAULT_PERIOD = 0.02;
+
+  private double m_period = DEFAULT_PERIOD;
+
+  // Prevents loop from starting if user calls setPeriod() in robotInit()
+  private boolean m_startLoop = false;
+
+  private Notifier m_loop = new Notifier(() -> {
+    loopFunc();
+  });
+
+  public TimedRobot() {
+    // HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Periodic);
+    HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
+  }
+
+  /**
+   * Provide an alternate "main loop" via startCompetition().
+   */
+  public void startCompetition() {
+    // loop forever, calling the appropriate mode-dependent function
+    m_startLoop = true;
+    m_loop.startPeriodic(m_period);
+    while (true) {
+      try {
+        Thread.sleep(1000 * 60 * 60 * 24);
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
+    }
+  }
+
+  /**
+   * Set time period between calls to Periodic() functions.
+   *
+   * @param period Period in seconds.
+   */
+  public void setPeriod(double period) {
+    m_period = period;
+
+    if (m_startLoop) {
+      m_loop.startPeriodic(m_period);
+    }
+  }
+}