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.isSimulation(); 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 (!isSimulation()) { 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 getRuntimeType() == RuntimeType.kSimulation; 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, elements); 325 DriverStation.reportError( 326 "The robot program quit unexpectedly." 327 + " This is usually due to a code error.\n" 328 + " The above stacktrace can help determine where the error occurred.\n" 329 + " See https://wpilib.org/stacktrace for more information.\n", 330 false); 331 DriverStation.reportError("Could not instantiate robot " + robotName + "!", false); 332 return; 333 } 334 335 m_runMutex.lock(); 336 m_robotCopy = robot; 337 m_runMutex.unlock(); 338 339 if (!isSimulation()) { 340 final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini"); 341 try { 342 if (file.exists() && !file.delete()) { 343 throw new IOException("Failed to delete FRC_Lib_Version.ini"); 344 } 345 346 if (!file.createNewFile()) { 347 throw new IOException("Failed to create new FRC_Lib_Version.ini"); 348 } 349 350 try (OutputStream output = Files.newOutputStream(file.toPath())) { 351 output.write("Java ".getBytes(StandardCharsets.UTF_8)); 352 output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8)); 353 } 354 } catch (IOException ex) { 355 DriverStation.reportError("Could not write FRC_Lib_Version.ini: " + ex, ex.getStackTrace()); 356 } 357 } 358 359 boolean errorOnExit = false; 360 try { 361 robot.startCompetition(); 362 } catch (Throwable throwable) { 363 Throwable cause = throwable.getCause(); 364 if (cause != null) { 365 throwable = cause; 366 } 367 DriverStation.reportError("Unhandled exception: " + throwable, throwable.getStackTrace()); 368 errorOnExit = true; 369 } finally { 370 m_runMutex.lock(); 371 boolean suppressExitWarning = m_suppressExitWarning; 372 m_runMutex.unlock(); 373 if (!suppressExitWarning) { 374 // startCompetition never returns unless exception occurs.... 375 DriverStation.reportWarning( 376 "The robot program quit unexpectedly." 377 + " This is usually due to a code error.\n" 378 + " The above stacktrace can help determine where the error occurred.\n" 379 + " See https://wpilib.org/stacktrace for more information.", 380 false); 381 if (errorOnExit) { 382 DriverStation.reportError( 383 "The startCompetition() method (or methods called by it) should have " 384 + "handled the exception above.", 385 false); 386 } else { 387 DriverStation.reportError("Unexpected return from startCompetition() method.", false); 388 } 389 } 390 } 391 } 392 393 /** 394 * Suppress the "The robot program quit unexpectedly." message. 395 * 396 * @param value True if exit warning should be suppressed. 397 */ 398 public static void suppressExitWarning(boolean value) { 399 m_runMutex.lock(); 400 m_suppressExitWarning = value; 401 m_runMutex.unlock(); 402 } 403 404 /** 405 * Starting point for the applications. 406 * 407 * @param <T> Robot subclass. 408 * @param robotSupplier Function that returns an instance of the robot subclass. 409 */ 410 public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) { 411 if (!HAL.initialize(500, 0)) { 412 throw new IllegalStateException("Failed to initialize. Terminating"); 413 } 414 415 // Force refresh DS data 416 DriverStation.refreshData(); 417 418 // Call a CameraServer JNI function to force OpenCV native library loading 419 // Needed because all the OpenCV JNI functions don't have built in loading 420 CameraServerJNI.enumerateSinks(); 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 HAL.shutdown(); 457 458 System.exit(0); 459 } 460}