001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj;
006
007import edu.wpi.first.cameraserver.CameraServerShared;
008import edu.wpi.first.cameraserver.CameraServerSharedStore;
009import edu.wpi.first.hal.HAL;
010import edu.wpi.first.hal.HALUtil;
011import edu.wpi.first.math.MathShared;
012import edu.wpi.first.math.MathSharedStore;
013import edu.wpi.first.networktables.MultiSubscriber;
014import edu.wpi.first.networktables.NetworkTableEvent;
015import edu.wpi.first.networktables.NetworkTableInstance;
016import edu.wpi.first.util.WPIUtilJNI;
017import edu.wpi.first.wpilibj.util.WPILibVersion;
018import java.util.concurrent.locks.ReentrantLock;
019import java.util.function.Supplier;
020
021/**
022 * Implement a Robot Program framework. The RobotBase class is intended to be subclassed to create a
023 * robot program. The user must implement {@link #startCompetition()}, which will be called once and
024 * is not expected to exit. The user must also implement {@link #endCompetition()}, which signals to
025 * the code in {@link #startCompetition()} that it should exit.
026 *
027 * <p>It is not recommended to subclass this class directly - instead subclass IterativeRobotBase or
028 * TimedRobot.
029 */
030public abstract class RobotBase implements AutoCloseable {
031  /** The ID of the main Java thread. */
032  // This is usually 1, but it is best to make sure
033  private static long m_threadId = -1;
034
035  private final MultiSubscriber m_suball;
036
037  private final int m_connListenerHandle;
038
039  private static void setupCameraServerShared() {
040    CameraServerShared shared =
041        new CameraServerShared() {
042          @Override
043          public void reportUsage(String resource, String data) {
044            HAL.reportUsage(resource, data);
045          }
046
047          @Override
048          public void reportDriverStationError(String error) {
049            DriverStation.reportError(error, true);
050          }
051
052          @Override
053          public Long getRobotMainThreadId() {
054            return RobotBase.getMainThreadId();
055          }
056
057          @Override
058          public boolean isRoboRIO() {
059            return !RobotBase.isSimulation();
060          }
061        };
062
063    CameraServerSharedStore.setCameraServerShared(shared);
064  }
065
066  private static void setupMathShared() {
067    MathSharedStore.setMathShared(
068        new MathShared() {
069          @Override
070          public void reportError(String error, StackTraceElement[] stackTrace) {
071            DriverStation.reportError(error, stackTrace);
072          }
073
074          @Override
075          public void reportUsage(String resource, String data) {
076            HAL.reportUsage(resource, data);
077          }
078
079          @Override
080          public double getTimestamp() {
081            return Timer.getTimestamp();
082          }
083        });
084  }
085
086  /**
087   * Constructor for a generic robot program. User code can be placed in the constructor that runs
088   * before the Autonomous or Operator Control period starts. The constructor will run to completion
089   * before Autonomous is entered.
090   *
091   * <p>This must be used to ensure that the communications code starts. In the future it would be
092   * nice to put this code into its own task that loads on boot so ensure that it runs.
093   */
094  protected RobotBase() {
095    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
096    m_threadId = Thread.currentThread().getId();
097    setupCameraServerShared();
098    setupMathShared();
099    // subscribe to "" to force persistent values to propagate to local
100    m_suball = new MultiSubscriber(inst, new String[] {""});
101    if (!isSimulation()) {
102      inst.startServer("/home/systemcore/networktables.json");
103    } else {
104      inst.startServer();
105    }
106
107    // wait for the NT server to actually start
108    try {
109      int count = 0;
110      while (inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
111        Thread.sleep(10);
112        count++;
113        if (count > 100) {
114          throw new InterruptedException();
115        }
116      }
117    } catch (InterruptedException ex) {
118      System.err.println("timed out while waiting for NT server to start");
119    }
120
121    m_connListenerHandle =
122        inst.addConnectionListener(
123            false,
124            event -> {
125              if (event.is(NetworkTableEvent.Kind.kConnected)) {
126                HAL.reportUsage("NT/" + event.connInfo.remote_id, "");
127              }
128            });
129  }
130
131  /**
132   * Returns the main thread ID.
133   *
134   * @return The main thread ID.
135   */
136  public static long getMainThreadId() {
137    return m_threadId;
138  }
139
140  @Override
141  public void close() {
142    m_suball.close();
143    NetworkTableInstance.getDefault().removeListener(m_connListenerHandle);
144  }
145
146  /**
147   * Get the current runtime type.
148   *
149   * @return Current runtime type.
150   */
151  public static RuntimeType getRuntimeType() {
152    return RuntimeType.getValue(HALUtil.getHALRuntimeType());
153  }
154
155  /**
156   * Get if the robot is a simulation.
157   *
158   * @return If the robot is running in simulation.
159   */
160  public static boolean isSimulation() {
161    return getRuntimeType() == RuntimeType.kSimulation;
162  }
163
164  /**
165   * Get if the robot is real.
166   *
167   * @return If the robot is running in the real world.
168   */
169  public static boolean isReal() {
170    RuntimeType runtimeType = getRuntimeType();
171    return runtimeType == RuntimeType.kSystemCore;
172  }
173
174  /**
175   * Determine if the Robot is currently disabled.
176   *
177   * @return True if the Robot is currently disabled by the Driver Station.
178   */
179  public boolean isDisabled() {
180    return DriverStation.isDisabled();
181  }
182
183  /**
184   * Determine if the Robot is currently enabled.
185   *
186   * @return True if the Robot is currently enabled by the Driver Station.
187   */
188  public boolean isEnabled() {
189    return DriverStation.isEnabled();
190  }
191
192  /**
193   * Determine if the robot is currently in Autonomous mode as determined by the Driver Station.
194   *
195   * @return True if the robot is currently operating Autonomously.
196   */
197  public boolean isAutonomous() {
198    return DriverStation.isAutonomous();
199  }
200
201  /**
202   * Determine if the robot is currently in Autonomous mode and enabled as determined by the Driver
203   * Station.
204   *
205   * @return True if the robot is currently operating autonomously while enabled.
206   */
207  public boolean isAutonomousEnabled() {
208    return DriverStation.isAutonomousEnabled();
209  }
210
211  /**
212   * Determine if the robot is currently in Test mode as determined by the Driver Station.
213   *
214   * @return True if the robot is currently operating in Test mode.
215   */
216  public boolean isTest() {
217    return DriverStation.isTest();
218  }
219
220  /**
221   * Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
222   *
223   * @return True if the robot is currently operating in Test mode while enabled.
224   */
225  public boolean isTestEnabled() {
226    return DriverStation.isTestEnabled();
227  }
228
229  /**
230   * Determine if the robot is currently in Operator Control mode as determined by the Driver
231   * Station.
232   *
233   * @return True if the robot is currently operating in Tele-Op mode.
234   */
235  public boolean isTeleop() {
236    return DriverStation.isTeleop();
237  }
238
239  /**
240   * Determine if the robot is currently in Operator Control mode and enabled as determined by the
241   * Driver Station.
242   *
243   * @return True if the robot is currently operating in Tele-Op mode while enabled.
244   */
245  public boolean isTeleopEnabled() {
246    return DriverStation.isTeleopEnabled();
247  }
248
249  /**
250   * Start the main robot code. This function will be called once and should not exit until
251   * signalled by {@link #endCompetition()}
252   */
253  public abstract void startCompetition();
254
255  /** Ends the main loop in {@link #startCompetition()}. */
256  public abstract void endCompetition();
257
258  private static final ReentrantLock m_runMutex = new ReentrantLock();
259  private static RobotBase m_robotCopy;
260  private static boolean m_suppressExitWarning;
261
262  /** Run the robot main loop. */
263  private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
264    System.out.println("********** Robot program starting **********");
265
266    T robot;
267    try {
268      robot = robotSupplier.get();
269    } catch (Throwable throwable) {
270      Throwable cause = throwable.getCause();
271      if (cause != null) {
272        throwable = cause;
273      }
274      String robotName = "Unknown";
275      StackTraceElement[] elements = throwable.getStackTrace();
276      if (elements.length > 0) {
277        robotName = elements[0].getClassName();
278      }
279      DriverStation.reportError(
280          "Unhandled exception instantiating robot " + robotName + " " + throwable, elements);
281      DriverStation.reportError(
282          "The robot program quit unexpectedly."
283              + " This is usually due to a code error.\n"
284              + "  The above stacktrace can help determine where the error occurred.\n"
285              + "  See https://wpilib.org/stacktrace for more information.\n",
286          false);
287      DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
288      return;
289    }
290
291    m_runMutex.lock();
292    m_robotCopy = robot;
293    m_runMutex.unlock();
294
295    boolean errorOnExit = false;
296    try {
297      robot.startCompetition();
298    } catch (Throwable throwable) {
299      Throwable cause = throwable.getCause();
300      if (cause != null) {
301        throwable = cause;
302      }
303      DriverStation.reportError("Unhandled exception: " + throwable, throwable.getStackTrace());
304      errorOnExit = true;
305    } finally {
306      m_runMutex.lock();
307      boolean suppressExitWarning = m_suppressExitWarning;
308      m_runMutex.unlock();
309      if (!suppressExitWarning) {
310        // startCompetition never returns unless exception occurs....
311        DriverStation.reportWarning(
312            "The robot program quit unexpectedly."
313                + " This is usually due to a code error.\n"
314                + "  The above stacktrace can help determine where the error occurred.\n"
315                + "  See https://wpilib.org/stacktrace for more information.",
316            false);
317        if (errorOnExit) {
318          DriverStation.reportError(
319              "The startCompetition() method (or methods called by it) should have "
320                  + "handled the exception above.",
321              false);
322        } else {
323          DriverStation.reportError("Unexpected return from startCompetition() method.", false);
324        }
325      }
326    }
327  }
328
329  /**
330   * Suppress the "The robot program quit unexpectedly." message.
331   *
332   * @param value True if exit warning should be suppressed.
333   */
334  public static void suppressExitWarning(boolean value) {
335    m_runMutex.lock();
336    m_suppressExitWarning = value;
337    m_runMutex.unlock();
338  }
339
340  /**
341   * Starting point for the applications.
342   *
343   * @param <T> Robot subclass.
344   * @param robotSupplier Function that returns an instance of the robot subclass.
345   */
346  public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
347    // Check that the MSVC runtime is valid.
348    WPIUtilJNI.checkMsvcRuntime();
349
350    if (!HAL.initialize(500, 0)) {
351      throw new IllegalStateException("Failed to initialize. Terminating");
352    }
353
354    // Force refresh DS data
355    DriverStation.refreshData();
356
357    HAL.reportUsage("Language", "Java");
358    HAL.reportUsage("WPILibVersion", WPILibVersion.Version);
359
360    if (!Notifier.setHALThreadPriority(true, 40)) {
361      DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
362    }
363
364    if (HAL.hasMain()) {
365      Thread thread =
366          new Thread(
367              () -> {
368                runRobot(robotSupplier);
369                HAL.exitMain();
370              },
371              "robot main");
372      thread.setDaemon(true);
373      thread.start();
374      HAL.runMain();
375      suppressExitWarning(true);
376      m_runMutex.lock();
377      RobotBase robot = m_robotCopy;
378      m_runMutex.unlock();
379      if (robot != null) {
380        robot.endCompetition();
381      }
382      try {
383        thread.join(1000);
384      } catch (InterruptedException ex) {
385        Thread.currentThread().interrupt();
386      }
387    } else {
388      runRobot(robotSupplier);
389    }
390
391    // On RIO, this will just terminate rather than shutting down cleanly (it's a no-op in sim).
392    // It's not worth the risk of hanging on shutdown when we want the code to restart as quickly
393    // as possible.
394    HAL.terminate();
395
396    HAL.shutdown();
397
398    System.exit(0);
399  }
400}