From e2c190487b25832df04567e8cb199b63a8270645 Mon Sep 17 00:00:00 2001 From: m10653 Date: Sat, 5 Aug 2023 17:15:05 -0400 Subject: [PATCH] [examples] Add flywheel bang-bang controller example (#4071) Co-authored-by: Tyler Veness --- .../FlywheelBangBangController/cpp/Robot.cpp | 104 ++++++++++++++++++ .../src/main/cpp/examples/examples.json | 12 ++ .../wpi/first/wpilibj/examples/examples.json | 13 +++ .../flywheelbangbangcontroller/Main.java | 25 +++++ .../flywheelbangbangcontroller/Robot.java | 95 ++++++++++++++++ 5 files changed, 249 insertions(+) create mode 100644 wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Main.java create mode 100644 wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java diff --git a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp new file mode 100644 index 00000000000..86807e60fe7 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * 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 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(); +} +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index a472716d115..2cc151958ea 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/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 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index a04ec9902a1..d74ef838c79 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/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" } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Main.java new file mode 100644 index 00000000000..d114f8ba57a --- /dev/null +++ b/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. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/flywheelbangbangcontroller/Robot.java new file mode 100644 index 00000000000..a025b6ad65c --- /dev/null +++ b/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()); + } +}