1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include <wpi/interpolating_map.h>
#include "frc/EigenCore.h"
#include "frc/geometry/Pose2d.h"
#include "frc/kinematics/ChassisSpeeds.h"
#include "frc/trajectory/Trajectory.h"
#include "units/angular_velocity.h"
#include "units/time.h"
#include "units/velocity.h"
namespace frc {
/**
* The linear time-varying unicycle controller has a similar form to the LQR,
* but the model used to compute the controller gain is the nonlinear unicycle
* model linearized around the drivetrain's current state.
*
* This controller is a roughly drop-in replacement for RamseteController with
* more optimal feedback gains in the "least-squares error" sense.
*
* See section 8.9 in Controls Engineering in FRC for a derivation of the
* control law we used shown in theorem 8.9.1.
*/
class WPILIB_DLLEXPORT LTVUnicycleController {
public:
/**
* Constructs a linear time-varying unicycle controller with default maximum
* desired error tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad)
* and default maximum desired control effort of (linear velocity = 1 m/s,
* angular velocity = 2 rad/s).
*
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
* @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
*/
explicit LTVUnicycleController(
units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
/**
* Constructs a linear time-varying unicycle controller.
*
* See
* https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning
* for how to select the tolerances.
*
* @param Qelems The maximum desired error tolerance for each state (x, y,
* heading).
* @param Relems The maximum desired control effort for each input (linear
* velocity, angular velocity).
* @param dt Discretization timestep.
* @param maxVelocity The maximum velocity for the controller gain lookup
* table.
* @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s.
*/
LTVUnicycleController(const wpi::array<double, 3>& Qelems,
const wpi::array<double, 2>& Relems, units::second_t dt,
units::meters_per_second_t maxVelocity = 9_mps);
/**
* Move constructor.
*/
LTVUnicycleController(LTVUnicycleController&&) = default;
/**
* Move assignment operator.
*/
LTVUnicycleController& operator=(LTVUnicycleController&&) = default;
/**
* Returns true if the pose error is within tolerance of the reference.
*/
bool AtReference() const;
/**
* Sets the pose error which is considered tolerable for use with
* AtReference().
*
* @param poseTolerance Pose error which is tolerable.
*/
void SetTolerance(const Pose2d& poseTolerance);
/**
* Returns the linear and angular velocity outputs of the LTV controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param poseRef The desired pose.
* @param linearVelocityRef The desired linear velocity.
* @param angularVelocityRef The desired angular velocity.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
units::meters_per_second_t linearVelocityRef,
units::radians_per_second_t angularVelocityRef);
/**
* Returns the linear and angular velocity outputs of the LTV controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param desiredState The desired pose, linear velocity, and angular velocity
* from a trajectory.
*/
ChassisSpeeds Calculate(const Pose2d& currentPose,
const Trajectory::State& desiredState);
/**
* Enables and disables the controller for troubleshooting purposes.
*
* @param enabled If the controller is enabled or not.
*/
void SetEnabled(bool enabled);
private:
// LUT from drivetrain linear velocity to LQR gain
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 3>> m_table;
Pose2d m_poseError;
Pose2d m_poseTolerance;
bool m_enabled = true;
};
} // namespace frc