Moved DrivetrainWriter from y2014_bot3
Change-Id: I9d4cc47d06a50271a007bf543a2d9e3defe7a7c9
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 943d809..56d72f1 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/buffered_solenoid.h"
#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -205,39 +206,6 @@
::std::atomic<bool> run_{true};
};
-// Writes out drivetrain voltages.
-class DrivetrainWriter : public LoopOutputHandler {
- public:
- void set_left_drivetrain_talon(::std::unique_ptr<Talon> t) {
- left_drivetrain_talon_ = ::std::move(t);
- }
-
- void set_right_drivetrain_talon(::std::unique_ptr<Talon> t) {
- right_drivetrain_talon_ = ::std::move(t);
- }
-
- private:
- virtual void Read() override {
- ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
- }
-
- virtual void Write() override {
- auto &queue = ::frc971::control_loops::drivetrain_queue.output;
- LOG_STRUCT(DEBUG, "will output", *queue);
- left_drivetrain_talon_->SetSpeed(-queue->left_voltage / 12.0);
- right_drivetrain_talon_->SetSpeed(queue->right_voltage / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- left_drivetrain_talon_->SetDisabled();
- right_drivetrain_talon_->SetDisabled();
- }
-
- ::std::unique_ptr<Talon> left_drivetrain_talon_;
- ::std::unique_ptr<Talon> right_drivetrain_talon_;
-};
-
// Writes out rollers voltages.
class RollersWriter : public LoopOutputHandler {
public:
@@ -319,11 +287,11 @@
::std::thread gyro_thread(::std::ref(gyro_sender));
// Outputs
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_left_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(5)));
- drivetrain_writer.set_right_drivetrain_talon(
- ::std::unique_ptr<Talon>(new Talon(2)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<Talon>(new Talon(5)), true);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<Talon>(new Talon(2)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
RollersWriter rollers_writer;