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 )