frc971/wpilib/encoder_and_potentiometer.h - RealtimeRoboticsGroup/test - Gitiles

 #ifndef FRC971_ENCODER_AND_POTENTIOMETER_H_
 #define FRC971_ENCODER_AND_POTENTIOMETER_H_

 #include <atomic>
 #include <cmath>
 #include <thread>

 #include "aos/macros.h"
 #include "aos/time/time.h"
 #include "frc971/wpilib/ahal/AnalogInput.h"
 #include "frc971/wpilib/ahal/Counter.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/DigitalSource.h"
 #include "frc971/wpilib/ahal/Encoder.h"
 #include "frc971/wpilib/dma.h"
 #include "frc971/wpilib/dma_edge_counting.h"

 namespace frc971::wpilib {

 // Latches values from an encoder on positive edges from another input using
 // DMA.
 class DMAEncoder : public DMASampleHandlerInterface {
  public:
   DMAEncoder() {}

   void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
     encoder_ = ::std::move(encoder);
   }
   frc::Encoder *encoder() const { return encoder_.get(); }

   void set_index(::std::unique_ptr<frc::DigitalSource> index) {
     index_ = ::std::move(index);
   }
   frc::DigitalSource *index() const { return index_.get(); }

   // Returns the most recent polled value of the encoder.
   uint32_t polled_encoder_value() const { return polled_encoder_value_; }

   // Returns the number of poseges that have happened on the index input.
   uint32_t index_posedge_count() const { return index_posedge_count_; }
   // Returns the value of the encoder at the last index posedge.
   int32_t last_encoder_value() const { return last_encoder_value_; }

   void UpdateFromSample(const DMASample &sample) override {
     DoUpdateFromSample(sample);
   }

   void PollFromSample(const DMASample &sample) override {
     polled_encoder_value_ = sample.GetRaw(encoder_.get());
   }

   void UpdatePolledValue() override {
     polled_encoder_value_ = encoder_->GetRaw();
   }

   void AddToDMA(DMA *dma) override {
     dma->Add(encoder_.get());
     dma->Add(index_.get());
     dma->SetExternalTrigger(index_.get(), true, true);
   }

  protected:
   // The same as UpdateFromSample except also returns true if this sample is a
   // new edge on the index.
   bool DoUpdateFromSample(const DMASample &sample);

  private:
   ::std::unique_ptr<frc::Encoder> encoder_;
   ::std::unique_ptr<frc::DigitalSource> index_;

   int32_t polled_encoder_value_ = 0;

   int32_t last_encoder_value_ = 0;

   uint32_t index_posedge_count_ = 0;

   // Whether or not it was triggered in the last sample.
   bool index_last_value_ = false;

   DISALLOW_COPY_AND_ASSIGN(DMAEncoder);
 };

 // Latches values from an encoder and potentiometer on positive edges from
 // another input using DMA.
 class DMAEncoderAndPotentiometer : public DMAEncoder {
  public:
   DMAEncoderAndPotentiometer() {}

   void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
     potentiometer_ = ::std::move(potentiometer);
   }
   frc::AnalogInput *potentiometer() const { return potentiometer_.get(); }

   // Returns the most recent polled voltage of the potentiometer.
   float polled_potentiometer_voltage() const {
     return polled_potentiometer_voltage_;
   }

   // Returns the voltage of the potentiometer at the last index posedge.
   float last_potentiometer_voltage() const {
     return last_potentiometer_voltage_;
   }

   void UpdateFromSample(const DMASample &sample) override {
     if (DMAEncoder::DoUpdateFromSample(sample)) {
       last_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
     }
   }

   void PollFromSample(const DMASample &sample) override {
     polled_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
     DMAEncoder::PollFromSample(sample);
   }

   void UpdatePolledValue() override {
     polled_potentiometer_voltage_ = potentiometer_->GetVoltage();
     DMAEncoder::UpdatePolledValue();
   }

   void AddToDMA(DMA *dma) override {
     dma->Add(potentiometer_.get());
     DMAEncoder::AddToDMA(dma);
   }

  private:
   ::std::unique_ptr<frc::AnalogInput> potentiometer_;

   float polled_potentiometer_voltage_ = 0.0f;

   float last_potentiometer_voltage_ = 0.0f;

   DISALLOW_COPY_AND_ASSIGN(DMAEncoderAndPotentiometer);
 };

 // Class to read duty cycle of an input.  This is tuned for the CTRE encoder's
 // absolute position output.
 class DutyCycleReader {
  public:
   // Configure the reader to use the provided digital input.
   void set_input(::std::unique_ptr<::frc::DigitalInput> input) {
     high_counter_.reset(new ::frc::Counter(input.get()));
     high_counter_->SetMaxPeriod(kMaxPeriod);
     high_counter_->SetSemiPeriodMode(true);

     period_length_counter_.reset(new ::frc::Counter(input.get()));
     period_length_counter_->SetMaxPeriod(kMaxPeriod);
     period_length_counter_->SetUpSourceEdge(true, false);

     input_ = ::std::move(input);
   }

   // Returns the last duty cycle or nan if the signal is stale.
   double Read() const {
     const double high_time = high_counter_->GetPeriod();
     const double period_length = period_length_counter_->GetPeriod();
     if (!::std::isfinite(high_time) || !::std::isfinite(period_length)) {
       return ::std::numeric_limits<double>::quiet_NaN();
     }
     return high_time / period_length;
   }

  private:
   static constexpr ::std::chrono::nanoseconds kNominalPeriod =
       ::std::chrono::microseconds(4096);
   static constexpr double kMaxPeriod =
       ::aos::time::DurationInSeconds(kNominalPeriod * 2);

   ::std::unique_ptr<::frc::Counter> high_counter_, period_length_counter_;
   ::std::unique_ptr<::frc::DigitalInput> input_;
 };

 class DMAAbsoluteEncoderAndPotentiometer {
  public:
   void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
     duty_cycle_input_ = ::std::move(input);
     duty_cycle_reader_.set_input(duty_cycle_input_.get());
   }

   void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
     encoder_ = ::std::move(encoder);
   }

   void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
     potentiometer_ = ::std::move(potentiometer);
   }

   double ReadAbsoluteEncoder() const {
     return duty_cycle_reader_.last_width() / duty_cycle_reader_.last_period();
   }
   int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
   double ReadPotentiometerVoltage() const {
     return potentiometer_->GetVoltage();
   }

   DMAPulseWidthReader &reader() { return duty_cycle_reader_; }

  private:
   DMAPulseWidthReader duty_cycle_reader_;
   ::std::unique_ptr<::frc::DigitalInput> duty_cycle_input_;
   ::std::unique_ptr<frc::Encoder> encoder_;
   ::std::unique_ptr<frc::AnalogInput> potentiometer_;
 };

 // Class to hold a CTRE encoder with absolute angle pwm and potentiometer pair.
 class AbsoluteEncoderAndPotentiometer {
  public:
   void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
     duty_cycle_.set_input(::std::move(input));
   }

   void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
     encoder_ = ::std::move(encoder);
   }

   void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
     potentiometer_ = ::std::move(potentiometer);
   }

   double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
   int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
   double ReadPotentiometerVoltage() const {
     return potentiometer_->GetVoltage();
   }

  private:
   DutyCycleReader duty_cycle_;
   ::std::unique_ptr<frc::Encoder> encoder_;
   ::std::unique_ptr<frc::AnalogInput> potentiometer_;
 };

 // Class to hold two CTRE encoders, one with both index pulses and absolute
 // angle pwm, and another that can only turn once and only reports absolute
 // angle pwm.
 class AbsoluteAndAbsoluteEncoder {
  public:
   void set_single_turn_absolute_pwm(
       ::std::unique_ptr<frc::DigitalInput> input) {
     single_turn_duty_cycle_.set_input(::std::move(input));
   }

   void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
     duty_cycle_.set_input(::std::move(input));
   }

   void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
     encoder_ = ::std::move(encoder);
   }

   double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
   double ReadSingleTurnAbsoluteEncoder() const {
     return single_turn_duty_cycle_.Read();
   }
   int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }

  private:
   DutyCycleReader duty_cycle_;
   DutyCycleReader single_turn_duty_cycle_;
   ::std::unique_ptr<frc::Encoder> encoder_;
 };

 class AbsoluteEncoder {
  public:
   void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
     duty_cycle_.set_input(::std::move(input));
   }

   void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
     encoder_ = ::std::move(encoder);
   }

   double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
   int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }

  private:
   DutyCycleReader duty_cycle_;
   ::std::unique_ptr<frc::Encoder> encoder_;
 };

 }  // namespace frc971::wpilib

 #endif  // FRC971_ENCODER_AND_POTENTIOMETER_H_
	#ifndef FRC971_ENCODER_AND_POTENTIOMETER_H_
	#define FRC971_ENCODER_AND_POTENTIOMETER_H_

	#include <atomic>
	#include <cmath>
	#include <thread>

	#include "aos/macros.h"
	#include "aos/time/time.h"
	#include "frc971/wpilib/ahal/AnalogInput.h"
	#include "frc971/wpilib/ahal/Counter.h"
	#include "frc971/wpilib/ahal/DigitalInput.h"
	#include "frc971/wpilib/ahal/DigitalSource.h"
	#include "frc971/wpilib/ahal/Encoder.h"
	#include "frc971/wpilib/dma.h"
	#include "frc971/wpilib/dma_edge_counting.h"

	namespace frc971::wpilib {

	// Latches values from an encoder on positive edges from another input using
	// DMA.
	class DMAEncoder : public DMASampleHandlerInterface {
	public:
	DMAEncoder() {}

	void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
	encoder_ = ::std::move(encoder);
	}
	frc::Encoder *encoder() const { return encoder_.get(); }

	void set_index(::std::unique_ptr<frc::DigitalSource> index) {
	index_ = ::std::move(index);
	}
	frc::DigitalSource *index() const { return index_.get(); }

	// Returns the most recent polled value of the encoder.
	uint32_t polled_encoder_value() const { return polled_encoder_value_; }

	// Returns the number of poseges that have happened on the index input.
	uint32_t index_posedge_count() const { return index_posedge_count_; }
	// Returns the value of the encoder at the last index posedge.
	int32_t last_encoder_value() const { return last_encoder_value_; }

	void UpdateFromSample(const DMASample &sample) override {
	DoUpdateFromSample(sample);
	}

	void PollFromSample(const DMASample &sample) override {
	polled_encoder_value_ = sample.GetRaw(encoder_.get());
	}

	void UpdatePolledValue() override {
	polled_encoder_value_ = encoder_->GetRaw();
	}

	void AddToDMA(DMA *dma) override {
	dma->Add(encoder_.get());
	dma->Add(index_.get());
	dma->SetExternalTrigger(index_.get(), true, true);
	}

	protected:
	// The same as UpdateFromSample except also returns true if this sample is a
	// new edge on the index.
	bool DoUpdateFromSample(const DMASample &sample);

	private:
	::std::unique_ptr<frc::Encoder> encoder_;
	::std::unique_ptr<frc::DigitalSource> index_;

	int32_t polled_encoder_value_ = 0;

	int32_t last_encoder_value_ = 0;

	uint32_t index_posedge_count_ = 0;

	// Whether or not it was triggered in the last sample.
	bool index_last_value_ = false;

	DISALLOW_COPY_AND_ASSIGN(DMAEncoder);
	};

	// Latches values from an encoder and potentiometer on positive edges from
	// another input using DMA.
	class DMAEncoderAndPotentiometer : public DMAEncoder {
	public:
	DMAEncoderAndPotentiometer() {}

	void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
	potentiometer_ = ::std::move(potentiometer);
	}
	frc::AnalogInput *potentiometer() const { return potentiometer_.get(); }

	// Returns the most recent polled voltage of the potentiometer.
	float polled_potentiometer_voltage() const {
	return polled_potentiometer_voltage_;
	}

	// Returns the voltage of the potentiometer at the last index posedge.
	float last_potentiometer_voltage() const {
	return last_potentiometer_voltage_;
	}

	void UpdateFromSample(const DMASample &sample) override {
	if (DMAEncoder::DoUpdateFromSample(sample)) {
	last_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
	}
	}

	void PollFromSample(const DMASample &sample) override {
	polled_potentiometer_voltage_ = sample.GetVoltage(potentiometer_.get());
	DMAEncoder::PollFromSample(sample);
	}

	void UpdatePolledValue() override {
	polled_potentiometer_voltage_ = potentiometer_->GetVoltage();
	DMAEncoder::UpdatePolledValue();
	}

	void AddToDMA(DMA *dma) override {
	dma->Add(potentiometer_.get());
	DMAEncoder::AddToDMA(dma);
	}

	private:
	::std::unique_ptr<frc::AnalogInput> potentiometer_;

	float polled_potentiometer_voltage_ = 0.0f;

	float last_potentiometer_voltage_ = 0.0f;

	DISALLOW_COPY_AND_ASSIGN(DMAEncoderAndPotentiometer);
	};

	// Class to read duty cycle of an input. This is tuned for the CTRE encoder's
	// absolute position output.
	class DutyCycleReader {
	public:
	// Configure the reader to use the provided digital input.
	void set_input(::std::unique_ptr<::frc::DigitalInput> input) {
	high_counter_.reset(new ::frc::Counter(input.get()));
	high_counter_->SetMaxPeriod(kMaxPeriod);
	high_counter_->SetSemiPeriodMode(true);

	period_length_counter_.reset(new ::frc::Counter(input.get()));
	period_length_counter_->SetMaxPeriod(kMaxPeriod);
	period_length_counter_->SetUpSourceEdge(true, false);

	input_ = ::std::move(input);
	}

	// Returns the last duty cycle or nan if the signal is stale.
	double Read() const {
	const double high_time = high_counter_->GetPeriod();
	const double period_length = period_length_counter_->GetPeriod();
	if (!::std::isfinite(high_time) \|\| !::std::isfinite(period_length)) {
	return ::std::numeric_limits<double>::quiet_NaN();
	}
	return high_time / period_length;
	}

	private:
	static constexpr ::std::chrono::nanoseconds kNominalPeriod =
	::std::chrono::microseconds(4096);
	static constexpr double kMaxPeriod =
	::aos::time::DurationInSeconds(kNominalPeriod * 2);

	::std::unique_ptr<::frc::Counter> high_counter_, period_length_counter_;
	::std::unique_ptr<::frc::DigitalInput> input_;
	};

	class DMAAbsoluteEncoderAndPotentiometer {
	public:
	void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
	duty_cycle_input_ = ::std::move(input);
	duty_cycle_reader_.set_input(duty_cycle_input_.get());
	}

	void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
	encoder_ = ::std::move(encoder);
	}

	void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
	potentiometer_ = ::std::move(potentiometer);
	}

	double ReadAbsoluteEncoder() const {
	return duty_cycle_reader_.last_width() / duty_cycle_reader_.last_period();
	}
	int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
	double ReadPotentiometerVoltage() const {
	return potentiometer_->GetVoltage();
	}

	DMAPulseWidthReader &reader() { return duty_cycle_reader_; }

	private:
	DMAPulseWidthReader duty_cycle_reader_;
	::std::unique_ptr<::frc::DigitalInput> duty_cycle_input_;
	::std::unique_ptr<frc::Encoder> encoder_;
	::std::unique_ptr<frc::AnalogInput> potentiometer_;
	};

	// Class to hold a CTRE encoder with absolute angle pwm and potentiometer pair.
	class AbsoluteEncoderAndPotentiometer {
	public:
	void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
	duty_cycle_.set_input(::std::move(input));
	}

	void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
	encoder_ = ::std::move(encoder);
	}

	void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
	potentiometer_ = ::std::move(potentiometer);
	}

	double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
	int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
	double ReadPotentiometerVoltage() const {
	return potentiometer_->GetVoltage();
	}

	private:
	DutyCycleReader duty_cycle_;
	::std::unique_ptr<frc::Encoder> encoder_;
	::std::unique_ptr<frc::AnalogInput> potentiometer_;
	};

	// Class to hold two CTRE encoders, one with both index pulses and absolute
	// angle pwm, and another that can only turn once and only reports absolute
	// angle pwm.
	class AbsoluteAndAbsoluteEncoder {
	public:
	void set_single_turn_absolute_pwm(
	::std::unique_ptr<frc::DigitalInput> input) {
	single_turn_duty_cycle_.set_input(::std::move(input));
	}

	void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
	duty_cycle_.set_input(::std::move(input));
	}

	void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
	encoder_ = ::std::move(encoder);
	}

	double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
	double ReadSingleTurnAbsoluteEncoder() const {
	return single_turn_duty_cycle_.Read();
	}
	int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }

	private:
	DutyCycleReader duty_cycle_;
	DutyCycleReader single_turn_duty_cycle_;
	::std::unique_ptr<frc::Encoder> encoder_;
	};

	class AbsoluteEncoder {
	public:
	void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
	duty_cycle_.set_input(::std::move(input));
	}

	void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
	encoder_ = ::std::move(encoder);
	}

	double ReadAbsoluteEncoder() const { return duty_cycle_.Read(); }
	int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }

	private:
	DutyCycleReader duty_cycle_;
	::std::unique_ptr<frc::Encoder> encoder_;
	};

	} // namespace frc971::wpilib

	#endif // FRC971_ENCODER_AND_POTENTIOMETER_H_