WPILib中的梯形运动轮廓

备注

本文介绍了梯形运动剖面的代码生成。文档将更详细地描述所涉及的概念。

备注

有关在基于命令的框架:ref:command-based framework <docs/software/commandbased/what-is-command-based:What Is “Command-Based” Programming?> , 请参阅 通过TrapezoidProfileSubsystems和TrapezoidProfileCommands进行运动分析.

备注

单独使用的“ TrapezoidProfile”类在与自定义控制器(例如具有内置PID功能的“智能”电机控制器)组合使用时最有用。要将其与WPILib的PIDController集成,请参阅 将运动分析和PID控制与ProfiledPIDController相结合.

尽管前馈和反馈控制提供了实现给定设定值的便捷方法,但我们仍然经常面临为我们的机构生成设定值的问题。尽管天真地立即将某种机制控制到所需状态的方法可能会起作用,但它通常不是最佳的。为了改善对机制的处理,我们通常希望命令机制达到一定的“设定值”序列,以便在其当前状态和所需目标状态之间平滑地内插。

To help users do this, WPILib provides a TrapezoidProfile class (Java, C++, Python).

创建梯形轮廓

备注

在C ++中,“ TrapezoidProfile”类在用于距离测量的单位类型(可能是角度的或线性的)上模板化。传入的值*必须*具有与距离单位一致的单位,否则将引发编译时错误。有关C ++单元的更多信息,请参阅:docs / software / basic-programming / cpp-units:C ++单元库。

约束条件

备注

各种前馈帮助类 feedforward helper classes 提供了用于计算最大同时可达到的速度和机构加速度的方法。这些对于为您的``TrapezoidProfile’’计算适当的运动约束非常有用。

In order to create a trapezoidal motion profile, we must first impose some constraints on the desired motion. Namely, we must specify a maximum velocity and acceleration that the mechanism will be expected to achieve during the motion. To do this, we create an instance of the TrapezoidProfile.Constraints class (Java, C++, Python):

// Creates a new set of trapezoidal motion profile constraints
// Max velocity of 10 meters per second
// Max acceleration of 20 meters per second squared
new TrapezoidProfile.Constraints(10, 20);
// Creates a new set of trapezoidal motion profile constraints
// Max velocity of 10 meters per second
// Max acceleration of 20 meters per second squared
frc::TrapezoidProfile<units::meters>::Constraints{10_mps, 20_mps_sq};
from wpimath.trajectory import TrapezoidProfile

# Creates a new set of trapezoidal motion profile constraints
# Max velocity of 10 meters per second
# Max acceleration of 20 meters per second squared
TrapezoidProfile.Constraints(10, 20)

起始和结束状态

Next, we must specify the desired starting and ending states for our mechanisms using the TrapezoidProfile.State class (Java, C++, Python). Each state has a position and a velocity:

// Creates a new state with a position of 5 meters
// and a velocity of 0 meters per second
new TrapezoidProfile.State(5, 0);
// Creates a new state with a position of 5 meters
// and a velocity of 0 meters per second
frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps};
from wpimath.trajectory import TrapezoidProfile

# Creates a new state with a position of 5 meters
# and a velocity of 0 meters per second
TrapezoidProfile.State(5, 0)

汇总

备注

C ++通常能够推断内部类的类型,因此可以将简单的初始化列表(没有类名)作为参数发送。为了清楚起见,完整的类名包含在下面的示例中。

Now that we know how to create a set of constraints and the desired start/end states, we are ready to create our motion profile. The TrapezoidProfile constructor takes 1 parameter: the constraints.

// Creates a new TrapezoidProfile
// Profile will have a max vel of 5 meters per second
// Profile will have a max acceleration of 10 meters per second squared
TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5, 10));
// Creates a new TrapezoidProfile
// Profile will have a max vel of 5 meters per second
// Profile will have a max acceleration of 10 meters per second squared
frc::TrapezoidProfile<units::meters> profile{
  frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq}};
from wpimath.trajectory import TrapezoidProfile

