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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
// 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 <array>
#include <units/length.h>
#include <units/mass.h>
#include <units/velocity.h>
#include "frc/simulation/LinearSystemSim.h"
#include "frc/system/plant/DCMotor.h"
namespace frc::sim {
/**
* Represents a simulated elevator mechanism.
*/
class ElevatorSim : public LinearSystemSim<2, 1, 2> {
public:
template <typename Distance>
using Velocity_t = units::unit_t<
units::compound_unit<Distance, units::inverse<units::seconds>>>;
template <typename Distance>
using Acceleration_t = units::unit_t<units::compound_unit<
units::compound_unit<Distance, units::inverse<units::seconds>>,
units::inverse<units::seconds>>>;
/**
* Constructs a simulated elevator mechanism.
*
* @param plant The linear system that represents the elevator.
* This system can be created with
* LinearSystemId::ElevatorSystem().
* @param gearbox The type of and number of motors in your
* elevator gearbox.
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
*
* @param gearbox The type of and number of motors in your
* elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater
* than 1 represent reductions).
* @param carriageMass The mass of the elevator carriage.
* @param drumRadius The radius of the drum that your cable is
* wrapped around.
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const DCMotor& gearbox, double gearing,
units::kilogram_t carriageMass, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
*
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in your
* elevator gearbox.
* @param minHeight The minimum allowed height of the elevator.
* @param maxHeight The maximum allowed height of the elevator.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
template <typename Distance>
requires std::same_as<units::meter, Distance> ||
std::same_as<units::radian, Distance>
ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
decltype(1_V / Acceleration_t<Distance>(1)) kA,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
using LinearSystemSim::SetState;
/**
* Sets the elevator's state. The new position will be limited between the
* minimum and maximum allowed heights.
* @param position The new position
* @param velocity The new velocity
*/
void SetState(units::meter_t position, units::meters_per_second_t velocity);
/**
* Returns whether the elevator would hit the lower limit.
*
* @param elevatorHeight The elevator height.
* @return Whether the elevator would hit the lower limit.
*/
bool WouldHitLowerLimit(units::meter_t elevatorHeight) const;
/**
* Returns whether the elevator would hit the upper limit.
*
* @param elevatorHeight The elevator height.
* @return Whether the elevator would hit the upper limit.
*/
bool WouldHitUpperLimit(units::meter_t elevatorHeight) const;
/**
* Returns whether the elevator has hit the lower limit.
*
* @return Whether the elevator has hit the lower limit.
*/
bool HasHitLowerLimit() const;
/**
* Returns whether the elevator has hit the upper limit.
*
* @return Whether the elevator has hit the upper limit.
*/
bool HasHitUpperLimit() const;
/**
* Returns the position of the elevator.
*
* @return The position of the elevator.
*/
units::meter_t GetPosition() const;
/**
* Returns the velocity of the elevator.
*
* @return The velocity of the elevator.
*/
units::meters_per_second_t GetVelocity() const;
/**
* Returns the elevator current draw.
*
* @return The elevator current draw.
*/
units::ampere_t GetCurrentDraw() const;
/**
* Sets the input voltage for the elevator.
*
* @param voltage The input voltage.
*/
void SetInputVoltage(units::volt_t voltage);
protected:
/**
* Updates the state estimate of the elevator.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dt The time difference between controller updates.
*/
Vectord<2> UpdateX(const Vectord<2>& currentXhat, const Vectord<1>& u,
units::second_t dt) override;
private:
DCMotor m_gearbox;
units::meter_t m_minHeight;
units::meter_t m_maxHeight;
bool m_simulateGravity;
};
} // namespace frc::sim