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
136
137
138
// 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/controller/DifferentialDriveWheelVoltages.h"
#include "frc/geometry/Pose2d.h"
#include "frc/system/LinearSystem.h"
#include "frc/trajectory/Trajectory.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
namespace frc {
/**
* The linear time-varying differential drive controller has a similar form to
* the LQR, but the model used to compute the controller gain is the nonlinear
* differential drive model linearized around the drivetrain's current state. We
* precompute gains for important places in our state-space, then interpolate
* between them with a lookup table to save computational resources.
*
* This controller has a flat hierarchy with pose and wheel velocity references
* and voltage outputs. This is different from a Ramsete controller's nested
* hierarchy where the top-level controller has a pose reference and chassis
* velocity command outputs, and the low-level controller has wheel velocity
* references and voltage outputs. Flat hierarchies are easier to tune in one
* shot. Furthermore, this controller is more optimal in the "least-squares
* error" sense than a controller based on Ramsete.
*
* See section 8.7 in Controls Engineering in FRC for a derivation of the
* control law we used shown in theorem 8.7.4.
*/
class WPILIB_DLLEXPORT LTVDifferentialDriveController {
public:
/**
* Constructs a linear time-varying differential drive 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 plant The differential drive velocity plant.
* @param trackwidth The distance between the differential drive's left and
* right wheels.
* @param Qelems The maximum desired error tolerance for each state.
* @param Relems The maximum desired control effort for each input.
* @param dt Discretization timestep.
* @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s
* or >= 15 m/s.
*/
LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant,
units::meter_t trackwidth,
const wpi::array<double, 5>& Qelems,
const wpi::array<double, 2>& Relems,
units::second_t dt);
/**
* Move constructor.
*/
LTVDifferentialDriveController(LTVDifferentialDriveController&&) = default;
/**
* Move assignment operator.
*/
LTVDifferentialDriveController& operator=(LTVDifferentialDriveController&&) =
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.
* @param leftVelocityTolerance Left velocity error which is tolerable.
* @param rightVelocityTolerance Right velocity error which is tolerable.
*/
void SetTolerance(const Pose2d& poseTolerance,
units::meters_per_second_t leftVelocityTolerance,
units::meters_per_second_t rightVelocityTolerance);
/**
* Returns the left and right output voltages of the LTV controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param leftVelocity The current left velocity.
* @param rightVelocity The current right velocity.
* @param poseRef The desired pose.
* @param leftVelocityRef The desired left velocity.
* @param rightVelocityRef The desired right velocity.
*/
DifferentialDriveWheelVoltages Calculate(
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, const Pose2d& poseRef,
units::meters_per_second_t leftVelocityRef,
units::meters_per_second_t rightVelocityRef);
/**
* Returns the left and right output voltages of the LTV controller.
*
* The reference pose, linear velocity, and angular velocity should come from
* a drivetrain trajectory.
*
* @param currentPose The current pose.
* @param leftVelocity The left velocity.
* @param rightVelocity The right velocity.
* @param desiredState The desired pose, linear velocity, and angular velocity
* from a trajectory.
*/
DifferentialDriveWheelVoltages Calculate(
const Pose2d& currentPose, units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity,
const Trajectory::State& desiredState);
private:
units::meter_t m_trackwidth;
// LUT from drivetrain linear velocity to LQR gain
wpi::interpolating_map<units::meters_per_second_t, Matrixd<2, 5>> m_table;
Vectord<5> m_error;
Vectord<5> m_tolerance;
};
} // namespace frc