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

 #ifndef FRC971_WPILIB_SENSOR_READER_H_
 #define FRC971_WPILIB_SENSOR_READER_H_

 #include <atomic>
 #include <chrono>

 #include "aos/events/event_loop.h"
 #include "aos/events/shm_event_loop.h"
 #include "aos/stl_mutex/stl_mutex.h"
 #include "aos/time/time.h"
 #include "frc971/control_loops/control_loops_static.h"
 #include "frc971/control_loops/drivetrain/drivetrain_position_static.h"
 #include "frc971/input/robot_state_generated.h"
 #include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/DriverStation.h"
 #include "frc971/wpilib/dma.h"
 #include "frc971/wpilib/dma_edge_counting.h"
 #include "frc971/wpilib/encoder_and_potentiometer.h"

 using ::aos::monotonic_clock;
 namespace chrono = ::std::chrono;

 namespace frc971::wpilib {

 class SensorReader {
  public:
   SensorReader(::aos::ShmEventLoop *event_loop);
   virtual ~SensorReader() {}

   // Updates the fast and medium encoder filter frequencies.
   void UpdateFastEncoderFilterHz(int hz);
   void UpdateMediumEncoderFilterHz(int hz);

   // Sets the left drivetrain encoder.
   void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder);

   // Sets the right drivetrain encoder.
   void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder);

   // Adds a sensor to DMA.
   void AddToDMA(DMASampleHandlerInterface *handler) {
     if (!dma_synchronizer_) {
       dma_synchronizer_.reset(
           new ::frc971::wpilib::DMASynchronizer(std::make_unique<DMA>()));
     }
     dma_synchronizer_->Add(handler);
   }

   // Sets PWM trigger mode.  If true, synchronize the control loops with the PWM
   // pulses.  The sensors are sampled 50 uS after the falling edge of the PWM
   // pulse.
   void set_pwm_trigger(bool trigger) { pwm_trigger_ = trigger; }

   // Stops the pwm trigger on the next iteration.
   void Quit() { run_ = false; }

   virtual void RunIteration() = 0;
   // Runs the DMA iteration after the synchronizer.  This only gets run if a
   // sensor has been added to DMA.
   virtual void RunDmaIteration() {}

  protected:
   // Copies a DMAEncoder to a IndexPosition with the correct unit and direction
   // changes.
   void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
                     ::frc971::IndexPositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(encoder.polled_encoder_value(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));
     position->set_latched_encoder(
         multiplier * encoder_translate(encoder.last_encoder_value(),
                                        encoder_counts_per_revolution,
                                        encoder_ratio));
     position->set_index_pulses(encoder.index_posedge_count());
   }

   // Copies a AbsoluteEncoderAndPotentiometer to a PotAndAbsolutePosition with
   // the correct unit and direction changes.
   void CopyPosition(
       const ::frc971::wpilib::AbsoluteEncoderAndPotentiometer &encoder,
       ::frc971::PotAndAbsolutePositionStatic *position,
       double encoder_counts_per_revolution, double encoder_ratio,
       ::std::function<double(double)> potentiometer_translate, bool reverse,
       double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_pot(multiplier * potentiometer_translate(
                                        encoder.ReadPotentiometerVoltage()) +
                       pot_offset);
     position->set_encoder(multiplier *
                           encoder_translate(encoder.ReadRelativeEncoder(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));

     position->set_absolute_encoder((reverse
                                         ? (1.0 - encoder.ReadAbsoluteEncoder())
                                         : encoder.ReadAbsoluteEncoder()) *
                                    encoder_ratio * (2.0 * M_PI));
   }

   void CopyPosition(
       const ::frc971::wpilib::DMAAbsoluteEncoderAndPotentiometer &encoder,
       ::frc971::PotAndAbsolutePositionStatic *position,
       double encoder_counts_per_revolution, double encoder_ratio,
       ::std::function<double(double)> potentiometer_translate, bool reverse,
       double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_pot(multiplier * potentiometer_translate(
                                        encoder.ReadPotentiometerVoltage()) +
                       pot_offset);
     position->set_encoder(multiplier *
                           encoder_translate(encoder.ReadRelativeEncoder(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));

     position->set_absolute_encoder((reverse
                                         ? (1.0 - encoder.ReadAbsoluteEncoder())
                                         : encoder.ReadAbsoluteEncoder()) *
                                    encoder_ratio * (2.0 * M_PI));
   }

   // Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
   // with the correct unit and direction changes.
   void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
                     ::frc971::AbsoluteAndAbsolutePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     double single_turn_encoder_ratio, bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(encoder.ReadRelativeEncoder(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));

     position->set_absolute_encoder((reverse
                                         ? (1.0 - encoder.ReadAbsoluteEncoder())
                                         : encoder.ReadAbsoluteEncoder()) *
                                    encoder_ratio * (2.0 * M_PI));

     position->set_single_turn_absolute_encoder(
         (reverse ? (1.0 - encoder.ReadSingleTurnAbsoluteEncoder())
                  : encoder.ReadSingleTurnAbsoluteEncoder()) *
         single_turn_encoder_ratio * (2.0 * M_PI));
   }

   // Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
   // and direction changes.
   void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
                     ::frc971::HallEffectAndPositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(counter.polled_encoder(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));
     position->set_current(!counter.polled_value());
     position->set_posedge_count(counter.negative_count());
     position->set_negedge_count(counter.positive_count());
     position->set_posedge_value(
         multiplier * encoder_translate(counter.last_negative_encoder_value(),
                                        encoder_counts_per_revolution,
                                        encoder_ratio));
     position->set_negedge_value(
         multiplier * encoder_translate(counter.last_positive_encoder_value(),
                                        encoder_counts_per_revolution,
                                        encoder_ratio));
   }

   // Copies a Absolute Encoder with the correct unit
   // and direction changes.
   void CopyPosition(const ::frc971::wpilib::AbsoluteEncoder &encoder,
                     ::frc971::AbsolutePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(encoder.ReadRelativeEncoder(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));

     position->set_absolute_encoder((reverse
                                         ? (1.0 - encoder.ReadAbsoluteEncoder())
                                         : encoder.ReadAbsoluteEncoder()) *
                                    encoder_ratio * (2.0 * M_PI));
   }

   void CopyPosition(const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
                     ::frc971::PotAndIndexPositionStatic *position,
                     ::std::function<double(int32_t)> encoder_translate,
                     ::std::function<double(double)> potentiometer_translate,
                     bool reverse, double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(encoder.polled_encoder_value()));
     position->set_pot(multiplier * potentiometer_translate(
                                        encoder.polled_potentiometer_voltage()) +
                       pot_offset);
     position->set_latched_encoder(
         multiplier * encoder_translate(encoder.last_encoder_value()));
     position->set_latched_pot(
         multiplier *
             potentiometer_translate(encoder.last_potentiometer_voltage()) +
         pot_offset);
     position->set_index_pulses(encoder.index_posedge_count());
   }

   // Copies a relative digital encoder.
   void CopyPosition(const ::frc::Encoder &encoder,
                     ::frc971::RelativePositionStatic *position,
                     double encoder_counts_per_revolution, double encoder_ratio,
                     bool reverse) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(multiplier *
                           encoder_translate(encoder.GetRaw(),
                                             encoder_counts_per_revolution,
                                             encoder_ratio));
   }

   // Copies a potentiometer
   void CopyPosition(const ::frc::AnalogInput &input,
                     ::frc971::RelativePositionStatic *position,
                     ::std::function<double(double)> potentiometer_translate,
                     bool reverse, double pot_offset) {
     const double multiplier = reverse ? -1.0 : 1.0;
     position->set_encoder(
         multiplier * potentiometer_translate(input.GetVoltage()) + pot_offset);
   }

   double encoder_translate(int32_t value, double counts_per_revolution,
                            double ratio) {
     return static_cast<double>(value) / counts_per_revolution * ratio *
            (2.0 * M_PI);
   }

   void SendDrivetrainPosition(
       aos::Sender<control_loops::drivetrain::PositionStatic>::StaticBuilder
           builder,
       std::function<double(double input)> velocity_translate,
       std::function<double(double input)> encoder_to_meters, bool left_inverted,
       bool right_inverted);

   ::aos::EventLoop *event_loop_;
   ::aos::Sender<::aos::RobotState> robot_state_sender_;

   frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_;

   ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
       drivetrain_right_encoder_;

  private:
   // Gets called right before the DMA synchronizer is up and running.
   virtual void Start() {}

   // Sets up everything during startup.
   void DoStart();

   // Runs a single iteration.
   void Loop();

   // Returns the monotonic time of the start of the first PWM cycle.
   // Returns min_time if no start time could be calculated.
   monotonic_clock::time_point GetPWMStartTime();

   bool pwm_trigger_ = false;

   ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;

   ::std::atomic<bool> run_{true};
   ::frc::DriverStation *ds_;

   const int32_t my_pid_;

   // Pointer to the timer handler used to modify the wakeup.
   ::aos::TimerHandler *timer_handler_;

   // Last time we got called.
   ::aos::monotonic_clock::time_point last_monotonic_now_ =
       ::aos::monotonic_clock::min_time;
   // The current period.
   chrono::microseconds period_ = chrono::microseconds(5000);
 };

 }  // namespace frc971::wpilib

 #endif  // FRC971_WPILIB_SENSOR_READER_H_
	#ifndef FRC971_WPILIB_SENSOR_READER_H_
	#define FRC971_WPILIB_SENSOR_READER_H_

	#include <atomic>
	#include <chrono>

	#include "aos/events/event_loop.h"
	#include "aos/events/shm_event_loop.h"
	#include "aos/stl_mutex/stl_mutex.h"
	#include "aos/time/time.h"
	#include "frc971/control_loops/control_loops_static.h"
	#include "frc971/control_loops/drivetrain/drivetrain_position_static.h"
	#include "frc971/input/robot_state_generated.h"
	#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
	#include "frc971/wpilib/ahal/DigitalInput.h"
	#include "frc971/wpilib/ahal/DriverStation.h"
	#include "frc971/wpilib/dma.h"
	#include "frc971/wpilib/dma_edge_counting.h"
	#include "frc971/wpilib/encoder_and_potentiometer.h"

	using ::aos::monotonic_clock;
	namespace chrono = ::std::chrono;

	namespace frc971::wpilib {

	class SensorReader {
	public:
	SensorReader(::aos::ShmEventLoop *event_loop);
	virtual ~SensorReader() {}

	// Updates the fast and medium encoder filter frequencies.
	void UpdateFastEncoderFilterHz(int hz);
	void UpdateMediumEncoderFilterHz(int hz);

	// Sets the left drivetrain encoder.
	void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder);

	// Sets the right drivetrain encoder.
	void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder);

	// Adds a sensor to DMA.
	void AddToDMA(DMASampleHandlerInterface *handler) {
	if (!dma_synchronizer_) {
	dma_synchronizer_.reset(
	new ::frc971::wpilib::DMASynchronizer(std::make_unique<DMA>()));
	}
	dma_synchronizer_->Add(handler);
	}

	// Sets PWM trigger mode. If true, synchronize the control loops with the PWM
	// pulses. The sensors are sampled 50 uS after the falling edge of the PWM
	// pulse.
	void set_pwm_trigger(bool trigger) { pwm_trigger_ = trigger; }

	// Stops the pwm trigger on the next iteration.
	void Quit() { run_ = false; }

	virtual void RunIteration() = 0;
	// Runs the DMA iteration after the synchronizer. This only gets run if a
	// sensor has been added to DMA.
	virtual void RunDmaIteration() {}

	protected:
	// Copies a DMAEncoder to a IndexPosition with the correct unit and direction
	// changes.
	void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
	::frc971::IndexPositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	bool reverse) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(encoder.polled_encoder_value(),
	encoder_counts_per_revolution,
	encoder_ratio));
	position->set_latched_encoder(
	multiplier * encoder_translate(encoder.last_encoder_value(),
	encoder_counts_per_revolution,
	encoder_ratio));
	position->set_index_pulses(encoder.index_posedge_count());
	}

	// Copies a AbsoluteEncoderAndPotentiometer to a PotAndAbsolutePosition with
	// the correct unit and direction changes.
	void CopyPosition(
	const ::frc971::wpilib::AbsoluteEncoderAndPotentiometer &encoder,
	::frc971::PotAndAbsolutePositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	::std::function<double(double)> potentiometer_translate, bool reverse,
	double pot_offset) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_pot(multiplier * potentiometer_translate(
	encoder.ReadPotentiometerVoltage()) +
	pot_offset);
	position->set_encoder(multiplier *
	encoder_translate(encoder.ReadRelativeEncoder(),
	encoder_counts_per_revolution,
	encoder_ratio));

	position->set_absolute_encoder((reverse
	? (1.0 - encoder.ReadAbsoluteEncoder())
	: encoder.ReadAbsoluteEncoder()) *
	encoder_ratio * (2.0 * M_PI));
	}

	void CopyPosition(
	const ::frc971::wpilib::DMAAbsoluteEncoderAndPotentiometer &encoder,
	::frc971::PotAndAbsolutePositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	::std::function<double(double)> potentiometer_translate, bool reverse,
	double pot_offset) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_pot(multiplier * potentiometer_translate(
	encoder.ReadPotentiometerVoltage()) +
	pot_offset);
	position->set_encoder(multiplier *
	encoder_translate(encoder.ReadRelativeEncoder(),
	encoder_counts_per_revolution,
	encoder_ratio));

	position->set_absolute_encoder((reverse
	? (1.0 - encoder.ReadAbsoluteEncoder())
	: encoder.ReadAbsoluteEncoder()) *
	encoder_ratio * (2.0 * M_PI));
	}

	// Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
	// with the correct unit and direction changes.
	void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,
	::frc971::AbsoluteAndAbsolutePositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	double single_turn_encoder_ratio, bool reverse) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(encoder.ReadRelativeEncoder(),
	encoder_counts_per_revolution,
	encoder_ratio));

	position->set_absolute_encoder((reverse
	? (1.0 - encoder.ReadAbsoluteEncoder())
	: encoder.ReadAbsoluteEncoder()) *
	encoder_ratio * (2.0 * M_PI));

	position->set_single_turn_absolute_encoder(
	(reverse ? (1.0 - encoder.ReadSingleTurnAbsoluteEncoder())
	: encoder.ReadSingleTurnAbsoluteEncoder()) *
	single_turn_encoder_ratio * (2.0 * M_PI));
	}

	// Copies a DMAEdgeCounter to a HallEffectAndPosition with the correct unit
	// and direction changes.
	void CopyPosition(const ::frc971::wpilib::DMAEdgeCounter &counter,
	::frc971::HallEffectAndPositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	bool reverse) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(counter.polled_encoder(),
	encoder_counts_per_revolution,
	encoder_ratio));
	position->set_current(!counter.polled_value());
	position->set_posedge_count(counter.negative_count());
	position->set_negedge_count(counter.positive_count());
	position->set_posedge_value(
	multiplier * encoder_translate(counter.last_negative_encoder_value(),
	encoder_counts_per_revolution,
	encoder_ratio));
	position->set_negedge_value(
	multiplier * encoder_translate(counter.last_positive_encoder_value(),
	encoder_counts_per_revolution,
	encoder_ratio));
	}

	// Copies a Absolute Encoder with the correct unit
	// and direction changes.
	void CopyPosition(const ::frc971::wpilib::AbsoluteEncoder &encoder,
	::frc971::AbsolutePositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	bool reverse) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(encoder.ReadRelativeEncoder(),
	encoder_counts_per_revolution,
	encoder_ratio));

	position->set_absolute_encoder((reverse
	? (1.0 - encoder.ReadAbsoluteEncoder())
	: encoder.ReadAbsoluteEncoder()) *
	encoder_ratio * (2.0 * M_PI));
	}

	void CopyPosition(const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
	::frc971::PotAndIndexPositionStatic *position,
	::std::function<double(int32_t)> encoder_translate,
	::std::function<double(double)> potentiometer_translate,
	bool reverse, double pot_offset) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(encoder.polled_encoder_value()));
	position->set_pot(multiplier * potentiometer_translate(
	encoder.polled_potentiometer_voltage()) +
	pot_offset);
	position->set_latched_encoder(
	multiplier * encoder_translate(encoder.last_encoder_value()));
	position->set_latched_pot(
	multiplier *
	potentiometer_translate(encoder.last_potentiometer_voltage()) +
	pot_offset);
	position->set_index_pulses(encoder.index_posedge_count());
	}

	// Copies a relative digital encoder.
	void CopyPosition(const ::frc::Encoder &encoder,
	::frc971::RelativePositionStatic *position,
	double encoder_counts_per_revolution, double encoder_ratio,
	bool reverse) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(multiplier *
	encoder_translate(encoder.GetRaw(),
	encoder_counts_per_revolution,
	encoder_ratio));
	}

	// Copies a potentiometer
	void CopyPosition(const ::frc::AnalogInput &input,
	::frc971::RelativePositionStatic *position,
	::std::function<double(double)> potentiometer_translate,
	bool reverse, double pot_offset) {
	const double multiplier = reverse ? -1.0 : 1.0;
	position->set_encoder(
	multiplier * potentiometer_translate(input.GetVoltage()) + pot_offset);
	}

	double encoder_translate(int32_t value, double counts_per_revolution,
	double ratio) {
	return static_cast<double>(value) / counts_per_revolution * ratio *
	(2.0 * M_PI);
	}

	void SendDrivetrainPosition(
	aos::Sender<control_loops::drivetrain::PositionStatic>::StaticBuilder
	builder,
	std::function<double(double input)> velocity_translate,
	std::function<double(double input)> encoder_to_meters, bool left_inverted,
	bool right_inverted);

	::aos::EventLoop *event_loop_;
	::aos::Sender<::aos::RobotState> robot_state_sender_;

	frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_;

	::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
	drivetrain_right_encoder_;

	private:
	// Gets called right before the DMA synchronizer is up and running.
	virtual void Start() {}

	// Sets up everything during startup.
	void DoStart();

	// Runs a single iteration.
	void Loop();

	// Returns the monotonic time of the start of the first PWM cycle.
	// Returns min_time if no start time could be calculated.
	monotonic_clock::time_point GetPWMStartTime();

	bool pwm_trigger_ = false;

	::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;

	::std::atomic<bool> run_{true};
	::frc::DriverStation *ds_;

	const int32_t my_pid_;

	// Pointer to the timer handler used to modify the wakeup.
	::aos::TimerHandler *timer_handler_;

	// Last time we got called.
	::aos::monotonic_clock::time_point last_monotonic_now_ =
	::aos::monotonic_clock::min_time;
	// The current period.
	chrono::microseconds period_ = chrono::microseconds(5000);
	};

	} // namespace frc971::wpilib

	#endif // FRC971_WPILIB_SENSOR_READER_H_