diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp index 518114a0a2..51fc29ed6e 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -14,6 +15,7 @@ #include #include #include +#include class Robot : public frc::TimedRobot { public: @@ -31,21 +33,34 @@ class Robot : public frc::TimedRobot { } // Run controller and update motor output - m_motor.Set( - m_controller.Calculate(units::meter_t{m_encoder.GetDistance()})); + m_motor.SetVoltage( + units::volt_t{ + m_controller.Calculate(units::meter_t{m_encoder.GetDistance()})} + + m_feedforward.Calculate(m_controller.GetSetpoint().velocity)); } private: + static constexpr units::meters_per_second_t kMaxVelocity = 1.75_mps; + static constexpr units::meters_per_second_squared_t kMaxAcceleration = + 0.75_mps_sq; + static constexpr double kP = 1.3; + static constexpr double kI = 0.0; + static constexpr double kD = 0.7; + static constexpr units::volt_t kS = 1.1_V; + static constexpr units::volt_t kG = 1.2_V; + static constexpr auto kV = 1.3_V / 1_mps; + frc::Joystick m_joystick{1}; frc::Encoder m_encoder{1, 2}; frc::PWMSparkMax m_motor{1}; // Create a PID controller whose setpoint's change is subject to maximum // velocity and acceleration constraints. - frc::TrapezoidProfile::Constraints m_constraints{1.75_mps, - 0.75_mps_sq}; - frc::ProfiledPIDController m_controller{1.3, 0.0, 0.7, + frc::TrapezoidProfile::Constraints m_constraints{ + kMaxVelocity, kMaxAcceleration}; + frc::ProfiledPIDController m_controller{kP, kI, kD, m_constraints, kDt}; + frc::ElevatorFeedforward m_feedforward{kS, kG, kV}; }; #ifndef RUNNING_FRC_TESTS diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java index 15d5e2314f..b6228c9c9e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.examples.elevatorprofiledpid; +import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.Encoder; @@ -12,8 +13,17 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +@SuppressWarnings("PMD.RedundantFieldInitializer") public class Robot extends TimedRobot { private static double kDt = 0.02; + private static double kMaxVelocity = 1.75; + private static double kMaxAcceleration = 0.75; + private static double kP = 1.3; + private static double kI = 0.0; + private static double kD = 0.7; + private static double kS = 1.1; + private static double kG = 1.2; + private static double kV = 1.3; private final Joystick m_joystick = new Joystick(1); private final Encoder m_encoder = new Encoder(1, 2); @@ -22,9 +32,10 @@ public class Robot extends TimedRobot { // Create a PID controller whose setpoint's change is subject to maximum // velocity and acceleration constraints. private final TrapezoidProfile.Constraints m_constraints = - new TrapezoidProfile.Constraints(1.75, 0.75); + new TrapezoidProfile.Constraints(kMaxVelocity, kMaxAcceleration); private final ProfiledPIDController m_controller = - new ProfiledPIDController(1.3, 0.0, 0.7, m_constraints, kDt); + new ProfiledPIDController(kP, kI, kD, m_constraints, kDt); + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward(kS, kG, kV); @Override public void robotInit() { @@ -40,6 +51,8 @@ public class Robot extends TimedRobot { } // Run controller and update motor output - m_motor.set(m_controller.calculate(m_encoder.getDistance())); + m_motor.setVoltage( + m_controller.calculate(m_encoder.getDistance()) + + m_feedforward.calculate(m_controller.getSetpoint().velocity)); } }