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
// 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 <memory>
#include <hal/AnalogGyro.h>
#include <hal/Types.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/geometry/Rotation2d.h"
namespace frc {
class AnalogInput;
/**
* Use a rate gyro to return the robots heading relative to a starting position.
* The Gyro class tracks the robots heading based on the starting position. As
* the robot rotates the new heading is computed by integrating the rate of
* rotation returned by the sensor. When the class is instantiated, it does a
* short calibration routine where it samples the gyro while at rest to
* determine the default offset. This is subtracted from each sample to
* determine the heading. This gyro class must be used with a channel that is
* assigned one of the Analog accumulators from the FPGA. See AnalogInput for
* the current accumulator assignments.
*
* This class is for gyro sensors that connect to an analog input.
*/
class AnalogGyro : public wpi::Sendable,
public wpi::SendableHelper<AnalogGyro> {
public:
static constexpr int kOversampleBits = 10;
static constexpr int kAverageBits = 0;
static constexpr double kSamplesPerSecond = 50.0;
static constexpr double kCalibrationSampleTime = 5.0;
static constexpr double kDefaultVoltsPerDegreePerSecond = 0.007;
/**
* %Gyro constructor using the Analog Input channel number.
*
* @param channel The analog channel the gyro is connected to. Gyros can only
* be used on on-board Analog Inputs 0-1.
*/
explicit AnalogGyro(int channel);
/**
* Gyro constructor with a precreated AnalogInput object.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* Gyros can only be used on on-board channels 0-1.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
explicit AnalogGyro(AnalogInput* channel);
/**
* %Gyro constructor with a precreated AnalogInput object.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
*/
explicit AnalogGyro(std::shared_ptr<AnalogInput> channel);
/**
* %Gyro constructor using the Analog Input channel number with parameters for
* presetting the center and offset values. Bypasses calibration.
*
* @param channel The analog channel the gyro is connected to. Gyros can only
* be used on on-board Analog Inputs 0-1.
* @param center Preset uncalibrated value to use as the accumulator center
* value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
AnalogGyro(int channel, int center, double offset);
/**
* %Gyro constructor with a precreated AnalogInput object and calibrated
* parameters.
*
* Use this constructor when the analog channel needs to be shared.
* This object will not clean up the AnalogInput object when using this
* constructor.
*
* @param channel A pointer to the AnalogInput object that the gyro is
* connected to.
* @param center Preset uncalibrated value to use as the accumulator center
* value.
* @param offset Preset uncalibrated value to use as the gyro offset.
*/
AnalogGyro(std::shared_ptr<AnalogInput> channel, int center, double offset);
AnalogGyro(AnalogGyro&& rhs) = default;
AnalogGyro& operator=(AnalogGyro&& rhs) = default;
~AnalogGyro() override = default;
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on the current accumulator value corrected by the
* oversampling rate, the gyro type and the A/D calibration values. The angle
* is continuous, that is it will continue from 360->361 degrees. This allows
* algorithms that wouldn't want to see a discontinuity in the gyro output as
* it sweeps from 360 to 0 on the second time around.
*
* @return The current heading of the robot in degrees. This heading is based
* on integration of the returned rate from the gyro.
*/
double GetAngle() const;
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro analog value
*
* @return the current rate in degrees per second
*/
double GetRate() const;
/**
* Return the gyro center value. If run after calibration,
* the center value can be used as a preset later.
*
* @return the current center value
*/
virtual int GetCenter() const;
/**
* Return the gyro offset value. If run after calibration,
* the offset value can be used as a preset later.
*
* @return the current offset value
*/
virtual double GetOffset() const;
/**
* Set the gyro sensitivity.
*
* This takes the number of volts/degree/second sensitivity of the gyro and
* uses it in subsequent calculations to allow the code to work with multiple
* gyros. This value is typically found in the gyro datasheet.
*
* @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second
*/
void SetSensitivity(double voltsPerDegreePerSecond);
/**
* Set the size of the neutral zone.
*
* Any voltage from the gyro less than this amount from the center is
* considered stationary. Setting a deadband will decrease the amount of
* drift when the gyro isn't rotating, but will make it less accurate.
*
* @param volts The size of the deadband in volts
*/
void SetDeadband(double volts);
/**
* Reset the gyro.
*
* Resets the gyro to a heading of zero. This can be used if there is
* significant drift in the gyro and it needs to be recalibrated after it has
* been running.
*/
void Reset();
/**
* Initialize the gyro.
*
* Calibration is handled by Calibrate().
*/
void InitGyro();
/**
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use the center value as the Accumulator center value for
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the competition
* starts.
*/
void Calibrate();
/**
* Return the heading of the robot as a Rotation2d.
*
* The angle is continuous, that is it will continue from 360 to 361 degrees.
* This allows algorithms that wouldn't want to see a discontinuity in the
* gyro output as it sweeps past from 360 to 0 on the second time around.
*
* The angle is expected to increase as the gyro turns counterclockwise when
* looked at from the top. It needs to follow the NWU axis convention.
*
* @return the current heading of the robot as a Rotation2d. This heading is
* based on integration of the returned rate from the gyro.
*/
Rotation2d GetRotation2d() const;
/**
* Gets the analog input for the gyro.
*
* @return AnalogInput
*/
std::shared_ptr<AnalogInput> GetAnalogInput() const;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
std::shared_ptr<AnalogInput> m_analog;
hal::Handle<HAL_GyroHandle, HAL_FreeAnalogGyro> m_gyroHandle;
};
} // namespace frc