Allow passing DS Instance to Robot and OpModes

wpilibj/src/main/java/org/wpilib/driverstation/DefaultUserControls.java

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.driverstation;
+
+/**
+ * A default implementation of UserControls that provides Gamepad instances for
+ * each of the 6 joystick ports provided by the DS.
+ */
+public class DefaultUserControls implements UserControls {
+    private final Gamepad[] m_gamepads;
+
+    /**
+     * Constructs a DefaultUserControls instance with Gamepads for each port.
+     */
+    public DefaultUserControls() {
+        m_gamepads = new Gamepad[DriverStation.kJoystickPorts];
+        for (int i = 0; i < m_gamepads.length; i++) {
+            m_gamepads[i] = new Gamepad(i);
+        }
+    }
+
+    /**
+     * Returns the Gamepad instance for the specified port.
+     *
+     * @param port The joystick port number.
+     * @return The Gamepad instance for the given port.
+     */
+    public Gamepad getGamepad(int port) {
+        return m_gamepads[port];
+    }
+}

wpilibj/src/main/java/org/wpilib/driverstation/UserControls.java

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.driverstation;
+
+/**
+ * An interface representing user controls such as gamepads or joysticks.
+ * If your main robot class has a UserControlsInstance attribute with a
+ * class implementing this interface, the constructor is able to receive
+ * an instance of that class. Additionally, any OpModes can also receive
+ * that same instance.
+ */
+public interface UserControls {
+}

wpilibj/src/main/java/org/wpilib/driverstation/UserControlsInstance.java

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.driverstation;
+
+import java.lang.annotation.ElementType;
+import java.lang.annotation.Retention;
+import java.lang.annotation.RetentionPolicy;
+import java.lang.annotation.Target;
+
+/**
+ * An annotation to specify the UserControls implementation class to be used
+ * for a robot. Apply this annotation to your main robot class, providing
+ * a class that implements the UserControls interface.
+ */
+@Target(ElementType.TYPE)
+@Retention(RetentionPolicy.RUNTIME)
+public @interface UserControlsInstance {
+    /**
+     * The UserControls implementation class to be used.
+     *
+     * @return The class that implements UserControls.
+     */
+    Class<? extends UserControls> value();
+}

wpilibj/src/main/java/org/wpilib/framework/OpModeRobot.java

@@ -12,11 +12,14 @@
 import java.util.Enumeration;
 import java.util.HashMap;
 import java.util.Map;
+import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
 import java.util.function.Supplier;
 import java.util.jar.JarEntry;
 import java.util.jar.JarFile;
 import org.wpilib.driverstation.DriverStation;
+import org.wpilib.driverstation.UserControls;
+import org.wpilib.driverstation.UserControlsInstance;
 import org.wpilib.hardware.hal.ControlWord;
 import org.wpilib.hardware.hal.DriverStationJNI;
 import org.wpilib.hardware.hal.HAL;
@@ -56,12 +59,120 @@ private static void reportAddOpModeError(Class<?> cls, String message) {
     DriverStation.reportError("Error adding OpMode " + cls.getSimpleName() + ": " + message, false);
   }
 
