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++).
创建梯形轮廓
备注
在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++):
// 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};
起始和结束状态
Next, we must specify the desired starting and ending states for our mechanisms using the TrapezoidProfile.State
class (Java, C++). 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};
汇总
备注
C ++通常能够推断内部类的类型,因此可以将简单的初始化列表(没有类名)作为参数发送。为了清楚起见,完整的类名包含在下面的示例中。
既然我们知道如何创建一组约束和所需的开始/结束状态,我们就可以创建运动轮廓了。 TrapezoidProfile构造函数按顺序接受3个参数:约束,目标状态和初始状态。
// 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 will end stationary at 5 meters
// Profile will start stationary at zero position
TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(5, 10),
new TrapezoidProfile.State(5, 0),
new TrapezoidProfile.State(0, 0));
// 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 will end stationary at 5 meters
// Profile will start stationary at zero position
frc::TrapezoidProfile<units::meters> profile{
frc::TrapezoidProfile<units::meters>::Constraints{5_mps, 10_mps_sq},
frc::TrapezoidProfile<units::meters>::State{5_m, 0_mps},
frc::TrapezoidProfile<units::meters>::State{0_m, 0_mps}};
使用``TrapezoidProfile``
采样配置文件
创建了“ TrapezoidProfile”后,使用起来非常简单:要在配置文件启动后的给定时间获取配置文件状态,请调用calculate()方法:
// Returns the motion profile state after 5 seconds of motion
profile.calculate(5);
// Returns the motion profile state after 5 seconds of motion
profile.Calculate(5_s);
使用状态
calculate方法返回一个TrapezoidProfile.State类(在构造配置文件时用于指定初始/结束状态的类)。要将其用于实际控制,只需将包含的位置和速度值传递到所需的任何控制器(例如PIDController):
var setpoint = profile.calculate(elapsedTime);
controller.calculate(encoder.getDistance(), setpoint.position);
auto setpoint = profile.Calculate(elapsedTime);
controller.Calculate(encoder.GetDistance(), setpoint.position.value());
完整用法示例
备注
In this example, the profile 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.
在ElevatorTrapezoidProfile示例项目中提供了更完整的``TrapezoidProfile’’用法示例(Java,C ++):
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 private final TrapezoidProfile.Constraints m_constraints =
21 new TrapezoidProfile.Constraints(1.75, 0.75);
22 private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
23 private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
24
25 @Override
26 public void robotInit() {
27 // Note: These gains are fake, and will have to be tuned for your robot.
28 m_motor.setPID(1.3, 0.0, 0.7);
29 }
30
31 @Override
32 public void teleopPeriodic() {
33 if (m_joystick.getRawButtonPressed(2)) {
34 m_goal = new TrapezoidProfile.State(5, 0);
35 } else if (m_joystick.getRawButtonPressed(3)) {
36 m_goal = new TrapezoidProfile.State(0, 0);
37 }
38
39 // Create a motion profile with the given maximum velocity and maximum
40 // acceleration constraints for the next setpoint, the desired goal, and the
41 // current setpoint.
42 var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
43
44 // Retrieve the profiled setpoint for the next timestep. This setpoint moves
45 // toward the goal while obeying the constraints.
46 m_setpoint = profile.calculate(kDt);
47
48 // Send setpoint to offboard controller PID
49 m_motor.setSetpoint(
50 ExampleSmartMotorController.PIDMode.kPosition,
51 m_setpoint.position,
52 m_feedforward.calculate(m_setpoint.velocity) / 12.0);
53 }
54}
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 // Create a motion profile with the given maximum velocity and maximum
36 // acceleration constraints for the next setpoint, the desired goal, and the
37 // current setpoint.
38 frc::TrapezoidProfile<units::meters> profile{m_constraints, m_goal,
39 m_setpoint};
40
41 // Retrieve the profiled setpoint for the next timestep. This setpoint moves
42 // toward the goal while obeying the constraints.
43 m_setpoint = profile.Calculate(kDt);
44
45 // Send setpoint to offboard controller PID
46 m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
47 m_setpoint.position.value(),
48 m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
49 }
50
51 private:
52 frc::Joystick m_joystick{1};
53 ExampleSmartMotorController m_motor{1};
54 frc::SimpleMotorFeedforward<units::meters> m_feedforward{
55 // Note: These gains are fake, and will have to be tuned for your robot.
56 1_V, 1.5_V * 1_s / 1_m};
57
58 frc::TrapezoidProfile<units::meters>::Constraints m_constraints{1.75_mps,
59 0.75_mps_sq};
60 frc::TrapezoidProfile<units::meters>::State m_goal;
61 frc::TrapezoidProfile<units::meters>::State m_setpoint;
62};
63
64#ifndef RUNNING_FRC_TESTS
65int main() {
66 return frc::StartRobot<Robot>();
67}
68#endif