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
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
// 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 <frc/EigenCore.h>
#include <frc/kinematics/DifferentialDriveKinematics.h>
#include <frc/system/LinearSystem.h>
#include <frc/system/plant/DCMotor.h>
#include <units/length.h>
#include <units/moment_of_inertia.h>
#include <units/time.h>
#include <units/voltage.h>
namespace frc::sim {
class DifferentialDrivetrainSim {
public:
/**
* Creates a simulated differential drivetrain.
*
* @param plant The LinearSystem representing the robot's drivetrain. This
* system can be created with
* LinearSystemId::DrivetrainVelocitySystem() or
* LinearSystemId::IdentifyDrivetrainSystem().
* @param trackWidth The robot's track width.
* @param driveMotor A DCMotor representing the left side of the drivetrain.
* @param gearingRatio The gearingRatio ratio of the left side, as output over
* input. This must be the same ratio as the ratio used to
* identify or create the plant.
* @param wheelRadius The radius of the wheels on the drivetrain, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity,
* left distance, right distance]ᵀ. Can be omitted
* if no noise is desired. Gyro standard deviations
* of 0.0001 radians, velocity standard deviations
* of 0.05 m/s, and position measurement standard
* deviations of 0.005 meters are a reasonable
* starting point.
*/
DifferentialDrivetrainSim(
LinearSystem<2, 2, 2> plant, units::meter_t trackWidth,
DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
const std::array<double, 7>& measurementStdDevs = {});
/**
* Creates a simulated differential drivetrain.
*
* @param driveMotor A DCMotor representing the left side of the drivetrain.
* @param gearing The gearing on the drive between motor and wheel, as
* output over input. This must be the same ratio as the
* ratio used to identify or create the plant.
* @param J The moment of inertia of the drivetrain about its
* center.
* @param mass The mass of the drivebase.
* @param wheelRadius The radius of the wheels on the drivetrain.
* @param trackWidth The robot's track width, or distance between left and
* right wheels.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity,
* left distance, right distance]ᵀ. Can be omitted
* if no noise is desired. Gyro standard deviations
* of 0.0001 radians, velocity standard deviations
* of 0.05 m/s, and position measurement standard
* deviations of 0.005 meters are a reasonable
* starting point.
*/
DifferentialDrivetrainSim(
frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
units::kilogram_t mass, units::meter_t wheelRadius,
units::meter_t trackWidth,
const std::array<double, 7>& measurementStdDevs = {});
/**
* Clamp the input vector such that no element exceeds the battery voltage.
* If any does, the relative magnitudes of the input will be maintained.
*
* @param u The input vector.
* @return The normalized input.
*/
Eigen::Vector2d ClampInput(const Eigen::Vector2d& u);
/**
* Sets the applied voltage to the drivetrain. Note that positive voltage must
* make that side of the drivetrain travel forward (+X).
*
* @param leftVoltage The left voltage.
* @param rightVoltage The right voltage.
*/
void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
/**
* Sets the gearing reduction on the drivetrain. This is commonly used for
* shifting drivetrains.
*
* @param newGearing The new gear ratio, as output over input.
*/
void SetGearing(double newGearing);
/**
* Updates the simulation.
*
* @param dt The time that's passed since the last Update(units::second_t)
* call.
*/
void Update(units::second_t dt);
/**
* Returns the current gearing reduction of the drivetrain, as output over
* input.
*/
double GetGearing() const;
/**
* Returns the direction the robot is pointing.
*
* Note that this angle is counterclockwise-positive, while most gyros are
* clockwise positive.
*/
Rotation2d GetHeading() const;
/**
* Returns the current pose.
*/
Pose2d GetPose() const;
/**
* Get the right encoder position in meters.
* @return The encoder position.
*/
units::meter_t GetRightPosition() const {
return units::meter_t{GetOutput(State::kRightPosition)};
}
/**
* Get the right encoder velocity in meters per second.
* @return The encoder velocity.
*/
units::meters_per_second_t GetRightVelocity() const {
return units::meters_per_second_t{GetOutput(State::kRightVelocity)};
}
/**
* Get the left encoder position in meters.
* @return The encoder position.
*/
units::meter_t GetLeftPosition() const {
return units::meter_t{GetOutput(State::kLeftPosition)};
}
/**
* Get the left encoder velocity in meters per second.
* @return The encoder velocity.
*/
units::meters_per_second_t GetLeftVelocity() const {
return units::meters_per_second_t{GetOutput(State::kLeftVelocity)};
}
/**
* Returns the currently drawn current for the right side.
*/
units::ampere_t GetRightCurrentDraw() const;
/**
* Returns the currently drawn current for the left side.
*/
units::ampere_t GetLeftCurrentDraw() const;
/**
* Returns the currently drawn current.
*/
units::ampere_t GetCurrentDraw() const;
/**
* Sets the system state.
*
* @param state The state.
*/
void SetState(const Vectord<7>& state);
/**
* Sets the system pose.
*
* @param pose The pose.
*/
void SetPose(const frc::Pose2d& pose);
/**
* The differential drive dynamics function.
*
* @param x The state.
* @param u The input.
* @return The state derivative with respect to time.
*/
Vectord<7> Dynamics(const Vectord<7>& x, const Eigen::Vector2d& u);
class State {
public:
static constexpr int kX = 0;
static constexpr int kY = 1;
static constexpr int kHeading = 2;
static constexpr int kLeftVelocity = 3;
static constexpr int kRightVelocity = 4;
static constexpr int kLeftPosition = 5;
static constexpr int kRightPosition = 6;
};
/**
* Represents a gearing option of the Toughbox mini.
* 12.75:1 -- 14:50 and 14:50
* 10.71:1 -- 14:50 and 16:48
* 8.45:1 -- 14:50 and 19:45
* 7.31:1 -- 14:50 and 21:43
* 5.95:1 -- 14:50 and 24:40
*/
class KitbotGearing {
public:
/// Gear ratio of 12.75:1.
static constexpr double k12p75 = 12.75;
/// Gear ratio of 10.71:1.
static constexpr double k10p71 = 10.71;
/// Gear ratio of 8.45:1.
static constexpr double k8p45 = 8.45;
/// Gear ratio of 7.31:1.
static constexpr double k7p31 = 7.31;
/// Gear ratio of 5.95:1.
static constexpr double k5p95 = 5.95;
};
/**
* Represents common motor layouts of the kit drivetrain.
*/
class KitbotMotor {
public:
/// One CIM motor per drive side.
static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
/// Two CIM motors per drive side.
static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
/// One Mini CIM motor per drive side.
static constexpr frc::DCMotor SingleMiniCIMPerSide =
frc::DCMotor::MiniCIM(1);
/// Two Mini CIM motors per drive side.
static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
/// One Falcon 500 motor per drive side.
static constexpr frc::DCMotor SingleFalcon500PerSide =
frc::DCMotor::Falcon500(1);
/// Two Falcon 500 motors per drive side.
static constexpr frc::DCMotor DualFalcon500PerSide =
frc::DCMotor::Falcon500(2);
/// One NEO motor per drive side.
static constexpr frc::DCMotor SingleNEOPerSide = frc::DCMotor::NEO(1);
/// Two NEO motors per drive side.
static constexpr frc::DCMotor DualNEOPerSide = frc::DCMotor::NEO(2);
};
/**
* Represents common wheel sizes of the kit drivetrain.
*/
class KitbotWheelSize {
public:
/// Six inch diameter wheels.
static constexpr units::meter_t kSixInch = 6_in;
/// Eight inch diameter wheels.
static constexpr units::meter_t kEightInch = 8_in;
/// Ten inch diameter wheels.
static constexpr units::meter_t kTenInch = 10_in;
};
/**
* Create a sim for the standard FRC kitbot.
*
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.
*/
static DifferentialDrivetrainSim CreateKitbotSim(
frc::DCMotor motor, double gearing, units::meter_t wheelSize,
const std::array<double, 7>& measurementStdDevs = {}) {
// MOI estimation -- note that I = mr² for point masses
units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
2 // CIM plus toughbox per side
* (26_in / 2) * (26_in / 2);
return DifferentialDrivetrainSim{
motor, gearing, batteryMoi + gearboxMoi, 60_lb,
wheelSize / 2.0, 26_in, measurementStdDevs};
}
/**
* Create a sim for the standard FRC kitbot.
*
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param J The moment of inertia of the drivebase. This can be
* calculated using SysId.
* @param measurementStdDevs Standard deviations for measurements, in the form
* [x, y, heading, left velocity, right velocity, left distance, right
* distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
* deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
* position measurement standard deviations of 0.005 meters are a reasonable
* starting point.
*/
static DifferentialDrivetrainSim CreateKitbotSim(
frc::DCMotor motor, double gearing, units::meter_t wheelSize,
units::kilogram_square_meter_t J,
const std::array<double, 7>& measurementStdDevs = {}) {
return DifferentialDrivetrainSim{
motor, gearing, J, 60_lb, wheelSize / 2.0, 26_in, measurementStdDevs};
}
private:
/**
* Returns an element of the state vector.
*
* @param output The row of the output vector.
*/
double GetOutput(int output) const;
/**
* Returns the current output vector y.
*/
Vectord<7> GetOutput() const;
/**
* Returns an element of the state vector. Note that this will not include
* noise!
*
* @param output The row of the output vector.
*/
double GetState(int state) const;
/**
* Returns the current state vector x. Note that this will not include noise!
*/
Vectord<7> GetState() const;
LinearSystem<2, 2, 2> m_plant;
units::meter_t m_rb;
units::meter_t m_wheelRadius;
DCMotor m_motor;
double m_originalGearing;
double m_currentGearing;
Vectord<7> m_x;
Eigen::Vector2d m_u;
Vectord<7> m_y;
std::array<double, 7> m_measurementStdDevs;
};
} // namespace frc::sim