Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 1 | /*----------------------------------------------------------------------------*/ |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 2 | /* Copyright (c) 2008-2020 FIRST. All Rights Reserved. */ |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 3 | /* Open Source Software - may be modified and shared by FRC teams. The code */ |
| 4 | /* must be accompanied by the FIRST BSD license file in the root directory of */ |
| 5 | /* the project. */ |
| 6 | /*----------------------------------------------------------------------------*/ |
| 7 | |
| 8 | #include "frc/Counter.h" |
| 9 | |
| 10 | #include <utility> |
| 11 | |
| 12 | #include <hal/Counter.h> |
| 13 | #include <hal/FRCUsageReporting.h> |
| 14 | |
| 15 | #include "frc/AnalogTrigger.h" |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 16 | #include "frc/Base.h" |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 17 | #include "frc/DigitalInput.h" |
| 18 | #include "frc/WPIErrors.h" |
| 19 | #include "frc/smartdashboard/SendableBuilder.h" |
| 20 | #include "frc/smartdashboard/SendableRegistry.h" |
| 21 | |
| 22 | using namespace frc; |
| 23 | |
| 24 | Counter::Counter(Mode mode) { |
| 25 | int32_t status = 0; |
| 26 | m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status); |
| 27 | wpi_setHALError(status); |
| 28 | |
| 29 | SetMaxPeriod(0.5); |
| 30 | |
| 31 | HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1); |
| 32 | SendableRegistry::GetInstance().AddLW(this, "Counter", m_index); |
| 33 | } |
| 34 | |
| 35 | Counter::Counter(int channel) : Counter(kTwoPulse) { |
| 36 | SetUpSource(channel); |
| 37 | ClearDownSource(); |
| 38 | } |
| 39 | |
| 40 | Counter::Counter(DigitalSource* source) : Counter(kTwoPulse) { |
| 41 | SetUpSource(source); |
| 42 | ClearDownSource(); |
| 43 | } |
| 44 | |
| 45 | Counter::Counter(std::shared_ptr<DigitalSource> source) : Counter(kTwoPulse) { |
| 46 | SetUpSource(source); |
| 47 | ClearDownSource(); |
| 48 | } |
| 49 | |
| 50 | Counter::Counter(const AnalogTrigger& trigger) : Counter(kTwoPulse) { |
| 51 | SetUpSource(trigger.CreateOutput(AnalogTriggerType::kState)); |
| 52 | ClearDownSource(); |
| 53 | } |
| 54 | |
| 55 | Counter::Counter(EncodingType encodingType, DigitalSource* upSource, |
| 56 | DigitalSource* downSource, bool inverted) |
| 57 | : Counter(encodingType, |
| 58 | std::shared_ptr<DigitalSource>(upSource, |
| 59 | NullDeleter<DigitalSource>()), |
| 60 | std::shared_ptr<DigitalSource>(downSource, |
| 61 | NullDeleter<DigitalSource>()), |
| 62 | inverted) {} |
| 63 | |
| 64 | Counter::Counter(EncodingType encodingType, |
| 65 | std::shared_ptr<DigitalSource> upSource, |
| 66 | std::shared_ptr<DigitalSource> downSource, bool inverted) |
| 67 | : Counter(kExternalDirection) { |
| 68 | if (encodingType != k1X && encodingType != k2X) { |
| 69 | wpi_setWPIErrorWithContext( |
| 70 | ParameterOutOfRange, |
| 71 | "Counter only supports 1X and 2X quadrature decoding."); |
| 72 | return; |
| 73 | } |
| 74 | SetUpSource(upSource); |
| 75 | SetDownSource(downSource); |
| 76 | int32_t status = 0; |
| 77 | |
| 78 | if (encodingType == k1X) { |
| 79 | SetUpSourceEdge(true, false); |
| 80 | HAL_SetCounterAverageSize(m_counter, 1, &status); |
| 81 | } else { |
| 82 | SetUpSourceEdge(true, true); |
| 83 | HAL_SetCounterAverageSize(m_counter, 2, &status); |
| 84 | } |
| 85 | |
| 86 | wpi_setHALError(status); |
| 87 | SetDownSourceEdge(inverted, true); |
| 88 | } |
| 89 | |
| 90 | Counter::~Counter() { |
| 91 | SetUpdateWhenEmpty(true); |
| 92 | |
| 93 | int32_t status = 0; |
| 94 | HAL_FreeCounter(m_counter, &status); |
| 95 | wpi_setHALError(status); |
| 96 | } |
| 97 | |
| 98 | void Counter::SetUpSource(int channel) { |
| 99 | if (StatusIsFatal()) return; |
| 100 | SetUpSource(std::make_shared<DigitalInput>(channel)); |
| 101 | SendableRegistry::GetInstance().AddChild(this, m_upSource.get()); |
| 102 | } |
| 103 | |
| 104 | void Counter::SetUpSource(AnalogTrigger* analogTrigger, |
| 105 | AnalogTriggerType triggerType) { |
| 106 | SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger, |
| 107 | NullDeleter<AnalogTrigger>()), |
| 108 | triggerType); |
| 109 | } |
| 110 | |
| 111 | void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger, |
| 112 | AnalogTriggerType triggerType) { |
| 113 | if (StatusIsFatal()) return; |
| 114 | SetUpSource(analogTrigger->CreateOutput(triggerType)); |
| 115 | } |
| 116 | |
| 117 | void Counter::SetUpSource(DigitalSource* source) { |
| 118 | SetUpSource( |
| 119 | std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>())); |
| 120 | } |
| 121 | |
| 122 | void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) { |
| 123 | if (StatusIsFatal()) return; |
| 124 | m_upSource = source; |
| 125 | if (m_upSource->StatusIsFatal()) { |
| 126 | CloneError(*m_upSource); |
| 127 | } else { |
| 128 | int32_t status = 0; |
| 129 | HAL_SetCounterUpSource( |
| 130 | m_counter, source->GetPortHandleForRouting(), |
| 131 | (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(), |
| 132 | &status); |
| 133 | wpi_setHALError(status); |
| 134 | } |
| 135 | } |
| 136 | |
| 137 | void Counter::SetUpSource(DigitalSource& source) { |
| 138 | SetUpSource( |
| 139 | std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>())); |
| 140 | } |
| 141 | |
| 142 | void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) { |
| 143 | if (StatusIsFatal()) return; |
| 144 | if (m_upSource == nullptr) { |
| 145 | wpi_setWPIErrorWithContext( |
| 146 | NullParameter, |
| 147 | "Must set non-nullptr UpSource before setting UpSourceEdge"); |
| 148 | } |
| 149 | int32_t status = 0; |
| 150 | HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status); |
| 151 | wpi_setHALError(status); |
| 152 | } |
| 153 | |
| 154 | void Counter::ClearUpSource() { |
| 155 | if (StatusIsFatal()) return; |
| 156 | m_upSource.reset(); |
| 157 | int32_t status = 0; |
| 158 | HAL_ClearCounterUpSource(m_counter, &status); |
| 159 | wpi_setHALError(status); |
| 160 | } |
| 161 | |
| 162 | void Counter::SetDownSource(int channel) { |
| 163 | if (StatusIsFatal()) return; |
| 164 | SetDownSource(std::make_shared<DigitalInput>(channel)); |
| 165 | SendableRegistry::GetInstance().AddChild(this, m_downSource.get()); |
| 166 | } |
| 167 | |
| 168 | void Counter::SetDownSource(AnalogTrigger* analogTrigger, |
| 169 | AnalogTriggerType triggerType) { |
| 170 | SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger, |
| 171 | NullDeleter<AnalogTrigger>()), |
| 172 | triggerType); |
| 173 | } |
| 174 | |
| 175 | void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger, |
| 176 | AnalogTriggerType triggerType) { |
| 177 | if (StatusIsFatal()) return; |
| 178 | SetDownSource(analogTrigger->CreateOutput(triggerType)); |
| 179 | } |
| 180 | |
| 181 | void Counter::SetDownSource(DigitalSource* source) { |
| 182 | SetDownSource( |
| 183 | std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>())); |
| 184 | } |
| 185 | |
| 186 | void Counter::SetDownSource(DigitalSource& source) { |
| 187 | SetDownSource( |
| 188 | std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>())); |
| 189 | } |
| 190 | |
| 191 | void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) { |
| 192 | if (StatusIsFatal()) return; |
| 193 | m_downSource = source; |
| 194 | if (m_downSource->StatusIsFatal()) { |
| 195 | CloneError(*m_downSource); |
| 196 | } else { |
| 197 | int32_t status = 0; |
| 198 | HAL_SetCounterDownSource( |
| 199 | m_counter, source->GetPortHandleForRouting(), |
| 200 | (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(), |
| 201 | &status); |
| 202 | wpi_setHALError(status); |
| 203 | } |
| 204 | } |
| 205 | |
| 206 | void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) { |
| 207 | if (StatusIsFatal()) return; |
| 208 | if (m_downSource == nullptr) { |
| 209 | wpi_setWPIErrorWithContext( |
| 210 | NullParameter, |
| 211 | "Must set non-nullptr DownSource before setting DownSourceEdge"); |
| 212 | } |
| 213 | int32_t status = 0; |
| 214 | HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status); |
| 215 | wpi_setHALError(status); |
| 216 | } |
| 217 | |
| 218 | void Counter::ClearDownSource() { |
| 219 | if (StatusIsFatal()) return; |
| 220 | m_downSource.reset(); |
| 221 | int32_t status = 0; |
| 222 | HAL_ClearCounterDownSource(m_counter, &status); |
| 223 | wpi_setHALError(status); |
| 224 | } |
| 225 | |
| 226 | void Counter::SetUpDownCounterMode() { |
| 227 | if (StatusIsFatal()) return; |
| 228 | int32_t status = 0; |
| 229 | HAL_SetCounterUpDownMode(m_counter, &status); |
| 230 | wpi_setHALError(status); |
| 231 | } |
| 232 | |
| 233 | void Counter::SetExternalDirectionMode() { |
| 234 | if (StatusIsFatal()) return; |
| 235 | int32_t status = 0; |
| 236 | HAL_SetCounterExternalDirectionMode(m_counter, &status); |
| 237 | wpi_setHALError(status); |
| 238 | } |
| 239 | |
| 240 | void Counter::SetSemiPeriodMode(bool highSemiPeriod) { |
| 241 | if (StatusIsFatal()) return; |
| 242 | int32_t status = 0; |
| 243 | HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status); |
| 244 | wpi_setHALError(status); |
| 245 | } |
| 246 | |
| 247 | void Counter::SetPulseLengthMode(double threshold) { |
| 248 | if (StatusIsFatal()) return; |
| 249 | int32_t status = 0; |
| 250 | HAL_SetCounterPulseLengthMode(m_counter, threshold, &status); |
| 251 | wpi_setHALError(status); |
| 252 | } |
| 253 | |
| 254 | void Counter::SetReverseDirection(bool reverseDirection) { |
| 255 | if (StatusIsFatal()) return; |
| 256 | int32_t status = 0; |
| 257 | HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status); |
| 258 | wpi_setHALError(status); |
| 259 | } |
| 260 | |
| 261 | void Counter::SetSamplesToAverage(int samplesToAverage) { |
| 262 | if (samplesToAverage < 1 || samplesToAverage > 127) { |
| 263 | wpi_setWPIErrorWithContext( |
| 264 | ParameterOutOfRange, |
| 265 | "Average counter values must be between 1 and 127"); |
| 266 | } |
| 267 | int32_t status = 0; |
| 268 | HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status); |
| 269 | wpi_setHALError(status); |
| 270 | } |
| 271 | |
| 272 | int Counter::GetSamplesToAverage() const { |
| 273 | int32_t status = 0; |
| 274 | int samples = HAL_GetCounterSamplesToAverage(m_counter, &status); |
| 275 | wpi_setHALError(status); |
| 276 | return samples; |
| 277 | } |
| 278 | |
| 279 | int Counter::GetFPGAIndex() const { return m_index; } |
| 280 | |
| 281 | int Counter::Get() const { |
| 282 | if (StatusIsFatal()) return 0; |
| 283 | int32_t status = 0; |
| 284 | int value = HAL_GetCounter(m_counter, &status); |
| 285 | wpi_setHALError(status); |
| 286 | return value; |
| 287 | } |
| 288 | |
| 289 | void Counter::Reset() { |
| 290 | if (StatusIsFatal()) return; |
| 291 | int32_t status = 0; |
| 292 | HAL_ResetCounter(m_counter, &status); |
| 293 | wpi_setHALError(status); |
| 294 | } |
| 295 | |
| 296 | double Counter::GetPeriod() const { |
| 297 | if (StatusIsFatal()) return 0.0; |
| 298 | int32_t status = 0; |
| 299 | double value = HAL_GetCounterPeriod(m_counter, &status); |
| 300 | wpi_setHALError(status); |
| 301 | return value; |
| 302 | } |
| 303 | |
| 304 | void Counter::SetMaxPeriod(double maxPeriod) { |
| 305 | if (StatusIsFatal()) return; |
| 306 | int32_t status = 0; |
| 307 | HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status); |
| 308 | wpi_setHALError(status); |
| 309 | } |
| 310 | |
| 311 | void Counter::SetUpdateWhenEmpty(bool enabled) { |
| 312 | if (StatusIsFatal()) return; |
| 313 | int32_t status = 0; |
| 314 | HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status); |
| 315 | wpi_setHALError(status); |
| 316 | } |
| 317 | |
| 318 | bool Counter::GetStopped() const { |
| 319 | if (StatusIsFatal()) return false; |
| 320 | int32_t status = 0; |
| 321 | bool value = HAL_GetCounterStopped(m_counter, &status); |
| 322 | wpi_setHALError(status); |
| 323 | return value; |
| 324 | } |
| 325 | |
| 326 | bool Counter::GetDirection() const { |
| 327 | if (StatusIsFatal()) return false; |
| 328 | int32_t status = 0; |
| 329 | bool value = HAL_GetCounterDirection(m_counter, &status); |
| 330 | wpi_setHALError(status); |
| 331 | return value; |
| 332 | } |
| 333 | |
| 334 | void Counter::InitSendable(SendableBuilder& builder) { |
| 335 | builder.SetSmartDashboardType("Counter"); |
Austin Schuh | 1e69f94 | 2020-11-14 15:06:14 -0800 | [diff] [blame^] | 336 | builder.AddDoubleProperty( |
| 337 | "Value", [=]() { return Get(); }, nullptr); |
Brian Silverman | 8fce748 | 2020-01-05 13:18:21 -0800 | [diff] [blame] | 338 | } |