# Creates a new TrapezoidProfile
# Profile will have a max vel of 5 meters per second
# Profile will have a max acceleration of 10 meters per second squared
profile = TrapezoidProfile(TrapezoidProfile.Constraints(5, 10))

使用``TrapezoidProfile``

采样配置文件

Once we’ve created a TrapezoidProfile, using it is very simple: to get the profile state at the given time after the profile has started, call the calculate() method with the goal state and initial state:

// Profile will start stationary at zero position
// Profile will end stationary at 5 meters
// Returns the motion profile state after 5 seconds of motion
profile.calculate(5, new TrapezoidProfile.State(0, 0), new TrapezoidProfile.State(5, 0));
// Profile will start stationary at zero position
// Profile will end stationary at 5 meters
// Returns the motion profile state after 5 seconds of motion
profile.Calculate(5_s,
frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps},
frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps});
# Profile will start stationary at zero position
# Profile will end stationary at 5 meters
# Returns the motion profile state after 5 seconds of motion
profile.calculate(5, TrapezoidProfile.State(0, 0), TrapezoidProfile.State(5, 0))

使用状态

The calculate method returns a TrapezoidProfile.State class (the same one that was used to specify the initial/end states when calculating the profile state). To use this for actual control, simply pass the contained position and velocity values to whatever controller you wish (for example, a PIDController):

var setpoint = profile.calculate(elapsedTime, initialState, goalState);
controller.calculate(encoder.getDistance(), setpoint.position);
auto setpoint = profile.Calculate(elapsedTime, initialState, goalState);
controller.Calculate(encoder.GetDistance(), setpoint.position.value());
setpoint = profile.calculate(elapsedTime, initialState, goalState)
controller.calculate(encoder.getDistance(), setpoint.position)

完整用法示例

备注

In this example, the initial state is re-computed every timestep. This is a somewhat different usage technique than is detailed above, but works according to the same principles - the profile is sampled at a time corresponding to the loop period to get the setpoint for the next loop iteration.

A more complete example of TrapezoidProfile usage is provided in the ElevatorTrapezoidProfile example project (Java, C++, Python):

 5package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
 6
 7import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 8import edu.wpi.first.math.trajectory.TrapezoidProfile;
 9import edu.wpi.first.wpilibj.Joystick;
10import edu.wpi.first.wpilibj.TimedRobot;
11
12public class Robot extends TimedRobot {
13  private static double kDt = 0.02;
14
15  private final Joystick m_joystick = new Joystick(1);
16  private final ExampleSmartMotorController m_motor = new ExampleSmartMotorController(1);
17  // Note: These gains are fake, and will have to be tuned for your robot.
18  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1.5);
19
20  // Create a motion profile with the given maximum velocity and maximum
21  // acceleration constraints for the next setpoint.
22  private final TrapezoidProfile m_profile =
23      new TrapezoidProfile(new TrapezoidProfile.Constraints(1.75, 0.75));
24  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
25  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
26
27  @Override
28  public void robotInit() {
29    // Note: These gains are fake, and will have to be tuned for your robot.
30    m_motor.setPID(1.3, 0.0, 0.7);
31  }
32
33  @Override
34  public void teleopPeriodic() {
35    if (m_joystick.getRawButtonPressed(2)) {
36      m_goal = new TrapezoidProfile.State(5, 0);
37    } else if (m_joystick.getRawButtonPressed(3)) {
38      m_goal = new TrapezoidProfile.State();
39    }
40
41    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
42    // toward the goal while obeying the constraints.
43    m_setpoint = m_profile.calculate(kDt, m_setpoint, m_goal);
44
45    // Send setpoint to offboard controller PID
46    m_motor.setSetpoint(
47        ExampleSmartMotorController.PIDMode.kPosition,
48        m_setpoint.position,
49        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
50  }
51}
 5#include <numbers>
 6
 7#include <frc/Joystick.h>
 8#include <frc/TimedRobot.h>
 9#include <frc/controller/SimpleMotorFeedforward.h>
