WPILib’de Trapezoid Hareket Profilleri

Not

Bu makale, trapezoidal hareket profillerinin kod içi üretimini kapsar. İlgili kavramları daha ayrıntılı olarak açıklayan belgeler yakında çıkacaktır.

Not

TrapezoidProfile sınıfının komut tabanlı framework çerçevesinde uygulanmasına ilişkin bir kılavuz için bkz TrapezProfileSubsystems ve TrapezoidProfileCommands ile Hareket Profilleme .

Not

Kendi başına kullanılan TrapezoidProfile sınıfı, özel bir kontrolörle (yerleşik PID işlevselliğine sahip “akıllı” bir motor kontrolörü gibi) oluşturulduğunda çok kullanışlıdır. Bir WPILib PIDController ile entegre etmek için, bakınız Motion Profiling ve PID Control’ü ProfiledPIDController ile Birleştirme.

İleri besleme ve geri bildirim kontrolü, belirli bir ayar noktasına ulaşmak için uygun yollar sunsa da, çoğu zaman mekanizmalarımız için ayar noktaları oluşturma sorunuyla karşı karşıyayız. Bir mekanizmaya derhal istenen duruma getirme komutu yaklaşımı işe yarayabilirken, çoğu zaman yetersizdir. Mekanizmalarımızın işlevini iyileştirmek için, mekanizmalara, mevcut durumu ile istenen hedef durumu arasında sorunsuz bir şekilde geçiş yapan bir dizi ayar noktasına komut vermek isteriz.

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

Trapezoidal Profil Oluşturma

Not

C ++ ‘da, TrapezoidProfile ‘sınıfı, açısal veya doğrusal olabilen mesafe ölçümleri için kullanılan birim tipine göre şablonlanır. İletilen değerler zorunlu mesafe birimleriyle tutarlı birimlere sahip olmalıdır; aksi takdirde derleme zamanı hatası atılır. C ++ birimleri hakkında daha fazla bilgi için bkz . C ++ Ünite Kitaplığı.

Kısıtlamalar

Not

Çeşitli ileribesleme yardımcı sınıfları, bir mekanizmanın aynı anda elde edilebilen maksimum hızını ve ivmesini hesaplamak için yöntemler sağlar. Bunlar, TrapezoidProfile için uygun hareket kısıtlamalarını hesaplamak için çok yararlı olabilir.

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)

Başlangıç ve Bitiş Durumları

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)

Hepsini bir araya koymak

Not

C++ genellikle iç sınıfların türünü çıkarabilir ve bu nedenle basit bir başlatıcı listesi (sınıf adı olmadan) parametre olarak gönderilebilir. Tam sınıf adları, netlik açısından aşağıdaki örnekte verilmiştir.

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))

Bir TrapezoidProfile Kullanmak

Profili Örnekleme

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))

Profil Durumunun Kullanılması

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)

Tam Kullanım Örneği

Not

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        )