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