diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp new file mode 100644 index 0000000000..2c3c0dc812 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include + +class Robot : public frc::TimedRobot { + public: + static constexpr units::second_t kDt = 20_ms; + + Robot() { m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); } + + void TeleopPeriodic() override { + if (m_joystick.GetRawButtonPressed(2)) { + m_goal = {5_m, 0_mps}; + } else if (m_joystick.GetRawButtonPressed(3)) { + m_goal = {0_m, 0_mps}; + } + + // Create a motion profile with the given maximum velocity and maximum + // acceleration constraints for the next setpoint, the desired goal, and the + // current setpoint. + frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint}; + + // Retrieve the profiled setpoint for the next timestep. This setpoint moves + // toward the goal while obeying the constraints. + m_setpoint = profile.Calculate(kDt); + + // Run controller with profiled setpoint and update motor output + double output = m_controller.Calculate(m_encoder.GetDistance(), + m_setpoint.position.to()); + m_motor.Set(output); + } + + private: + frc::Joystick m_joystick{1}; + frc::Encoder m_encoder{1, 2}; + frc::Spark m_motor{1}; + frc2::PIDController m_controller{1.3, 0.0, 0.7, kDt.to()}; + + frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, 0.75_mps_sq}; + frc::TrapezoidProfile::State m_goal; + frc::TrapezoidProfile::State m_setpoint; +}; + +#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 88591346d9..28c0806901 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -166,6 +166,18 @@ "foldername": "PotentiometerPID", "gradlebase": "cpp" }, + { + "name": "Elevator with trapezoid profiled PID", + "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.", + "tags": [ + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "ElevatorTrapezoidProfile", + "gradlebase": "cpp" + }, { "name": "Getting Started", "description": "An example program which demonstrates the simplest autonomous and teleoperated routines.", diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java new file mode 100644 index 0000000000..12255cdfb5 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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.elevatortrapezoidprofile; + +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/elevatortrapezoidprofile/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java new file mode 100644 index 0000000000..fddd4176ed --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 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.elevatortrapezoidprofile; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.Spark; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.PIDController; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; + +public class Robot extends TimedRobot { + private static double kDt = 0.02; + + private final Joystick m_joystick = new Joystick(1); + private final Encoder m_encoder = new Encoder(1, 2); + private final Spark m_motor = new Spark(1); + private final PIDController m_controller = new PIDController(1.3, 0.0, 0.7, kDt); + + private final TrapezoidProfile.Constraints m_constraints = + new TrapezoidProfile.Constraints(1.75, 0.75); + private TrapezoidProfile.State m_goal = new TrapezoidProfile.State(); + private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State(); + + @Override + public void robotInit() { + m_encoder.setDistancePerPulse(1.0 / 360.0 * 2.0 * 3.1415 * 1.5); + } + + @Override + public void teleopPeriodic() { + if (m_joystick.getRawButtonPressed(2)) { + m_goal = new TrapezoidProfile.State(5, 0); + } else if (m_joystick.getRawButtonPressed(3)) { + m_goal = new TrapezoidProfile.State(0, 0); + } + + // Create a motion profile with the given maximum velocity and maximum + // acceleration constraints for the next setpoint, the desired goal, and the + // current setpoint. + var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint); + + // Retrieve the profiled setpoint for the next timestep. This setpoint moves + // toward the goal while obeying the constraints. + m_setpoint = profile.calculate(kDt); + + double output = m_controller.calculate(m_encoder.getDistance(), + m_setpoint.position); + + // Run controller with profiled setpoint and update motor output + m_motor.set(output); + } +} 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 90dac6c058..32a4bda9eb 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 @@ -131,6 +131,19 @@ "gradlebase": "java", "mainclass": "Main" }, + { + "name": "Elevator with trapezoid profiled PID", + "description": "An example to demonstrate the use of an encoder and trapezoid profiled PID control to reach elevator position setpoints.", + "tags": [ + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "elevatortrapezoidprofile", + "gradlebase": "java", + "mainclass": "Main" + }, { "name": "Gyro", "description": "An example program showing how to drive straight with using a gyro sensor.",