Add flywheel bang-bang controller example

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

@@ -52,11 +52,13 @@
     "commandversion": 2
   },
   {
-    "name":"Mechanism2d",
-    "foldername":"Mechanism2d",
-    "gradlebase":"cpp",
-    "description":"An example usage of Mechanism2d to display mechanism states on a dashboard.",
-    "tags":["Mechanism2d"],
+    "name": "Mechanism2d",
+    "foldername": "Mechanism2d",
+    "gradlebase": "cpp",
+    "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "tags": [
+      "Mechanism2d"
+    ],
     "commandversion": 2
   },
   {
@@ -280,7 +282,6 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
-
   {
     "name": "Axis Camera Sample",
     "description": "An example program that acquires images from an Axis network camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display. This demonstrates the use of the AxisCamera class.",
@@ -773,5 +774,18 @@
     "gradlebase": "cpp",
     "mainclass": "Main",
     "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",
+      "Sensors",
+      "Joystick"
+    ],
+    "foldername": "flywheelbangbangcontroller",
+    "gradlebase": "cpp",
+    "commandversion": 2
   }
 ]

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

@@ -0,0 +1,85 @@
+// 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>
+
+/**
+ * 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 (rad/s) controlled by a joystick
+   */
+  void TeleopPeriodic() override {
+    // Scale setpoint value between 0 and maxSetpointValue
+    units::radians_per_second_t setpoint = units::math::max(
+        0_rad_per_s, m_joystick.GetRawAxis(0) * kMaxSetpointValue);
+    // Set setpoint and measurement of the bang bang controller
+    double bangOutput =
+        m_bangBangControler.Calculate(m_encoder.GetRate(), setpoint.value());
+
+    // Controls a motor with the output of the BangBang controller and a
+    // feedforward Shrinks the feedforward slightly to avoid overspeeding the
+    // shooter
+    double feedf = m_feedforward.Calculate(setpoint).value();
+    m_flywheelMotor.Set(bangOutput + 0.9 * feedf);
+  }
+
+  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(units::volt_t{m_flywheelMotor.Get()} *
+                                  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;
+
+  static constexpr units::radians_per_second_t kMaxSetpointValue{630.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!
+  frc::SimpleMotorFeedforward<units::radians> m_feedforward{
+      0.000954_V, 0.00186_V / 1_rad_per_s, 0.00286_V / 1_rad_per_s_sq};
+  frc::Joystick m_joystick{0};  // Joystick to control setpoint
+
+  // 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;
+  frc::sim::FlywheelSim m_flywheelSim{
+      frc::DCMotor::NEO(1), kFlywheelGearing,
+      0.5 * 1.5_lb * units::math::pow<2>(4_in)};  // 1/2*M*R^2
+  frc::sim::EncoderSim m_encoderSim{m_encoder};
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

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

@@ -209,9 +209,12 @@
     "mainclass": "Main",
     "commandversion": 2
   },
-  {"name": "Mechanism2d",
+  {
+    "name": "Mechanism2d",
     "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
-    "tags": ["Mechanism2d"],
+    "tags": [
+      "Mechanism2d"
+    ],
     "foldername": "mechanism2d",
     "gradlebase": "java",
     "mainclass": "Main",
@@ -793,5 +796,19 @@
     "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",
+      "Sensors",
+      "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,87 @@
+// 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.MotorController;
+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;
+
+  private final MotorController m_flywheelMotor = new PWMSparkMax(kMotorPort);
+  private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
+
+  // Create Bang Bang controler
+  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;
+  public static final double kFlywheelKv = 0.000195;
+  public static final double kFlywheelKa = 0.0003;
+  private final SimpleMotorFeedforward m_feedforward =
+      new SimpleMotorFeedforward(kFlywheelKs, kFlywheelKv, kFlywheelKa);
+
+  private final Joystick m_joystick = new Joystick(0); // Joystick to control setpoint
+  private static final double kMaxSetpointValue = 6000; // Max value for joystick control
+
+  // Simulation classes help us simulate our robot
+
+  private static final double kFlywheelMomentOfInertia =
+      0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); // 1/2*M*R^2
+
+  // 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;
+
+  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);
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Scale setpoint value between 0 and maxSetpointValue
+    double setpoint = Math.max(0, m_joystick.getRawAxis(0) * kMaxSetpointValue);
+
+    // Set setpoint and measurement of the bang bang controller
+    double bangOutput = m_bangBangControler.calculate(m_encoder.getRate(), setpoint);
+
+    // Controls a motor with the output of the BangBang controller and a feedforward
+    // Shrinks the feedforward slightly to avoid overspeeding the shooter
+    m_flywheelMotor.set(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.getAngularVelocityRPM());
+  }
+}