Source code

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.cscore.CameraServerJNI;
010import edu.wpi.first.hal.FRCNetComm.tInstances;
011import edu.wpi.first.hal.FRCNetComm.tResourceType;
012import edu.wpi.first.hal.HAL;
013import edu.wpi.first.hal.HALUtil;
014import edu.wpi.first.math.MathShared;
015import edu.wpi.first.math.MathSharedStore;
016import edu.wpi.first.math.MathUsageId;
017import edu.wpi.first.networktables.MultiSubscriber;
018import edu.wpi.first.networktables.NetworkTableInstance;
019import edu.wpi.first.util.WPIUtilJNI;
020import edu.wpi.first.wpilibj.livewindow.LiveWindow;
021import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
022import edu.wpi.first.wpilibj.util.WPILibVersion;
023import java.io.File;
024import java.io.IOException;
025import java.io.OutputStream;
026import java.nio.charset.StandardCharsets;
027import java.nio.file.Files;
028import java.util.concurrent.locks.ReentrantLock;
029import java.util.function.Supplier;
030
031/**
032 * Implement a Robot Program framework. The RobotBase class is intended to be subclassed to create a
033 * robot program. The user must implement {@link #startCompetition()}, which will be called once and
034 * is not expected to exit. The user must also implement {@link #endCompetition()}, which signals to
035 * the code in {@link #startCompetition()} that it should exit.
036 *
037 * <p>It is not recommended to subclass this class directly - instead subclass IterativeRobotBase or
038 * TimedRobot.
039 */
040public abstract class RobotBase implements AutoCloseable {
041  /** The ID of the main Java thread. */
042  // This is usually 1, but it is best to make sure
043  private static long m_threadId = -1;
044
045  private final MultiSubscriber m_suball;
046
047  private static void setupCameraServerShared() {
048    CameraServerShared shared =
049        new CameraServerShared() {
050          @Override
051          public void reportVideoServer(int id) {
052            HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
053          }
054
055          @Override
056          public void reportUsbCamera(int id) {
057            HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
058          }
059
060          @Override
061          public void reportDriverStationError(String error) {
062            DriverStation.reportError(error, true);
063          }
064
065          @Override
066          public void reportAxisCamera(int id) {
067            HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
068          }
069
070          @Override
071          public Long getRobotMainThreadId() {
072            return RobotBase.getMainThreadId();
073          }
074
075          @Override
076          public boolean isRoboRIO() {
077            return RobotBase.isReal();
078          }
079        };
080
081    CameraServerSharedStore.setCameraServerShared(shared);
082  }
083
084  private static void setupMathShared() {
085    MathSharedStore.setMathShared(
086        new MathShared() {
087          @Override
088          public void reportError(String error, StackTraceElement[] stackTrace) {
089            DriverStation.reportError(error, stackTrace);
090          }
091
092          @Override
093          public void reportUsage(MathUsageId id, int count) {
094            switch (id) {
095              case kKinematics_DifferentialDrive:
096                HAL.report(
097                    tResourceType.kResourceType_Kinematics,
098                    tInstances.kKinematics_DifferentialDrive);
099                break;
100              case kKinematics_MecanumDrive:
101                HAL.report(
102                    tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
103                break;
104              case kKinematics_SwerveDrive:
105                HAL.report(
106                    tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
107                break;
108              case kTrajectory_TrapezoidProfile:
109                HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
110                break;
111              case kFilter_Linear:
112                HAL.report(tResourceType.kResourceType_LinearFilter, count);
113                break;
114              case kOdometry_DifferentialDrive:
115                HAL.report(
116                    tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
117                break;
118              case kOdometry_SwerveDrive:
119                HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
120                break;
121              case kOdometry_MecanumDrive:
122                HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
123                break;
124              case kController_PIDController2:
125                HAL.report(tResourceType.kResourceType_PIDController2, count);
126                break;
127              case kController_ProfiledPIDController:
128                HAL.report(tResourceType.kResourceType_ProfiledPIDController, count);
129                break;
130              default:
131                break;
132            }
133          }
134
135          @Override
136          public double getTimestamp() {
137            return WPIUtilJNI.now() * 1.0e-6;
138          }
139        });
140  }
141
142  /**
143   * Constructor for a generic robot program. User code can be placed in the constructor that runs
144   * before the Autonomous or Operator Control period starts. The constructor will run to completion
145   * before Autonomous is entered.
146   *
147   * <p>This must be used to ensure that the communications code starts. In the future it would be
148   * nice to put this code into its own task that loads on boot so ensure that it runs.
149   */
150  protected RobotBase() {
151    final NetworkTableInstance inst = NetworkTableInstance.getDefault();
152    m_threadId = Thread.currentThread().getId();
153    setupCameraServerShared();
154    setupMathShared();
155    // subscribe to "" to force persistent values to propagate to local
156    m_suball = new MultiSubscriber(inst, new String[] {""});
157    if (isReal()) {
158      inst.startServer("/home/lvuser/networktables.json");
159    } else {
160      inst.startServer();
161    }
162
163    // wait for the NT server to actually start
164    try {
165      int count = 0;
166      while (inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
167        Thread.sleep(10);
168        count++;
169        if (count > 100) {
170          throw new InterruptedException();
171        }
172      }
173    } catch (InterruptedException ex) {
174      System.err.println("timed out while waiting for NT server to start");
175    }
176
177    LiveWindow.setEnabled(false);
178    Shuffleboard.disableActuatorWidgets();
179  }
180
181  public static long getMainThreadId() {
182    return m_threadId;
183  }
184
185  @Override
186  public void close() {
187    m_suball.close();
188  }
189
190  /**
191   * Get the current runtime type.
192   *
193   * @return Current runtime type.
194   */
195  public static RuntimeType getRuntimeType() {
196    return RuntimeType.getValue(HALUtil.getHALRuntimeType());
197  }
198
199  /**
200   * Get if the robot is a simulation.
201   *
202   * @return If the robot is running in simulation.
203   */
204  public static boolean isSimulation() {
205    return !isReal();
206  }
207
208  /**
209   * Get if the robot is real.
210   *
211   * @return If the robot is running in the real world.
212   */
213  public static boolean isReal() {
214    RuntimeType runtimeType = getRuntimeType();
215    return runtimeType == RuntimeType.kRoboRIO || runtimeType == RuntimeType.kRoboRIO2;
216  }
217
218  /**
219   * Determine if the Robot is currently disabled.
220   *
221   * @return True if the Robot is currently disabled by the Driver Station.
222   */
223  public boolean isDisabled() {
224    return DriverStation.isDisabled();
225  }
226
227  /**
228   * Determine if the Robot is currently enabled.
229   *
230   * @return True if the Robot is currently enabled by the Driver Station.
231   */
232  public boolean isEnabled() {
233    return DriverStation.isEnabled();
234  }
235
236  /**
237   * Determine if the robot is currently in Autonomous mode as determined by the Driver Station.
238   *
239   * @return True if the robot is currently operating Autonomously.
240   */
241  public boolean isAutonomous() {
242    return DriverStation.isAutonomous();
243  }
244
245  /**
246   * Determine if the robot is currently in Autonomous mode and enabled as determined by the Driver
247   * Station.
248   *
249   * @return True if the robot is currently operating autonomously while enabled.
250   */
251  public boolean isAutonomousEnabled() {
252    return DriverStation.isAutonomousEnabled();
253  }
254
255  /**
256   * Determine if the robot is currently in Test mode as determined by the Driver Station.
257   *
258   * @return True if the robot is currently operating in Test mode.
259   */
260  public boolean isTest() {
261    return DriverStation.isTest();
262  }
263
264  /**
265   * Determine if the robot is current in Test mode and enabled as determined by the Driver Station.
266   *
267   * @return True if the robot is currently operating in Test mode while enabled.
268   */
269  public boolean isTestEnabled() {
270    return DriverStation.isTestEnabled();
271  }
272
273  /**
274   * Determine if the robot is currently in Operator Control mode as determined by the Driver
275   * Station.
276   *
277   * @return True if the robot is currently operating in Tele-Op mode.
278   */
279  public boolean isTeleop() {
280    return DriverStation.isTeleop();
281  }
282
283  /**
284   * Determine if the robot is currently in Operator Control mode and enabled as determined by the
285   * Driver Station.
286   *
287   * @return True if the robot is currently operating in Tele-Op mode while enabled.
288   */
289  public boolean isTeleopEnabled() {
290    return DriverStation.isTeleopEnabled();
291  }
292
293  /**
294   * Start the main robot code. This function will be called once and should not exit until
295   * signalled by {@link #endCompetition()}
296   */
297  public abstract void startCompetition();
298
299  /** Ends the main loop in {@link #startCompetition()}. */
300  public abstract void endCompetition();
301
302  private static final ReentrantLock m_runMutex = new ReentrantLock();
303  private static RobotBase m_robotCopy;
304  private static boolean m_suppressExitWarning;
305
306  /** Run the robot main loop. */
307  private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
308    System.out.println("********** Robot program starting **********");
309
310    T robot;
311    try {
312      robot = robotSupplier.get();
313    } catch (Throwable throwable) {
314      Throwable cause = throwable.getCause();
315      if (cause != null) {
316        throwable = cause;
317      }
318      String robotName = "Unknown";
319      StackTraceElement[] elements = throwable.getStackTrace();
320      if (elements.length > 0) {
321        robotName = elements[0].getClassName();
322      }
323      DriverStation.reportError(
324          "Unhandled exception instantiating robot " + robotName + " " + throwable.toString(),
325          elements);
326      DriverStation.reportError(
327          "The robot program quit unexpectedly."
328              + " This is usually due to a code error.\n"
329              + "  The above stacktrace can help determine where the error occurred.\n"
330              + "  See https://wpilib.org/stacktrace for more information.\n",
331          false);
332      DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
333      return;
334    }
335
336    m_runMutex.lock();
337    m_robotCopy = robot;
338    m_runMutex.unlock();
339
340    if (isReal()) {
341      final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
342      try {
343        if (file.exists() && !file.delete()) {
344          throw new IOException("Failed to delete FRC_Lib_Version.ini");
345        }
346
347        if (!file.createNewFile()) {
348          throw new IOException("Failed to create new FRC_Lib_Version.ini");
349        }
350
351        try (OutputStream output = Files.newOutputStream(file.toPath())) {
352          output.write("Java ".getBytes(StandardCharsets.UTF_8));
353          output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8));
354        }
355      } catch (IOException ex) {
356        DriverStation.reportError(
357            "Could not write FRC_Lib_Version.ini: " + ex.toString(), ex.getStackTrace());
358      }
359    }
360
361    boolean errorOnExit = false;
362    try {
363      robot.startCompetition();
364    } catch (Throwable throwable) {
365      Throwable cause = throwable.getCause();
366      if (cause != null) {
367        throwable = cause;
368      }
369      DriverStation.reportError(
370          "Unhandled exception: " + throwable.toString(), throwable.getStackTrace());
371      errorOnExit = true;
372    } finally {
373      m_runMutex.lock();
374      boolean suppressExitWarning = m_suppressExitWarning;
375      m_runMutex.unlock();
376      if (!suppressExitWarning) {
377        // startCompetition never returns unless exception occurs....
378        DriverStation.reportWarning(
379            "The robot program quit unexpectedly."
380                + " This is usually due to a code error.\n"
381                + "  The above stacktrace can help determine where the error occurred.\n"
382                + "  See https://wpilib.org/stacktrace for more information.",
383            false);
384        if (errorOnExit) {
385          DriverStation.reportError(
386              "The startCompetition() method (or methods called by it) should have "
387                  + "handled the exception above.",
388              false);
389        } else {
390          DriverStation.reportError("Unexpected return from startCompetition() method.", false);
391        }
392      }
393    }
394  }
395
396  /**
397   * Suppress the "The robot program quit unexpectedly." message.
398   *
399   * @param value True if exit warning should be suppressed.
400   */
401  public static void suppressExitWarning(boolean value) {
402    m_runMutex.lock();
403    m_suppressExitWarning = value;
404    m_runMutex.unlock();
405  }
406
407  /**
408   * Starting point for the applications.
409   *
410   * @param <T> Robot subclass.
411   * @param robotSupplier Function that returns an instance of the robot subclass.
412   */
413  public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
414    if (!HAL.initialize(500, 0)) {
415      throw new IllegalStateException("Failed to initialize. Terminating");
416    }
417
418    // Force refresh DS data
419    DriverStation.refreshData();
420
421    // Call a CameraServer JNI function to force OpenCV native library loading
422    // Needed because all the OpenCV JNI functions don't have built in loading
423    CameraServerJNI.enumerateSinks();
424
425    HAL.report(
426        tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0, WPILibVersion.Version);
427
428    if (!Notifier.setHALThreadPriority(true, 40)) {
429      DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
430    }
431
432    if (HAL.hasMain()) {
433      Thread thread =
434          new Thread(
435              () -> {
436                runRobot(robotSupplier);
437                HAL.exitMain();
438              },
439              "robot main");
440      thread.setDaemon(true);
441      thread.start();
442      HAL.runMain();
443      suppressExitWarning(true);
444      m_runMutex.lock();
445      RobotBase robot = m_robotCopy;
446      m_runMutex.unlock();
447      if (robot != null) {
448        robot.endCompetition();
449      }
450      try {
451        thread.join(1000);
452      } catch (InterruptedException ex) {
453        Thread.currentThread().interrupt();
454      }
455    } else {
456      runRobot(robotSupplier);
457    }
458
459    HAL.shutdown();
460
461    System.exit(0);
462  }
463}