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