Added Drivetrain Writer.
Moved drivetrain writer out of year specific, started with changing
2019.
Change-Id: Id4738f4fe04ec390fb5d0a983a5597f1064b04db
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
new file mode 100644
index 0000000..61fa0ff
--- /dev/null
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -0,0 +1,53 @@
+#ifndef FRC971_WPILIB_DRIVETRAIN_WRITER_H_
+#define FRC971_WPILIB_DRIVETRAIN_WRITER_H_
+
+#include "aos/commonmath.h"
+
+#include "frc971/wpilib/ahal/PWM.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller0_ = ::std::move(t);
+ reversed_left0_ = reversed;
+ }
+
+ void set_right_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ right_controller0_ = ::std::move(t);
+ reversed_right0_ = reversed;
+ }
+
+ void set_left_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ left_controller1_ = ::std::move(t);
+ reversed_left1_ = reversed;
+ }
+
+ void set_right_controller1(::std::unique_ptr<::frc::PWM> t, bool reversed) {
+ right_controller1_ = ::std::move(t);
+ reversed_right1_ = reversed;
+ }
+
+ private:
+ void Write() override;
+ void Read() override;
+ void Stop() override;
+
+ double SafeSpeed(bool reversed, double voltage) {
+ return (::aos::Clip((reversed ? -1.0 : 1.0) * voltage, -12.0, 12.0) /
+ 12.0);
+ }
+
+ ::std::unique_ptr<::frc::PWM> left_controller0_, right_controller0_,
+ left_controller1_, right_controller1_;
+
+ bool reversed_right0_, reversed_left0_, reversed_right1_, reversed_left1_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_DRIVETRAIN_WRITER_H_