10#include <frc/trajectory/TrapezoidProfile.h>
11#include <units/acceleration.h>
12#include <units/length.h>
13#include <units/time.h>
14#include <units/velocity.h>
15#include <units/voltage.h>
16
17#include "ExampleSmartMotorController.h"
18
19class Robot : public frc::TimedRobot {
20 public:
21  static constexpr units::second_t kDt = 20_ms;
22
23  Robot() {
24    // Note: These gains are fake, and will have to be tuned for your robot.
25    m_motor.SetPID(1.3, 0.0, 0.7);
26  }
27
28  void TeleopPeriodic() override {
29    if (m_joystick.GetRawButtonPressed(2)) {
30      m_goal = {5_m, 0_mps};
31    } else if (m_joystick.GetRawButtonPressed(3)) {
32      m_goal = {0_m, 0_mps};
33    }
34
35    // Retrieve the profiled setpoint for the next timestep. This setpoint moves
36    // toward the goal while obeying the constraints.
37    m_setpoint = m_profile.Calculate(kDt, m_setpoint, m_goal);
38
39    // Send setpoint to offboard controller PID
40    m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
41                        m_setpoint.position.value(),
42                        m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
43  }
44
45 private:
46  frc::Joystick m_joystick{1};
47  ExampleSmartMotorController m_motor{1};
48  frc::SimpleMotorFeedforward<units::meters> m_feedforward{
49      // Note: These gains are fake, and will have to be tuned for your robot.
50      1_V, 1.5_V * 1_s / 1_m};
51
52  // Create a motion profile with the given maximum velocity and maximum
53  // acceleration constraints for the next setpoint.
54  frc::TrapezoidProfile<units::meters> m_profile{{1.75_mps, 0.75_mps_sq}};
55  frc::TrapezoidProfile<units::meters>::State m_goal;
56  frc::TrapezoidProfile<units::meters>::State m_setpoint;
57};
58
59#ifndef RUNNING_FRC_TESTS
60int main() {
61  return frc::StartRobot<Robot>();
62}
63#endif
 8import wpilib
 9import wpimath.controller
10import wpimath.trajectory
11import examplesmartmotorcontroller
12
13
14class MyRobot(wpilib.TimedRobot):
15    kDt = 0.02
16
17    def robotInit(self):
18        self.joystick = wpilib.Joystick(1)
19        self.motor = examplesmartmotorcontroller.ExampleSmartMotorController(1)
20        # Note: These gains are fake, and will have to be tuned for your robot.
21        self.feedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 1.5)
22
23        self.constraints = wpimath.trajectory.TrapezoidProfile.Constraints(1.75, 0.75)
24
25        self.goal = wpimath.trajectory.TrapezoidProfile.State()
26        self.setpoint = wpimath.trajectory.TrapezoidProfile.State()
27
28        # Note: These gains are fake, and will have to be tuned for your robot.
29        self.motor.setPID(1.3, 0.0, 0.7)
30
31    def teleopPeriodic(self):
32        if self.joystick.getRawButtonPressed(2):
33            self.goal = wpimath.trajectory.TrapezoidProfile.State(5, 0)
34        elif self.joystick.getRawButtonPressed(3):
35            self.goal = wpimath.trajectory.TrapezoidProfile.State(0, 0)
36
37        # Create a motion profile with the given maximum velocity and maximum
38        # acceleration constraints for the next setpoint, the desired goal, and the
39        # current setpoint.
40        profile = wpimath.trajectory.TrapezoidProfile(
41            self.constraints, self.goal, self.setpoint
42        )
43
44        # Retrieve the profiled setpoint for the next timestep. This setpoint moves
45        # toward the goal while obeying the constraints.
46        self.setpoint = profile.calculate(self.kDt)
47
48        # Send setpoint to offboard controller PID
49        self.motor.setSetPoint(
50            examplesmartmotorcontroller.ExampleSmartMotorController.PIDMode.kPosition,
51            self.setpoint.position,
52            self.feedforward.calculate(self.setpoint.velocity) / 12,
53        )