From 08a536291b009c437974b952bfdc44bfbc2c2fe4 Mon Sep 17 00:00:00 2001 From: Michael Leong <43558768+michaelleong25@users.noreply.github.com> Date: Fri, 3 Feb 2023 15:23:06 -0800 Subject: [PATCH] [examples] Improvements to Elevator Simulation Example (#4937) Co-authored-by: Abhay Shukla <105139789+aboombadev@users.noreply.github.com> Co-authored-by: Tyler Veness Co-authored-by: Ryan Blue --- .../examples/ElevatorSimulation/cpp/Robot.cpp | 40 ++++++++++++++----- .../src/main/cpp/examples/examples.json | 3 +- .../examples/elevatorsimulation/Robot.java | 38 +++++++++++++----- .../wpi/first/wpilibj/examples/examples.json | 3 +- 4 files changed, 64 insertions(+), 20 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 039a49982d..d0835eb11c 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -9,7 +9,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -27,8 +29,7 @@ #include /** - * This is a sample program to demonstrate how to use a state-space controller - * to control an arm. + * This is a sample program to demonstrate the use of elevator simulation. */ class Robot : public frc::TimedRobot { static constexpr int kMotorPort = 0; @@ -37,10 +38,19 @@ class Robot : public frc::TimedRobot { static constexpr int kJoystickPort = 0; static constexpr double kElevatorKp = 5.0; + static constexpr double kElevatorKi = 0.0; + static constexpr double kElevatorKd = 0.0; + + static constexpr units::volt_t kElevatorkS = 0.0_V; + static constexpr units::volt_t kElevatorkG = 0.0_V; + static constexpr auto kElevatorkV = 0.762_V / 1_mps; + static constexpr auto kElevatorkA = 0.762_V / 1_mps_sq; + static constexpr double kElevatorGearing = 10.0; static constexpr units::meter_t kElevatorDrumRadius = 2_in; static constexpr units::kilogram_t kCarriageMass = 4.0_kg; + static constexpr units::meter_t kSetpoint = 30_in; static constexpr units::meter_t kMinElevatorHeight = 2_in; static constexpr units::meter_t kMaxElevatorHeight = 50_in; @@ -53,7 +63,13 @@ class Robot : public frc::TimedRobot { frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); // Standard classes for controlling our elevator - frc2::PIDController m_controller{kElevatorKp, 0, 0}; + frc::TrapezoidProfile::Constraints m_constraints{2.45_mps, + 2.45_mps_sq}; + frc::ProfiledPIDController m_controller{ + kElevatorKp, kElevatorKi, kElevatorKd, m_constraints}; + + frc::ElevatorFeedforward m_feedforward{kElevatorkS, kElevatorkG, kElevatorkV, + kElevatorkA}; frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; frc::PWMSparkMax m_motor{kMotorPort}; frc::Joystick m_joystick{kJoystickPort}; @@ -111,15 +127,21 @@ class Robot : public frc::TimedRobot { void TeleopPeriodic() override { if (m_joystick.GetTrigger()) { - // Here, we run PID control like normal, with a constant setpoint of 30in. - double pidOutput = m_controller.Calculate(m_encoder.GetDistance(), - units::meter_t{30_in}.value()); - m_motor.SetVoltage(units::volt_t{pidOutput}); + // Here, we set the constant setpoint of 30in. + m_controller.SetGoal(kSetpoint); } else { - // Otherwise, we disable the motor. - m_motor.Set(0.0); + // Otherwise, we update the setpoint to 0. + m_controller.SetGoal(0.0_m); } + // With the setpoint value we run PID control like normal + double pidOutput = + m_controller.Calculate(units::meter_t{m_encoder.GetDistance()}); + units::volt_t feedforwardOutput = + m_feedforward.Calculate(m_controller.GetSetpoint().velocity); + m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput); } + // To view the Elevator in the simulator, select Network Tables -> + // SmartDashboard -> Elevator Sim void DisabledInit() override { // This just makes sure that our simulation code knows that the motor's off. diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index ae66bf773e..dab3c4b870 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -756,7 +756,8 @@ "Elevator", "State-Space", "Simulation", - "Mechanism2d" + "Mechanism2d", + "Profiled PID" ], "foldername": "ElevatorSimulation", "gradlebase": "cpp", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index 4bb6654237..2cf88fa3f7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -5,8 +5,10 @@ package edu.wpi.first.wpilibj.examples.elevatorsimulation; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Joystick; @@ -22,18 +24,27 @@ import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -/** This is a sample program to demonstrate the use of elevator simulation with existing code. */ +/** This is a sample program to demonstrate the use of elevator simulation. */ public class Robot extends TimedRobot { private static final int kMotorPort = 0; private static final int kEncoderAChannel = 0; private static final int kEncoderBChannel = 1; private static final int kJoystickPort = 0; - private static final double kElevatorKp = 5.0; + private static final double kElevatorKp = 5; + private static final double kElevatorKi = 0; + private static final double kElevatorKd = 0; + + private static final double kElevatorkS = 0.0; // volts (V) + private static final double kElevatorkG = 0.762; // volts (V) + private static final double kElevatorkV = 0.762; // volt per velocity (V/(m/s)) + private static final double kElevatorkA = 0.0; // volt per acceleration (V/(m/s²)) + private static final double kElevatorGearing = 10.0; private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0); private static final double kCarriageMass = 4.0; // kg + private static final double kSetpoint = Units.inchesToMeters(30); private static final double kMinElevatorHeight = Units.inchesToMeters(2); private static final double kMaxElevatorHeight = Units.inchesToMeters(50); @@ -42,10 +53,15 @@ public class Robot extends TimedRobot { private static final double kElevatorEncoderDistPerPulse = 2.0 * Math.PI * kElevatorDrumRadius / 4096; + // This gearbox represents a gearbox containing 4 Vex 775pro motors. private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4); // Standard classes for controlling our elevator - private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0); + private final ProfiledPIDController m_controller = + new ProfiledPIDController( + kElevatorKp, kElevatorKi, kElevatorKd, new TrapezoidProfile.Constraints(2.45, 2.45)); + ElevatorFeedforward m_feedforward = + new ElevatorFeedforward(kElevatorkS, kElevatorkG, kElevatorkV, kElevatorkA); private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort); private final Joystick m_joystick = new Joystick(kJoystickPort); @@ -103,14 +119,18 @@ public class Robot extends TimedRobot { @Override public void teleopPeriodic() { if (m_joystick.getTrigger()) { - // Here, we run PID control like normal, with a constant setpoint of 30in. - double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30)); - m_motor.setVoltage(pidOutput); + // Here, we set the constant setpoint of 30in. + m_controller.setGoal(kSetpoint); } else { - // Otherwise, we disable the motor. - m_motor.set(0.0); + // Otherwise, we update the setpoint to 0. + m_controller.setGoal(0.0); } + // With the setpoint value we run PID control like normal + double pidOutput = m_controller.calculate(m_encoder.getDistance()); + double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity); + m_motor.setVoltage(pidOutput + feedforwardOutput); } + // To view the Elevator in the simulator, select Network Tables -> SmartDashboard -> Elevator Sim @Override public void disabledInit() { 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 1be46757c1..117a831184 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 @@ -793,7 +793,8 @@ "Elevator", "State-Space", "Simulation", - "Mechanism2d" + "Mechanism2d", + "Profiled PID" ], "foldername": "elevatorsimulation", "gradlebase": "java",