[examples] Add flywheel bang-bang controller example (#4071)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp

@@ -0,0 +1,104 @@
+// 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.
+
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/controller/BangBangController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/simulation/FlywheelSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/moment_of_inertia.h>
+
+/**
+ * This is a sample program to demonstrate the use of a BangBangController with
+ * a flywheel to control speed.
+ */
+class Robot : public frc::TimedRobot {
+ public:
+  /**
+   * Controls flywheel to a set speed (RPM) controlled by a joystick.
+   */
+  void TeleopPeriodic() override {
+    // Scale setpoint value between 0 and maxSetpointValue
+    units::radians_per_second_t setpoint =
+        units::math::max(0_rpm, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
+
+    // Set setpoint and measurement of the bang-bang controller
+    units::volt_t bangOutput =
+        m_bangBangControler.Calculate(m_encoder.GetRate(), setpoint.value()) *
+        12_V;
+
+    // Controls a motor with the output of the BangBang controller and a
+    // feedforward. The feedforward is reduced slightly to avoid overspeeding
+    // the shooter.
+    m_flywheelMotor.SetVoltage(bangOutput +
+                               0.9 * m_feedforward.Calculate(setpoint));
+  }
+
+  void RobotInit() override {
+    // Add bang-bang controler to SmartDashboard and networktables.
+    frc::SmartDashboard::PutData("BangBangControler", &m_bangBangControler);
+  }
+
+  /**
+   * Update our simulation. This should be run every robot loop in simulation.
+   */
+  void SimulationPeriodic() override {
+    // To update our simulation, we set motor voltage inputs, update the
+    // simulation, and write the simulated velocities to our simulated encoder
+    m_flywheelSim.SetInputVoltage(
+        m_flywheelMotor.Get() *
+        units::volt_t{frc::RobotController::GetInputVoltage()});
+    m_flywheelSim.Update(0.02_s);
+    m_encoderSim.SetRate(m_flywheelSim.GetAngularVelocity().value());
+  }
+
+ private:
+  static constexpr int kMotorPort = 0;
+  static constexpr int kEncoderAChannel = 0;
+  static constexpr int kEncoderBChannel = 1;
+
+  // Max setpoint for joystick control
+  static constexpr units::radians_per_second_t kMaxSetpointValue = 6000_rpm;
+
+  // Joystick to control setpoint
+  frc::Joystick m_joystick{0};
+
+  frc::PWMSparkMax m_flywheelMotor{kMotorPort};
+  frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
+
+  frc::BangBangController m_bangBangControler;
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  static constexpr units::volt_t kFlywheelKs = 0.0001_V;
+  static constexpr decltype(1_V / 1_rad_per_s) kFlywheelKv = 0.000195_V / 1_rpm;
+  static constexpr decltype(1_V / 1_rad_per_s_sq) kFlywheelKa =
+      0.0003_V / 1_rev_per_m_per_s;
+  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
+      kFlywheelKs, kFlywheelKv, kFlywheelKa};
+
+  // Simulation classes help us simulate our robot
+
+  // Reduction between motors and encoder, as output over input. If the flywheel
+  // spins slower than the motors, this number should be greater than one.
+  static constexpr double kFlywheelGearing = 1.0;
+
+  // 1/2 MR²
+  static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia =
+      0.5 * 1.5_lb * 4_in * 4_in;
+
+  frc::sim::FlywheelSim m_flywheelSim{frc::DCMotor::NEO(1), kFlywheelGearing,
+                                      kFlywheelMomentOfInertia};
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -865,5 +865,17 @@
     "foldername": "SwerveDrivePoseEstimator",
     "gradlebase": "cpp",
     "commandversion": 2
+  },
+  {
+    "name": "Flywheel BangBangController",
+    "description": "A sample program to demonstrate the use of a BangBangController with a flywheel to control RPM",
+    "tags": [
+      "Flywheel",
+      "Simulation",
+      "Joystick"
+    ],
+    "foldername": "FlywheelBangBangController",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]

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

@@ -916,5 +916,18 @@
     "gradlebase": "java",
     "commandversion": 2,
     "mainclass": "Main"
+  },
+  {
+    "name": "Flywheel BangBangController",
+    "description": "A sample program to demonstrate the use of a BangBangController with a flywheel to control RPM",
+    "tags": [
+      "Flywheel",
+      "Simulation",
+      "Joystick"
+    ],
+    "foldername": "flywheelbangbangcontroller",
+    "gradlebase": "java",
+    "commandversion": 2,
+    "mainclass": "Main"
   }
 ]

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

