Added basic drivetrain encoders to SensorReader.
Moved drivetrain out of year specific, shifters will still have to be
year specific.
Change-Id: I5fc2418b2b741432299792186a171de96f54dc15
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 38989a4..24d33f8 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -10,6 +10,20 @@
SensorReader::SensorReader() {}
+void SensorReader::set_drivetrain_left_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_left_encoder_ = ::std::move(encoder);
+}
+
+void SensorReader::set_drivetrain_right_encoder(
+ ::std::unique_ptr<frc::Encoder> encoder) {
+ fast_encoder_filter_.Add(encoder.get());
+ drivetrain_right_encoder_ = ::std::move(encoder);
+}
+
+// All of the DMA-related set_* calls must be made before this, and it
+// doesn't hurt to do all of them.
void SensorReader::set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index a5c90ac..6453cef 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -21,13 +21,21 @@
public:
SensorReader();
- // All of the DMA-related set_* calls must be made before this, and it
- // doesn't hurt to do all of them.
+ // 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);
+
+ // Sets the dma.
void set_dma(::std::unique_ptr<DMA> dma);
+ // Sets the pwm trigger.
void set_pwm_trigger(::std::unique_ptr<frc::DigitalInput> pwm_trigger);
protected:
+ // Uses the pwm trigger to find the pwm cycle width and offset for that
+ // iteration.
void RunPWMDetecter();
::std::unique_ptr<frc::DigitalInput> pwm_trigger_;
@@ -44,6 +52,9 @@
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
::std::atomic<bool> run_{true};
+
+ ::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
+ drivetrain_right_encoder_;
};
} // namespace wpilib
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index b362487..2a422d9 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -116,18 +116,6 @@
hall_filter_.SetPeriodNanoSeconds(100000);
}
- // Left drivetrain side.
- void set_drivetrain_left_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- drivetrain_left_encoder_ = ::std::move(encoder);
- }
-
- // Right drivetrain side.
- void set_drivetrain_right_encoder(::std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- drivetrain_right_encoder_ = ::std::move(encoder);
- }
-
void operator()() {
::aos::SetCurrentThreadName("SensorReader");
@@ -198,12 +186,6 @@
void Quit() { run_ = false; }
private:
- 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);
- }
-
int32_t my_pid_;
::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,