-
Notifications
You must be signed in to change notification settings - Fork 6
/
Gyro.cpp
248 lines (212 loc) · 6.58 KB
/
Gyro.cpp
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
/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/
#include "Gyro.h"
#include "AnalogChannel.h"
#include "AnalogModule.h"
#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"
const uint32_t Gyro::kOversampleBits;
const uint32_t Gyro::kAverageBits;
constexpr float Gyro::kSamplesPerSecond;
constexpr float Gyro::kCalibrationSampleTime;
constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
/**
* Initialize the gyro.
* Calibrate the gyro by running for a number of samples and computing the center value for this
* part. 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 Gyro::InitGyro()
{
m_table = NULL;
if (!m_analog->IsAccumulatorChannel())
{
wpi_setWPIErrorWithContext(ParameterOutOfRange,
"moduleNumber and/or channel (must be accumulator channel)");
if (m_channelAllocated)
{
delete m_analog;
m_analog = NULL;
}
return;
}
m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
m_analog->SetAverageBits(kAverageBits);
m_analog->SetOversampleBits(kOversampleBits);
float sampleRate = kSamplesPerSecond *
(1 << (kAverageBits + kOversampleBits));
m_analog->GetModule()->SetSampleRate(sampleRate);
Wait(1.0);
m_analog->InitAccumulator();
Wait(kCalibrationSampleTime);
INT64 value;
uint32_t count;
m_analog->GetAccumulatorOutput(&value, &count);
m_center = (uint32_t)((float)value / (float)count + .5);
m_offset = ((float)value / (float)count) - (float)m_center;
m_analog->SetAccumulatorCenter(m_center);
m_analog->SetAccumulatorDeadband(0); ///< TODO: compute / parameterize this
m_analog->ResetAccumulator();
SetPIDSourceParameter(kAngle);
nUsageReporting::report(nUsageReporting::kResourceType_Gyro, m_analog->GetChannel(), m_analog->GetModuleNumber() - 1);
LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetModuleNumber(), m_analog->GetChannel(), this);
}
/**
* Gyro constructor given a slot and a channel.
*
* @param moduleNumber The analog module the gyro is connected to (1).
* @param channel The analog channel the gyro is connected to (1 or 2).
*/
Gyro::Gyro(uint8_t moduleNumber, uint32_t channel)
{
m_analog = new AnalogChannel(moduleNumber, channel);
m_channelAllocated = true;
InitGyro();
}
/**
* Gyro constructor with only a channel.
*
* Use the default analog module slot.
*
* @param channel The analog channel the gyro is connected to.
*/
Gyro::Gyro(uint32_t channel)
{
m_analog = new AnalogChannel(channel);
m_channelAllocated = true;
InitGyro();
}
/**
* Gyro constructor with a precreated analog channel object.
* Use this constructor when the analog channel needs to be shared. There
* is no reference counting when an AnalogChannel is passed to the gyro.
* @param channel The AnalogChannel object that the gyro is connected to.
*/
Gyro::Gyro(AnalogChannel *channel)
{
m_analog = channel;
m_channelAllocated = false;
if (channel == NULL)
{
wpi_setWPIError(NullParameter);
}
else
{
InitGyro();
}
}
Gyro::Gyro(AnalogChannel &channel)
{
m_analog = &channel;
m_channelAllocated = false;
InitGyro();
}
/**
* 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 Gyro::Reset()
{
m_analog->ResetAccumulator();
}
/**
* Delete (free) the accumulator and the analog components used for the gyro.
*/
Gyro::~Gyro()
{
if (m_channelAllocated)
delete m_analog;
}
/**
* 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 can go beyond 360 degrees. This make algorithms that wouldn't
* want to see a discontinuity in the gyro output as it sweeps past 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.
*/
float Gyro::GetAngle( void )
{
INT64 rawValue;
uint32_t count;
m_analog->GetAccumulatorOutput(&rawValue, &count);
INT64 value = rawValue - (INT64)((float)count * m_offset);
double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() * (double)(1 << m_analog->GetAverageBits()) /
(m_analog->GetModule()->GetSampleRate() * m_voltsPerDegreePerSecond);
return (float)scaledValue;
}
/**
* 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 Gyro::GetRate( void )
{
return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 * m_analog->GetLSBWeight()
/ ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
}
/**
* Set the gyro type based on the 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.
*
* @param voltsPerDegreePerSecond The type of gyro specified as the voltage that represents one degree/second.
*/
void Gyro::SetSensitivity( float voltsPerDegreePerSecond )
{
m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
}
void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
{
if(pidSource == 0 || pidSource > 2)
wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
m_pidSource = pidSource;
}
/**
* Get the angle in degrees for the PIDSource base object.
*
* @return The angle in degrees.
*/
double Gyro::PIDGet()
{
switch(m_pidSource){
case kRate:
return GetRate();
case kAngle:
return GetAngle();
default:
return 0;
}
}
void Gyro::UpdateTable() {
if (m_table != NULL) {
m_table->PutNumber("Value", GetAngle());
}
}
void Gyro::StartLiveWindowMode() {
}
void Gyro::StopLiveWindowMode() {
}
std::string Gyro::GetSmartDashboardType() {
return "Gyro";
}
void Gyro::InitTable(ITable *subTable) {
m_table = subTable;
UpdateTable();
}
ITable * Gyro::GetTable() {
return m_table;
}