Added Drivetrain Writer.
Moved drivetrain writer out of year specific, started with changing
2019.
Change-Id: Id4738f4fe04ec390fb5d0a983a5597f1064b04db
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 9cf14ad..c2707ec 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -299,3 +299,22 @@
"//third_party:wpilib",
],
)
+
+cc_library(
+ name = "drivetrain_writer",
+ srcs = [
+ "drivetrain_writer.cc",
+ ],
+ hdrs = [
+ "drivetrain_writer.h",
+ ],
+ restricted_to = ["//tools:roborio"],
+ deps = [
+ ":loop_output_handler",
+ "//aos:math",
+ "//aos/logging",
+ "//aos/logging:queue_logging",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//third_party:wpilib",
+ ],
+)
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
new file mode 100644
index 0000000..8d388dc
--- /dev/null
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -0,0 +1,48 @@
+#include "frc971/wpilib/drivetrain_writer.h"
+
+#include "aos/commonmath.h"
+#include "aos/logging/logging.h"
+#include "aos/logging/queue_logging.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/wpilib/ahal/PWM.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void DrivetrainWriter::Write() {
+ auto &queue = ::frc971::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, queue->left_voltage));
+ right_controller0_->SetSpeed(
+ SafeSpeed(reversed_right0_, queue->right_voltage));
+
+ if (left_controller1_) {
+ left_controller1_->SetSpeed(
+ SafeSpeed(reversed_left1_, queue->left_voltage));
+ }
+ if (right_controller1_) {
+ right_controller1_->SetSpeed(
+ SafeSpeed(reversed_right1_, queue->right_voltage));
+ }
+}
+
+void DrivetrainWriter::Read() {
+ ::frc971::control_loops::drivetrain_queue.output.FetchAnother();
+}
+
+void DrivetrainWriter::Stop() {
+ LOG(WARNING, "drivetrain output too old\n");
+ left_controller0_->SetDisabled();
+ right_controller0_->SetDisabled();
+
+ if (left_controller1_) {
+ left_controller1_->SetDisabled();
+ }
+ if (right_controller1_) {
+ right_controller1_->SetDisabled();
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
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_
diff --git a/y2019/BUILD b/y2019/BUILD
index 2b9d39b..9421a11 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -41,7 +41,7 @@
"//frc971/control_loops:queues",
"//frc971/control_loops/drivetrain:drivetrain_queue",
"//frc971/wpilib:ADIS16448",
- "//frc971/wpilib:buffered_pcm",
+ "//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 7a6a360..b216b4d 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -32,6 +32,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
#include "frc971/wpilib/dma.h"
+#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -128,40 +129,6 @@
}
};
-class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
- public:
- void set_drivetrain_left_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_left_victor_ = ::std::move(t);
- }
-
- void set_drivetrain_right_victor(::std::unique_ptr<::frc::VictorSP> t) {
- drivetrain_right_victor_ = ::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);
- drivetrain_left_victor_->SetSpeed(
- ::aos::Clip(queue->left_voltage, -12.0, 12.0) / 12.0);
- drivetrain_right_victor_->SetSpeed(
- ::aos::Clip(-queue->right_voltage, -12.0, 12.0) / 12.0);
- }
-
- virtual void Stop() override {
- LOG(WARNING, "drivetrain output too old\n");
- drivetrain_left_victor_->SetDisabled();
- drivetrain_right_victor_->SetDisabled();
- }
-
- ::std::unique_ptr<::frc::VictorSP> drivetrain_left_victor_,
- drivetrain_right_victor_;
-};
-
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
public:
::std::unique_ptr<frc::Encoder> make_encoder(int index) {
@@ -201,11 +168,11 @@
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- DrivetrainWriter drivetrain_writer;
- drivetrain_writer.set_drivetrain_left_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)));
- drivetrain_writer.set_drivetrain_right_victor(
- ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)));
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
+ drivetrain_writer.set_right_controller0(
+ ::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
// Wait forever. Not much else to do...