Convert y2016 over to use the new SensorReader class
Another step in converting to event loops.
Change-Id: I1cb1f54d29f231376184856d85ca2d5b711e869f
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 27f9190..9cf14ad 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -289,11 +289,13 @@
deps = [
":dma",
":dma_edge_counting",
+ ":encoder_and_potentiometer",
":wpilib_interface",
"//aos:init",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:phased_loop",
+ "//frc971/control_loops:queues",
"//third_party:wpilib",
],
)
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 47ca95c..fb25360 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -6,10 +6,12 @@
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
+#include "frc971/control_loops/control_loops.q.h"
#include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
#include "frc971/wpilib/ahal/DigitalInput.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;
@@ -51,6 +53,77 @@
void operator()();
protected:
+ // Copies a DMAEncoder to a IndexPosition with the correct unit and direction
+ // changes.
+ void CopyPosition(const ::frc971::wpilib::DMAEncoder &encoder,
+ ::frc971::IndexPosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(encoder.polled_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->latched_encoder =
+ multiplier * encoder_translate(encoder.last_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->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::PotAndAbsolutePosition *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->pot = multiplier * potentiometer_translate(
+ encoder.ReadPotentiometerVoltage()) +
+ pot_offset;
+ position->encoder =
+ multiplier * encoder_translate(encoder.ReadRelativeEncoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+
+ position->absolute_encoder =
+ (reverse ? (1.0 - encoder.ReadAbsoluteEncoder())
+ : encoder.ReadAbsoluteEncoder()) *
+ 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::HallEffectAndPosition *position,
+ double encoder_counts_per_revolution, double encoder_ratio,
+ bool reverse) {
+ const double multiplier = reverse ? -1.0 : 1.0;
+ position->encoder =
+ multiplier * encoder_translate(counter.polled_encoder(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->current = !counter.polled_value();
+ position->posedge_count = counter.negative_count();
+ position->negedge_count = counter.positive_count();
+ position->posedge_value =
+ multiplier * encoder_translate(counter.last_negative_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ position->negedge_value =
+ multiplier * encoder_translate(counter.last_positive_encoder_value(),
+ encoder_counts_per_revolution,
+ encoder_ratio);
+ }
+
+ 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);
+ }
+
frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_;
::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
diff --git a/y2016/BUILD b/y2016/BUILD
index 92f0c86..9c9afeb 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -99,7 +99,7 @@
"//frc971/wpilib:logging_queue",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:pdp_fetcher",
- "//frc971/wpilib:wpilib_interface",
+ "//frc971/wpilib:sensor_reader",
"//frc971/wpilib:wpilib_robot_base",
"//third_party:wpilib",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index a4ea3e8..52d0b1b 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -30,31 +30,29 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2016/control_loops/shooter/shooter.q.h"
+#include "frc971/wpilib/ADIS16448.h"
+#include "frc971/wpilib/buffered_pcm.h"
+#include "frc971/wpilib/buffered_solenoid.h"
+#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/dma_edge_counting.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
+#include "frc971/wpilib/gyro_sender.h"
+#include "frc971/wpilib/interrupt_edge_counting.h"
+#include "frc971/wpilib/joystick_sender.h"
+#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/loop_output_handler.h"
+#include "frc971/wpilib/pdp_fetcher.h"
+#include "frc971/wpilib/sensor_reader.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
+#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
-#include "frc971/wpilib/joystick_sender.h"
-#include "frc971/wpilib/loop_output_handler.h"
-#include "frc971/wpilib/buffered_solenoid.h"
-#include "frc971/wpilib/buffered_pcm.h"
-#include "frc971/wpilib/gyro_sender.h"
-#include "frc971/wpilib/dma_edge_counting.h"
-#include "frc971/wpilib/interrupt_edge_counting.h"
-#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "frc971/wpilib/logging.q.h"
-#include "frc971/wpilib/wpilib_interface.h"
-#include "frc971/wpilib/pdp_fetcher.h"
-#include "frc971/wpilib/ADIS16448.h"
-#include "frc971/wpilib/dma.h"
-
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
@@ -150,33 +148,16 @@
512.0 /* CPR */ * 4 /* index pulse every quarter cycle */;
// Class to send position messages with sensor readings to our loops.
-class SensorReader {
+class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader() {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
- drivetrain_shooter_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxDrivetrainShooterEncoderPulsesPerSecond * 1e9 +
- 0.5));
- superstructure_encoder_filter_.SetPeriodNanoSeconds(
- static_cast<int>(1 / 4.0 /* built-in tolerance */ /
- kMaxSuperstructureEncoderPulsesPerSecond * 1e9 +
- 0.5));
+ UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
+ UpdateMediumEncoderFilterHz(kMaxSuperstructureEncoderPulsesPerSecond);
hall_filter_.SetPeriodNanoSeconds(100000);
}
- // Drivetrain setters.
- void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
- drivetrain_left_encoder_ = ::std::move(encoder);
- }
-
- void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
- drivetrain_right_encoder_ = ::std::move(encoder);
- }
-
void set_drivetrain_left_hall(::std::unique_ptr<AnalogInput> analog) {
drivetrain_left_hall_ = ::std::move(analog);
}
@@ -187,18 +168,18 @@
// Shooter setters.
void set_shooter_left_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
shooter_left_encoder_ = ::std::move(encoder);
}
void set_shooter_right_encoder(::std::unique_ptr<Encoder> encoder) {
- drivetrain_shooter_encoder_filter_.Add(encoder.get());
+ fast_encoder_filter_.Add(encoder.get());
shooter_right_encoder_ = ::std::move(encoder);
}
// Intake setters.
void set_intake_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
intake_encoder_.set_encoder(::std::move(encoder));
}
@@ -207,13 +188,13 @@
}
void set_intake_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
intake_encoder_.set_index(::std::move(index));
}
// Shoulder setters.
void set_shoulder_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
shoulder_encoder_.set_encoder(::std::move(encoder));
}
@@ -223,13 +204,13 @@
}
void set_shoulder_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
shoulder_encoder_.set_index(::std::move(index));
}
// Wrist setters.
void set_wrist_encoder(::std::unique_ptr<Encoder> encoder) {
- superstructure_encoder_filter_.Add(encoder.get());
+ medium_encoder_filter_.Add(encoder.get());
wrist_encoder_.set_encoder(::std::move(encoder));
}
@@ -238,7 +219,7 @@
}
void set_wrist_index(::std::unique_ptr<DigitalInput> index) {
- superstructure_encoder_filter_.Add(index.get());
+ medium_encoder_filter_.Add(index.get());
wrist_encoder_.set_index(::std::move(index));
}
@@ -256,41 +237,13 @@
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
- void set_dma(::std::unique_ptr<DMA> dma) {
- dma_synchronizer_.reset(
- new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
- dma_synchronizer_->Add(&intake_encoder_);
- dma_synchronizer_->Add(&shoulder_encoder_);
- dma_synchronizer_->Add(&wrist_encoder_);
- }
-
- void operator()() {
- ::aos::SetCurrentThreadName("SensorReader");
-
- my_pid_ = getpid();
-
- dma_synchronizer_->Start();
-
- ::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
- ::std::chrono::milliseconds(4));
-
- ::aos::SetCurrentThreadRealtimePriority(40);
- while (run_) {
- {
- const int iterations = phased_loop.SleepUntilNext();
- if (iterations != 1) {
- LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
- }
- }
- RunIteration();
- }
+ void Start() {
+ AddToDMA(&intake_encoder_);
+ AddToDMA(&shoulder_encoder_);
+ AddToDMA(&wrist_encoder_);
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_);
-
- const auto &values = constants::GetValues();
-
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
drivetrain_message->right_encoder =
@@ -309,9 +262,10 @@
drivetrain_message.Send();
}
+ }
- dma_synchronizer_->RunIteration();
-
+ void RunDmaIteration() {
+ const auto &values = constants::GetValues();
{
auto shooter_message = shooter_queue.position.MakeMessage();
shooter_message->theta_left =
@@ -358,8 +312,6 @@
}
}
- void Quit() { run_ = false; }
-
private:
void CopyPotAndIndexPosition(
const ::frc971::wpilib::DMAEncoderAndPotentiometer &encoder,
@@ -382,12 +334,6 @@
position->index_pulses = encoder.index_posedge_count();
}
- int32_t my_pid_;
-
- ::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
-
- ::std::unique_ptr<Encoder> drivetrain_left_encoder_,
- drivetrain_right_encoder_;
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
::std::unique_ptr<Encoder> shooter_left_encoder_, shooter_right_encoder_;
@@ -397,9 +343,7 @@
::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
- ::std::atomic<bool> run_{true};
- DigitalGlitchFilter drivetrain_shooter_encoder_filter_, hall_filter_,
- superstructure_encoder_filter_;
+ DigitalGlitchFilter hall_filter_;
};
class SolenoidWriter {