[examples] Add Romi reference Java example and templates (#2905)

wpilibcExamples/src/main/cpp/templates/templates.json

@@ -48,5 +48,25 @@
     "foldername": "oldcommandbased",
     "gradlebase": "cpp",
     "commandversion": 1
+  },
+  {
+    "name": "Romi - Timed Robot",
+    "description": "Romi - Timed style",
+    "tags": [
+      "Timed", "Romi"
+    ],
+    "foldername": "timed",
+    "gradlebase": "cppromi",
+    "commandversion": 2
+  },
+  {
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
+    "tags": [
+      "Command", "Romi"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "cppromi",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -731,5 +731,17 @@
     "gradlebase": "java",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "RomiReference",
+    "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+    "tags": [
+      "Drivetrain",
+      "Romi"
+    ],
+    "foldername": "romireference",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java

@@ -0,0 +1,19 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants.  This class should not be used for any other purpose.  All constants should be
+ * declared globally (i.e. public static).  Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java

@@ -0,0 +1,113 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before
+   * LiveWindow and SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /**
+   * This function is called once each time the robot enters Disabled mode.
+   */
+  @Override
+  public void disabledInit() {
+  }
+
+  @Override
+  public void disabledPeriodic() {
+  }
+
+  /**
+   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /**
+   * This function is called periodically during autonomous.
+   */
+  @Override
+  public void autonomousPeriodic() {
+  }
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /**
+   * This function is called periodically during operator control.
+   */
+  @Override
+  public void teleopPeriodic() {
+  }
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /**
+   * This function is called periodically during test mode.
+   */
+  @Override
+  public void testPeriodic() {
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java

@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.button.Button;
+
+import edu.wpi.first.wpilibj.examples.romireference.commands.TeleopArcadeDrive;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO.ChannelMode;
+
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
+ * (including subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  private final Drivetrain m_drivetrain = new Drivetrain();
+  private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
+
+  // Assumes a gamepad plugged into channnel 0
+  private final Joystick m_controller = new Joystick(0);
+
+  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
+  // that is specified when launching the wpilib-ws server on the Romi raspberry pi.
+  // By default, the following are available (listed in order from inside of the board to outside):
+  // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+  // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+  // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+  // - PWM 2 (mapped to Arduino Pin 21)
+  // - PWM 3 (mapped to Arduino Pin 22)
+  //
+  // Your subsystem configuration should take the overlays into account
+
+  /**
+   * The container for the robot.  Contains subsystems, OI devices, and commands.
+   */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings.  Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Also set default commands here
+    m_drivetrain.setDefaultCommand(new TeleopArcadeDrive(m_drivetrain,
+        () -> m_controller.getRawAxis(1),
+        () -> -m_controller.getRawAxis(2)));
+
+    // Example of how to use the onboard IO
+    Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed);
+    onboardButtonA
+      .whenActive(new PrintCommand("Button A Pressed"))
+      .whenInactive(new PrintCommand("Button A Released"));
+  }
+
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // An ExampleCommand will run in autonomous
+    return null;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TeleopArcadeDrive.java

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+
+public class TeleopArcadeDrive extends CommandBase {
+  private final Drivetrain m_drivetrain;
+  private final Supplier<Double> m_xaxisSpeedSupplier;
+  private final Supplier<Double> m_zaxisRotateSupplier;
+
+  /**
+   * Creates a new TeleopArcadeDrive.
+   */
+  public TeleopArcadeDrive(Drivetrain drivetrain, Supplier<Double> xaxisSpeedSupplier,
+      Supplier<Double> zaxisRotateSuppplier) {
+    m_drivetrain = drivetrain;
+    m_xaxisSpeedSupplier = xaxisSpeedSupplier;
+    m_zaxisRotateSupplier = zaxisRotateSuppplier;
+
+    addRequirements(drivetrain);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java

@@ -0,0 +1,72 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Drivetrain extends SubsystemBase {
+  private static final double kCountsPerRevolution = 1440.0;
+  private static final double kWheelDiameterInch = 2.75;
+
+  // The Romi has the left and right motors set to
+  // PWM channels 0 and 1 respectively
+  private final Spark m_leftMotor = new Spark(0);
+  private final Spark m_rightMotor = new Spark(1);
+
+  // The Romi has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /**
+   * Creates a new Drivetrain.
+   */
+  public Drivetrain() {
+    // DifferentialDrive defaults to having the right side flipped
+    // We don't need to do this because the Romi has accounted for this
+    // in firmware/hardware
+    m_diffDrive.setRightSideInverted(false);
+    resetEncoders();
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public int getLeftEncoderCount() {
+    return m_leftEncoder.get();
+  }
+
+  public int getRightEncoderCount() {
+    return m_rightEncoder.get();
+  }
+
+  public double getLeftDistanceInch() {
+    return Math.PI * kWheelDiameterInch * (getLeftEncoderCount() / kCountsPerRevolution);
+  }
+
+  public double getRightDistanceInch() {
+    return Math.PI * kWheelDiameterInch * (getRightEncoderCount() / kCountsPerRevolution);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java

@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.examples.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This class represents the onboard IO of the Romi
+ * reference robot. This includes the pushbuttons and
+ * LEDs.
+ *
+ * <p>DIO 0 - Button A (input only)
+ * DIO 1 - Button B (input) or Green LED (output)
+ * DIO 2 - Button C (input) or Red LED (output)
+ * DIO 3 - Yellow LED (output only)
+ */
+public class OnBoardIO extends SubsystemBase {
+  private final DigitalInput m_buttonA = new DigitalInput(0);
+  private final DigitalOutput m_yellowLed = new DigitalOutput(3);
+
+  // DIO 1
+  private DigitalInput m_buttonB;
+  private DigitalOutput m_greenLed;
+
+  // DIO 2
+  private DigitalInput m_buttonC;
+  private DigitalOutput m_redLed;
+
+  private static final double MESSAGE_INTERVAL = 1.0;
+  private double m_nextMessageTime;
+
+  public enum ChannelMode {
+    INPUT,
+    OUTPUT
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param dio1 Mode for DIO 1 (input = Button B, output = green LED)
+   * @param dio2 Mode for DIO 2 (input = Button C, output = red LED)
+   */
+  public OnBoardIO(ChannelMode dio1, ChannelMode dio2) {
+    if (dio1 == ChannelMode.INPUT) {
+      m_buttonB = new DigitalInput(1);
+    } else {
+      m_greenLed = new DigitalOutput(1);
+    }
+
+    if (dio2 == ChannelMode.INPUT) {
+      m_buttonC = new DigitalInput(2);
+    } else {
+      m_redLed = new DigitalOutput(2);
+    }
+  }
+
+  /**
+   * Gets if the A button is pressed.
+   */
+  public boolean getButtonAPressed() {
+    return m_buttonA.get();
+  }
+
+  /**
+   * Gets if the B button is pressed.
+   */
+  public boolean getButtonBPressed() {
+    if (m_buttonB != null) {
+      return m_buttonB.get();
+    }
+
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      DriverStation.reportError("Button B was not configured", true);
+      m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+    }
+    return false;
+  }
+
+  /**
+   * Gets if the C button is pressed.
+   */
+  public boolean getButtonCPressed() {
+    if (m_buttonC != null) {
+      return m_buttonC.get();
+    }
+
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      DriverStation.reportError("Button C was not configured", true);
+      m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+    }
+    return false;
+  }
+
+  /**
+   * Sets the green LED.
+   */
+  public void setGreenLed(boolean value) {
+    if (m_greenLed != null) {
+      m_greenLed.set(value);
+    } else {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        DriverStation.reportError("Green LED was not configured", true);
+        m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+      }
+    }
+  }
+
+  /**
+   * Sets the red LED.
+   */
+  public void setRedLed(boolean value) {
+    if (m_redLed != null) {
+      m_redLed.set(value);
+    } else {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        DriverStation.reportError("Red LED was not configured", true);
+        m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+      }
+    }
+  }
+
+  /**
+   * Sets the yellow LED.
+   */
+  public void setYellowLed(boolean value) {
+    m_yellowLed.set(value);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json

@@ -53,5 +53,27 @@
     "gradlebase": "java",
     "mainclass": "Main",
     "commandversion": 1
+  },
+  {
+    "name": "Romi - Timed Robot",
+    "description": "Romi - Timed style",
+    "tags": [
+      "Timed", "Romi"
+    ],
+    "foldername": "timed",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
+    "tags": [
+      "Command", "Romi"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]