Moves Encoders to Handles and Moves WPILib Encoders to HAL (#124)

hal/lib/athena/FPGAEncoder.cpp

@@ -0,0 +1,272 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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 the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "FPGAEncoder.h"
+
+#include "DigitalInternal.h"
+#include "handles/LimitedHandleResource.h"
+
+static_assert(sizeof(uint32_t) <= sizeof(void*),
+              "This file shoves uint32_ts into pointers.");
+
+using namespace hal;
+
+namespace {
+struct Encoder {
+  tEncoder* encoder;
+  uint32_t index;
+};
+}
+
+static const double DECODING_SCALING_FACTOR = 0.25;
+
+static LimitedHandleResource<HalFPGAEncoderHandle, Encoder,
+                             tEncoder::kNumSystems, HalHandleEnum::FPGAEncoder>
+    fpgaEncoderHandles;
+
+extern "C" {
+HalFPGAEncoderHandle initializeFPGAEncoder(
+    uint8_t port_a_module, uint32_t port_a_pin, bool port_a_analog_trigger,
+    uint8_t port_b_module, uint32_t port_b_pin, bool port_b_analog_trigger,
+    bool reverseDirection, int32_t* index, int32_t* status) {
+  remapDigitalSource(port_a_analog_trigger, port_a_pin, port_a_module);
+  remapDigitalSource(port_b_analog_trigger, port_b_pin, port_b_module);
+
+  auto handle = fpgaEncoderHandles.Allocate();
+  if (handle == HAL_INVALID_HANDLE) {  // out of resources
+    *status = NO_AVAILABLE_RESOURCES;
+    return HAL_INVALID_HANDLE;
+  }
+
+  auto encoder = fpgaEncoderHandles.Get(handle);
+  if (encoder == nullptr) {  // will only error on thread issue
+    *status = PARAMETER_OUT_OF_RANGE;
+    return HAL_INVALID_HANDLE;
+  }
+
+  encoder->index = static_cast<uint32_t>(getHandleIndex(handle));
+
+  *index = encoder->index;
+  // TODO: if (index == ~0ul) { CloneError(quadEncoders); return; }
+  encoder->encoder = tEncoder::create(encoder->index, status);
+  encoder->encoder->writeConfig_ASource_Module(port_a_module, status);
+  encoder->encoder->writeConfig_ASource_Channel(port_a_pin, status);
+  encoder->encoder->writeConfig_ASource_AnalogTrigger(port_a_analog_trigger,
+                                                      status);
+  encoder->encoder->writeConfig_BSource_Module(port_b_module, status);
+  encoder->encoder->writeConfig_BSource_Channel(port_b_pin, status);
+  encoder->encoder->writeConfig_BSource_AnalogTrigger(port_b_analog_trigger,
+                                                      status);
+  encoder->encoder->strobeReset(status);
+  encoder->encoder->writeConfig_Reverse(reverseDirection, status);
+  encoder->encoder->writeTimerConfig_AverageSize(4, status);
+
+  return handle;
+}
+
+void freeFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
+                     int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  fpgaEncoderHandles.Free(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  delete encoder->encoder;
+}
+
+/**
+ * Reset the Encoder distance to zero.
+ * Resets the current count to zero on the encoder.
+ */
+void resetFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
+                      int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  encoder->encoder->strobeReset(status);
+}
+
+/**
+ * Gets the fpga value from the encoder.
+ * The fpga value is the actual count unscaled by the 1x, 2x, or 4x scale
+ * factor.
+ * @return Current fpga count from the encoder
+ */
+int32_t getFPGAEncoder(HalFPGAEncoderHandle fpga_encoder_handle,
+                       int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return 0;
+  }
+  return encoder->encoder->readOutput_Value(status);
+}
+
+/**
+ * Returns the period of the most recent pulse.
+ * Returns the period of the most recent Encoder pulse in seconds.
+ * This method compenstates for the decoding type.
+ *
+ * @deprecated Use GetRate() in favor of this method.  This returns unscaled
+ * periods and GetRate() scales using value from SetDistancePerPulse().
+ *
+ * @return Period in seconds of the most recent pulse.
+ */
+double getFPGAEncoderPeriod(HalFPGAEncoderHandle fpga_encoder_handle,
+                            int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return 0.0;
+  }
+  tEncoder::tTimerOutput output = encoder->encoder->readTimerOutput(status);
+  double value;
+  if (output.Stalled) {
+    // Return infinity
+    double zero = 0.0;
+    value = 1.0 / zero;
+  } else {
+    // output.Period is a fixed point number that counts by 2 (24 bits, 25
+    // integer bits)
+    value = (double)(output.Period << 1) / (double)output.Count;
+  }
+  double measuredPeriod = value * 2.5e-8;
+  return measuredPeriod / DECODING_SCALING_FACTOR;
+}
+
+/**
+ * Sets the maximum period for stopped detection.
+ * Sets the value that represents the maximum period of the Encoder before it
+ * will assume that the attached device is stopped. This timeout allows users
+ * to determine if the wheels or other shaft has stopped rotating.
+ * This method compensates for the decoding type.
+ *
+ * @deprecated Use SetMinRate() in favor of this method.  This takes unscaled
+ * periods and SetMinRate() scales using value from SetDistancePerPulse().
+ *
+ * @param maxPeriod The maximum time between rising and falling edges before the
+ * FPGA will
+ * report the device stopped. This is expressed in seconds.
+ */
+void setFPGAEncoderMaxPeriod(HalFPGAEncoderHandle fpga_encoder_handle,
+                             double maxPeriod, int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  encoder->encoder->writeTimerConfig_StallPeriod(
+      (uint32_t)(maxPeriod * 4.0e8 * DECODING_SCALING_FACTOR), status);
+}
+
+/**
+ * Determine if the encoder is stopped.
+ * Using the MaxPeriod value, a boolean is returned that is true if the encoder
+ * is considered stopped and false if it is still moving. A stopped encoder is
+ * one where the most recent pulse width exceeds the MaxPeriod.
+ * @return True if the encoder is considered stopped.
+ */
+bool getFPGAEncoderStopped(HalFPGAEncoderHandle fpga_encoder_handle,
+                           int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return false;
+  }
+  return encoder->encoder->readTimerOutput_Stalled(status) != 0;
+}
+
+/**
+ * The last direction the encoder value changed.
+ * @return The last direction the encoder value changed.
+ */
+bool getFPGAEncoderDirection(HalFPGAEncoderHandle fpga_encoder_handle,
+                             int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return false;
+  }
+  return encoder->encoder->readOutput_Direction(status);
+}
+
+/**
+ * Set the direction sensing for this encoder.
+ * This sets the direction sensing on the encoder so that it could count in the
+ * correct software direction regardless of the mounting.
+ * @param reverseDirection true if the encoder direction should be reversed
+ */
+void setFPGAEncoderReverseDirection(HalFPGAEncoderHandle fpga_encoder_handle,
+                                    bool reverseDirection, int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  encoder->encoder->writeConfig_Reverse(reverseDirection, status);
+}
+
+/**
+ * Set the Samples to Average which specifies the number of samples of the timer
+ * to average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @param samplesToAverage The number of samples to average from 1 to 127.
+ */
+void setFPGAEncoderSamplesToAverage(HalFPGAEncoderHandle fpga_encoder_handle,
+                                    uint32_t samplesToAverage,
+                                    int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  if (samplesToAverage < 1 || samplesToAverage > 127) {
+    *status = PARAMETER_OUT_OF_RANGE;
+  }
+  encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status);
+}
+
+/**
+ * Get the Samples to Average which specifies the number of samples of the timer
+ * to average when calculating the period. Perform averaging to account for
+ * mechanical imperfections or as oversampling to increase resolution.
+ * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+ */
+uint32_t getFPGAEncoderSamplesToAverage(
+    HalFPGAEncoderHandle fpga_encoder_handle, int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return 0;
+  }
+  return encoder->encoder->readTimerConfig_AverageSize(status);
+}
+
+/**
+ * Set an index source for an encoder, which is an input that resets the
+ * encoder's count.
+ */
+void setFPGAEncoderIndexSource(HalFPGAEncoderHandle fpga_encoder_handle,
+                               uint32_t pin, bool analogTrigger,
+                               bool activeHigh, bool edgeSensitive,
+                               int32_t* status) {
+  auto encoder = fpgaEncoderHandles.Get(fpga_encoder_handle);
+  if (encoder == nullptr) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    return;
+  }
+  encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status);
+  encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status);
+  encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger,
+                                                          status);
+  encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
+  encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
+}
+}