@@ -0,0 +1,25 @@
+// 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 edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
+
+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/flywheelbangbangcontroller/Robot.java

@@ -0,0 +1,95 @@
+// 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 edu.wpi.first.wpilibj.examples.flywheelbangbangcontroller;
+
+import edu.wpi.first.math.controller.BangBangController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.FlywheelSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This is a sample program to demonstrate the use of a BangBangController with a flywheel to
+ * control RPM.
+ */
+public class Robot extends TimedRobot {
+  private static final int kMotorPort = 0;
+  private static final int kEncoderAChannel = 0;
+  private static final int kEncoderBChannel = 1;
+
+  // Max setpoint for joystick control in RPM
+  private static final double kMaxSetpointValue = 6000.0;
+
+  // Joystick to control setpoint
+  private final Joystick m_joystick = new Joystick(0);
+
+  private final PWMSparkMax m_flywheelMotor = new PWMSparkMax(kMotorPort);
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  private final BangBangController m_bangBangControler = new BangBangController();
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  public static final double kFlywheelKs = 0.0001; // V
+  public static final double kFlywheelKv = 0.000195; // V/RPM
+  public static final double kFlywheelKa = 0.0003; // V/(RPM/s)
+  private final SimpleMotorFeedforward m_feedforward =
+      new SimpleMotorFeedforward(kFlywheelKs, kFlywheelKv, kFlywheelKa);
+
+  // Simulation classes help us simulate our robot
+
+  // Reduction between motors and encoder, as output over input. If the flywheel
+  // spins slower than the motors, this number should be greater than one.
+  private static final double kFlywheelGearing = 1.0;
+
+  // 1/2 MR²
+  private static final double kFlywheelMomentOfInertia =
+      0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2);
+
+  private final FlywheelSim m_flywheelSim =
+      new FlywheelSim(DCMotor.getNEO(1), kFlywheelGearing, kFlywheelMomentOfInertia);
+  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
+
+  @Override
+  public void robotInit() {
+    // Add bang-bang controler to SmartDashboard and networktables.
+    SmartDashboard.putData(m_bangBangControler);
+  }
+
+  /** Controls flywheel to a set speed (RPM) controlled by a joystick. */
+  @Override
+  public void teleopPeriodic() {
+    // Scale setpoint value between 0 and maxSetpointValue
+    double setpoint =
+        Math.max(
+            0.0,
+            m_joystick.getRawAxis(0)
+                * Units.rotationsPerMinuteToRadiansPerSecond(kMaxSetpointValue));
+
+    // Set setpoint and measurement of the bang-bang controller
+    double bangOutput = m_bangBangControler.calculate(m_encoder.getRate(), setpoint) * 12.0;
+
+    // Controls a motor with the output of the BangBang controller and a
+    // feedforward. The feedforward is reduced slightly to avoid overspeeding
+    // the shooter.
+    m_flywheelMotor.setVoltage(bangOutput + 0.9 * m_feedforward.calculate(setpoint));
+  }
+
+  /** Update our simulation. This should be run every robot loop in simulation. */
+  @Override
+  public void simulationPeriodic() {
+    // To update our simulation, we set motor voltage inputs, update the
+    // simulation, and write the simulated velocities to our simulated encoder
+    m_flywheelSim.setInputVoltage(m_flywheelMotor.get() * RobotController.getInputVoltage());
+    m_flywheelSim.update(0.02);
+    m_encoderSim.setRate(m_flywheelSim.getAngularVelocityRadPerSec());
+  }
+}