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}