-  /**
-   * Find a public constructor to instantiate the opmode. Prefer a single-arg public constructor
-   * whose parameter type is assignable from this.getClass() (if multiple, pick the most specific
-   * parameter type). Otherwise return the public no-arg constructor. Return null if neither exists.
-   */
-  private Constructor<?> findOpModeConstructor(Class<?> cls) {
+  private final Optional<Class<? extends UserControls>> m_userControlsBaseClass;
+  private UserControls m_userControlsInstance;
+
+  void setUserControlsInstance(UserControls userControlsInstance) {
+    if (m_userControlsBaseClass.isEmpty()) {
+      throw new IllegalStateException("No UserControls class specified");
+    }
+
+    if (!m_userControlsBaseClass.get().isAssignableFrom(userControlsInstance.getClass())) {
+      throw new IllegalArgumentException(userControlsInstance.getClass().getSimpleName()
+          + " is not assignable to "
+          + m_userControlsBaseClass.get().getSimpleName());
+    }
+    m_userControlsInstance = userControlsInstance;
+  }
+
+  private static class ConstructorMatch {
+    final Constructor<?> m_constructor;
+    final Optional<Class<?>> m_firstParam;
+    final Optional<Class<?>> m_secondParam;
+
+    ConstructorMatch(Constructor<?> constructor, Optional<Class<?>> firstParam,
+          Optional<Class<?>> secondParam) {
+      m_constructor = constructor;
+      m_firstParam = firstParam;
+      m_secondParam = secondParam;
+    }
+
+    public Object newInstance(OpModeRobot robot, UserControls userControls)
+        throws ReflectiveOperationException {
+      Object[] args = null;
+      if (m_firstParam.isPresent() && m_secondParam.isPresent()) {
+        // 2 constructor parameters
+        args = new Object[2];
+
+        if (m_firstParam.get().isAssignableFrom(robot.getClass())) {
+          // RobotBase parameter
+          args[0] = robot;
+        } else if (m_firstParam.get().isAssignableFrom(userControls.getClass())) {
+          // UserControls parameter
+          args[0] = userControls;
+        } else {
+          throw new IllegalStateException("First constructor parameter type not recognized");
+        }
+
+        if (m_secondParam.get().isAssignableFrom(robot.getClass())) {
+          // RobotBase parameter
+          args[1] = robot;
+        } else if (m_secondParam.get().isAssignableFrom(userControls.getClass())) {
+          // UserControls parameter
+          args[1] = userControls;
+        } else {
+          throw new IllegalStateException("Second constructor parameter type not recognized");
+        }
+      } else if (m_firstParam.isPresent()) {
+        // We have only 1 parameter, find it
+        args = new Object[1];
+        if (m_firstParam.get().isAssignableFrom(robot.getClass())) {
+          // RobotBase parameter
+          args[0] = robot;
+        } else if (m_firstParam.get().isAssignableFrom(userControls.getClass())) {
+          // UserControls parameter
+          args[0] = userControls;
+        } else {
+          throw new IllegalStateException("Constructor parameter type not recognized");
+        }
+      }
+      return m_constructor.newInstance(args);
+    }
+  }
+
+  private Optional<ConstructorMatch> find2ParameterConstructor(Class<?> cls) {
+    Constructor<?> bestCtor = null;
+    Class<?> bestFirstParam = null;
+    Class<?> bestSecondParam = null;
+    for (Constructor<?> ctor : cls.getConstructors()) {
+      Class<?>[] params = ctor.getParameterTypes();
+      if (params.length != 2) {
+        continue;
+      }
+      Class<?> firstParam = params[0];
+      Class<?> secondParam = params[1];
+      if (!firstParam.isAssignableFrom(getClass())) {
+        continue;
+      }
+      if (m_userControlsBaseClass.isEmpty()
+          || !m_userControlsBaseClass.get().isAssignableFrom(secondParam)) {
+        continue;
+      }
+      boolean isBetter = false;
+      if (bestCtor == null) {
+        isBetter = true;
+      } else if (bestFirstParam.isAssignableFrom(firstParam)) {
+        isBetter = true;
+      } else if (bestFirstParam.equals(firstParam)
+          && bestSecondParam.isAssignableFrom(secondParam)) {
+        isBetter = true;
+      }
+      if (isBetter) {
+        bestCtor = ctor;
+        bestFirstParam = firstParam;
+        bestSecondParam = secondParam;
+      }
+    }
+    if (bestCtor != null) {
+      return Optional.of(new ConstructorMatch(
+          bestCtor,
+          Optional.of(bestFirstParam),
+          Optional.of(bestSecondParam)));
+    }
+    return Optional.empty();
+  }
+
+  private Optional<ConstructorMatch> find1ParameterRobotConstructor(Class<?> cls) {
     Constructor<?> bestCtor = null;
     Class<?> bestParam = null;
     for (Constructor<?> ctor : cls.getConstructors()) {
@@ -79,28 +190,92 @@ private Constructor<?> findOpModeConstructor(Class<?> cls) {
       }
     }
     if (bestCtor != null) {
-      return bestCtor;
+      return Optional.of(new ConstructorMatch(bestCtor, Optional.of(bestParam), Optional.empty()));
     }
+    return Optional.empty();
+  }
+
+  private Optional<ConstructorMatch> find1ParameterUserControlsConstructor(Class<?> cls) {
+    if (m_userControlsBaseClass.isEmpty()) {
+      return Optional.empty();
+    }
+    Constructor<?> bestCtor = null;
+    Class<?> bestParam = null;
+    for (Constructor<?> ctor : cls.getConstructors()) {
+      Class<?>[] params = ctor.getParameterTypes();
+      if (params.length != 1) {
+        continue;
+      }
+      Class<?> param = params[0];
+      if (!m_userControlsBaseClass.get().isAssignableFrom(param)) {
+        continue;
+      }
+      if (bestCtor == null || bestParam.isAssignableFrom(param)) {
+        bestCtor = ctor;
+        bestParam = param;
+      }
+    }
+    if (bestCtor != null) {
+      return Optional.of(new ConstructorMatch(bestCtor, Optional.of(bestParam), Optional.empty()));
+    }
+    return Optional.empty();
+  }
+
+  private Optional<ConstructorMatch> findNoParameterConstructor(Class<?> cls) {
     try {
-      return cls.getConstructor();
+      Constructor<?> ctor = cls.getConstructor();
+      return Optional.of(new ConstructorMatch(ctor, Optional.empty(), Optional.empty()));
     } catch (NoSuchMethodException e) {
-      return null;
+      return Optional.empty();
     }
   }
 
-  private OpMode constructOpModeClass(Class<?> cls) {
-    Constructor<?> constructor = findOpModeConstructor(cls);
-    if (constructor == null) {
+  /**
+   * Find a public constructor to instantiate the opmode. This constructor can
+   * have up to 2 parameters. The first parameter (if present) must be assignable
+   * from this.getClass(). The second parameter (if present) must be assignable
+   * from DriverStationBase. If multiple, first sort by most parameters, then by
+   * most specific first, then by most specific second.
+   */
+  private Optional<ConstructorMatch> findOpModeConstructor(Class<?> cls) {
+    Optional<ConstructorMatch> ctor;
+
+    // try 2-parameter constructor
+    ctor = find2ParameterConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with RobotBase parameter
+    ctor = find1ParameterRobotConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try 1-parameter constructor with UserControls parameter
+    ctor = find1ParameterUserControlsConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    // try no-parameter constructor
+    ctor = findNoParameterConstructor(cls);
+    if (ctor.isPresent()) {
+      return ctor;
+    }
+
+    return Optional.empty();
+  }
+
+  private OpMode constructOpModeClass(Class<? extends OpMode> cls) {
+    Optional<ConstructorMatch> constructor = findOpModeConstructor(cls);
+    if (constructor.isEmpty()) {
       DriverStation.reportError(
           "No suitable constructor to instantiate OpMode " + cls.getSimpleName(), true);
       return null;
     }
     try {
-      if (constructor.getParameterCount() == 1) {
-        return (OpMode) constructor.newInstance(this);
-      } else {
-        return (OpMode) constructor.newInstance();
-      }
+      return cls.cast(constructor.get().newInstance(this, m_userControlsInstance));
     } catch (ReflectiveOperationException e) {
       DriverStation.reportError(
           "Could not instantiate OpMode " + cls.getSimpleName(), e.getStackTrace());
@@ -288,7 +463,7 @@ public void addOpMode(Class<? extends OpMode> cls, RobotMode mode, String name)
   }
 
   private void addOpModeClassImpl(
-      Class<?> cls,
+      Class<? extends OpMode> cls,
       RobotMode mode,
       String name,
       String group,
@@ -305,7 +480,7 @@ private void addOpModeClassImpl(
   }
 
   private void addAnnotatedOpModeImpl(
-      Class<?> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
+      Class<? extends OpMode> cls, Autonomous auto, Teleop teleop, TestOpMode test) {
     checkOpModeClass(cls);
 
     // add an opmode for each annotation
@@ -364,10 +539,10 @@ public void addAnnotatedOpMode(Class<? extends OpMode> cls) {
   private void addAnnotatedOpModeClass(String name) {
     // trim ".class" from end
     String className = name.replace('/', '.').substring(0, name.length() - 6);
-    Class<?> cls;
+    Class<? extends OpMode> cls;
     try {
-      cls = Class.forName(className);
-    } catch (ClassNotFoundException e) {
+      cls = Class.forName(className).asSubclass(OpMode.class);
+    } catch (ClassNotFoundException | ClassCastException e) {
       return;
     }
     Autonomous auto = cls.getAnnotation(Autonomous.class);
@@ -468,6 +643,14 @@ public void clearOpModes() {
   /** Constructor. */
   @SuppressWarnings("this-escape")
   public OpModeRobot() {
+    // Check to see if we have a DS annotation
+    UserControlsInstance userControlsAnnotation =
+        getClass().getAnnotation(UserControlsInstance.class);
+    if (userControlsAnnotation != null) {
+      m_userControlsBaseClass = Optional.of(userControlsAnnotation.value());
+    } else {
+      m_userControlsBaseClass = Optional.empty();
+    }
     // Scan for annotated opmode classes within the derived class's package and subpackages
     addAnnotatedOpModeClasses(getClass().getPackage());
     DriverStation.publishOpModes();