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
// 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 "frc/controller/DifferentialDriveWheelVoltages.h"
#include "frc/system/LinearSystem.h"
#include "frc/system/plant/LinearSystemId.h"
#include "units/acceleration.h"
#include "units/angular_acceleration.h"
#include "units/angular_velocity.h"
#include "units/length.h"
#include "units/time.h"
#include "units/velocity.h"
#include "units/voltage.h"
namespace frc {
/**
* A helper class which computes the feedforward outputs for a differential
* drive drivetrain.
*/
class WPILIB_DLLEXPORT DifferentialDriveFeedforward {
frc::LinearSystem<2, 2, 2> m_plant;
public:
/**
* Creates a new DifferentialDriveFeedforward with the specified parameters.
*
* @param kVLinear The linear velocity gain in volts per (meters per second).
* @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
* @param kVAngular The angular velocity gain in volts per (radians per
* second).
* @param kAAngular The angular acceleration gain in volts per (radians per
* second squared).
* @param trackwidth The distance between the differential drive's left and
* right wheels, in meters.
*/
constexpr DifferentialDriveFeedforward(
decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_rad_per_s) kVAngular,
decltype(1_V / 1_rad_per_s_sq) kAAngular, units::meter_t trackwidth)
// See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps),
// decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V /
// 1_rad_per_s_sq))
: DifferentialDriveFeedforward{kVLinear, kALinear,
kVAngular * 2.0 / trackwidth * 1_rad,
kAAngular * 2.0 / trackwidth * 1_rad} {}
/**
* Creates a new DifferentialDriveFeedforward with the specified parameters.
*
* @param kVLinear The linear velocity gain in volts per (meters per second).
* @param kALinear The linear acceleration gain in volts per (meters per
* second squared).
* @param kVAngular The angular velocity gain in volts per (meters per
* second).
* @param kAAngular The angular acceleration gain in volts per (meters per
* second squared).
*/
constexpr DifferentialDriveFeedforward(decltype(1_V / 1_mps) kVLinear,
decltype(1_V / 1_mps_sq) kALinear,
decltype(1_V / 1_mps) kVAngular,
decltype(1_V / 1_mps_sq) kAAngular)
: m_plant{frc::LinearSystemId::IdentifyDrivetrainSystem(
kVLinear, kALinear, kVAngular, kAAngular)},
m_kVLinear{kVLinear},
m_kALinear{kALinear},
m_kVAngular{kVAngular},
m_kAAngular{kAAngular} {}
/**
* Calculates the differential drive feedforward inputs given velocity
* setpoints.
*
* @param currentLeftVelocity The current left velocity of the differential
* drive in meters/second.
* @param nextLeftVelocity The next left velocity of the differential drive in
* meters/second.
* @param currentRightVelocity The current right velocity of the differential
* drive in meters/second.
* @param nextRightVelocity The next right velocity of the differential drive
* in meters/second.
* @param dt Discretization timestep.
*/
DifferentialDriveWheelVoltages Calculate(
units::meters_per_second_t currentLeftVelocity,
units::meters_per_second_t nextLeftVelocity,
units::meters_per_second_t currentRightVelocity,
units::meters_per_second_t nextRightVelocity, units::second_t dt);
decltype(1_V / 1_mps) m_kVLinear;
decltype(1_V / 1_mps_sq) m_kALinear;
decltype(1_V / 1_mps) m_kVAngular;
decltype(1_V / 1_mps_sq) m_kAAngular;
};
} // namespace frc
#include "frc/controller/proto/DifferentialDriveFeedforwardProto.h"
#include "frc/controller/struct/DifferentialDriveFeedforwardStruct.h"