Add code for prototyping with the 2012 drivebase
Change-Id: I16b5b2e9982f1911b410c25872eca7a00fa561f3
diff --git a/y2012/wpilib/wpilib_interface.cc b/y2012/wpilib/wpilib_interface.cc
new file mode 100644
index 0000000..337f5d2
--- /dev/null
+++ b/y2012/wpilib/wpilib_interface.cc
@@ -0,0 +1,367 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include <thread>
+#include <mutex>
+#include <functional>
+
+#include "Encoder.h"
+#include "Talon.h"
+#include "DriverStation.h"
+#include "AnalogInput.h"
+#include "Compressor.h"
+#include "Relay.h"
+#include "SampleRobot.h"
+#include "dma.h"
+#ifndef WPILIB2015
+#include "DigitalGlitchFilter.h"
+#endif
+#include "PowerDistributionPanel.h"
+#undef ERROR
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/common/util/log_interval.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/stl_mutex.h"
+#include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
+
+#include "y2012/control_loops/drivetrain/drivetrain.q.h"
+#include "y2012/control_loops/accessories/accessories.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"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::y2012::control_loops::drivetrain_queue;
+using ::y2012::control_loops::accessories_queue;
+
+namespace y2012 {
+namespace wpilib {
+
+template <class T, class... U>
+std::unique_ptr<T> make_unique(U &&... u) {
+ return std::unique_ptr<T>(new T(std::forward<U>(u)...));
+}
+
+double drivetrain_translate(int32_t in) {
+ return -static_cast<double>(in) /
+ (256.0 /*cpr*/ * 4.0 /*4x*/) *
+ 1 *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+}
+
+double drivetrain_velocity_translate(double in) {
+ return (1.0 / in) / 256.0 /*cpr*/ *
+ 1 *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
+}
+
+static const double kMaximumEncoderPulsesPerSecond =
+ 5600.0 /* free speed RPM */ * 14.0 / 48.0 /* bottom gear reduction */ *
+ 18.0 / 32.0 /* big belt reduction */ *
+ 18.0 / 66.0 /* top gear reduction */ * 48.0 / 18.0 /* encoder gears */ /
+ 60.0 /* seconds / minute */ * 256.0 /* CPR */;
+
+class SensorReader {
+ public:
+ SensorReader() {}
+
+ void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_left_encoder_ = ::std::move(encoder);
+ drivetrain_left_encoder_->SetMaxPeriod(0.005);
+ }
+
+ void set_drivetrain_right_encoder(::std::unique_ptr<Encoder> encoder) {
+ drivetrain_right_encoder_ = ::std::move(encoder);
+ drivetrain_right_encoder_->SetMaxPeriod(0.005);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("SensorReader");
+
+ my_pid_ = getpid();
+ ds_ =
+#ifdef WPILIB2015
+ DriverStation::GetInstance();
+#else
+ &DriverStation::GetInstance();
+#endif
+ ::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(5),
+ ::aos::time::Time::InMS(4));
+
+ ::aos::SetCurrentThreadRealtimePriority(kPriority);
+ while (run_) {
+ {
+ const int iterations = phased_loop.SleepUntilNext();
+ if (iterations != 1) {
+ LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ }
+ }
+ RunIteration();
+ }
+ }
+
+ void RunIteration() {
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_, nullptr);
+
+ {
+ auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ drivetrain_message->right_encoder =
+ drivetrain_translate(drivetrain_right_encoder_->GetRaw());
+ drivetrain_message->left_encoder =
+ -drivetrain_translate(drivetrain_left_encoder_->GetRaw());
+ drivetrain_message->left_speed =
+ drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod());
+ drivetrain_message->right_speed =
+ drivetrain_velocity_translate(drivetrain_right_encoder_->GetPeriod());
+
+ drivetrain_message.Send();
+ }
+
+ accessories_queue.position.MakeMessage().Send();
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ static const int kPriority = 30;
+ static const int kInterruptPriority = 55;
+
+ int32_t my_pid_;
+ DriverStation *ds_;
+
+ ::std::unique_ptr<Encoder> drivetrain_left_encoder_;
+ ::std::unique_ptr<Encoder> drivetrain_right_encoder_;
+ ::std::atomic<bool> run_{true};
+};
+
+class SolenoidWriter {
+ public:
+ SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
+ : pcm_(pcm),
+ drivetrain_(".y2012.control_loops.drivetrain_queue.output"),
+ accessories_(".y2012.control_loops.accessories_queue.output") {}
+
+ void set_drivetrain_high(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ drivetrain_high_ = ::std::move(s);
+ }
+
+ void set_drivetrain_low(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ drivetrain_low_ = ::std::move(s);
+ }
+
+ void set_s1(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ s1_ = ::std::move(s);
+ }
+
+ void set_s2(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ s2_ = ::std::move(s);
+ }
+
+ void set_s3(::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ s3_ = ::std::move(s);
+ }
+
+ void operator()() {
+ ::aos::SetCurrentThreadName("Solenoids");
+ ::aos::SetCurrentThreadRealtimePriority(30);
+
+ while (run_) {
+ ::aos::time::PhasedLoopXMS(20, 1000);
+
+ {
+ accessories_.FetchLatest();
+ if (accessories_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *accessories_);
+ s1_->Set(accessories_->solenoids[0]);
+ s2_->Set(accessories_->solenoids[1]);
+ s3_->Set(accessories_->solenoids[2]);
+ }
+ }
+
+ {
+ drivetrain_.FetchLatest();
+ if (drivetrain_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
+ const bool high = drivetrain_->left_high || drivetrain_->right_high;
+ drivetrain_high_->Set(high);
+ drivetrain_low_->Set(!high);
+ }
+ }
+
+ {
+ ::frc971::wpilib::PneumaticsToLog to_log;
+ pcm_->Flush();
+ to_log.read_solenoids = pcm_->GetAll();
+ LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ }
+ }
+ }
+
+ void Quit() { run_ = false; }
+
+ private:
+ const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_high_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_low_;
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s1_, s2_, s3_;
+
+ ::std::unique_ptr<Compressor> compressor_;
+
+ ::aos::Queue<::y2012::control_loops::DrivetrainQueue::Output> drivetrain_;
+ ::aos::Queue<::y2012::control_loops::AccessoriesQueue::Message> accessories_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+class DrivetrainWriter : public ::frc971::wpilib::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 {
+ ::y2012::control_loops::drivetrain_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2012::control_loops::drivetrain_queue.output;
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ left_drivetrain_talon_->Set(-queue->left_voltage / 12.0);
+ right_drivetrain_talon_->Set(queue->right_voltage / 12.0);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "drivetrain output too old\n");
+ left_drivetrain_talon_->Disable();
+ right_drivetrain_talon_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> left_drivetrain_talon_;
+ ::std::unique_ptr<Talon> right_drivetrain_talon_;
+};
+
+class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler {
+ public:
+ void set_talon1(::std::unique_ptr<Talon> t) {
+ talon1_ = ::std::move(t);
+ }
+
+ void set_talon2(::std::unique_ptr<Talon> t) {
+ talon2_ = ::std::move(t);
+ }
+
+ private:
+ virtual void Read() override {
+ ::y2012::control_loops::accessories_queue.output.FetchAnother();
+ }
+
+ virtual void Write() override {
+ auto &queue = ::y2012::control_loops::accessories_queue.output;
+ talon1_->Set(queue->sticks[0]);
+ talon2_->Set(queue->sticks[1]);
+ LOG_STRUCT(DEBUG, "will output", *queue);
+ }
+
+ virtual void Stop() override {
+ LOG(WARNING, "shooter output too old\n");
+ talon1_->Disable();
+ talon2_->Disable();
+ }
+
+ ::std::unique_ptr<Talon> talon1_, talon2_;
+};
+
+class WPILibRobot : public SampleRobot {
+ public:
+ ::std::unique_ptr<Encoder> make_encoder(int index) {
+ return make_unique<Encoder>(10 + index * 2, 11 + index * 2, false,
+ Encoder::k4X);
+ }
+
+ virtual void RobotMain() {
+ ::aos::InitNRT();
+ ::aos::SetCurrentThreadName("StartCompetition");
+
+ ::frc971::wpilib::JoystickSender joystick_sender;
+ ::std::thread joystick_thread(::std::ref(joystick_sender));
+
+ SensorReader reader;
+
+ reader.set_drivetrain_left_encoder(make_encoder(0));
+ reader.set_drivetrain_right_encoder(make_encoder(1));
+
+ ::std::thread reader_thread(::std::ref(reader));
+
+ DrivetrainWriter drivetrain_writer;
+ drivetrain_writer.set_left_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(3)));
+ drivetrain_writer.set_right_drivetrain_talon(
+ ::std::unique_ptr<Talon>(new Talon(4)));
+ ::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
+
+ ::y2012::wpilib::AccessoriesWriter accessories_writer;
+ accessories_writer.set_talon1(::std::unique_ptr<Talon>(new Talon(5)));
+ accessories_writer.set_talon2(::std::unique_ptr<Talon>(new Talon(6)));
+ ::std::thread accessories_writer_thread(::std::ref(accessories_writer));
+
+ ::std::unique_ptr<::frc971::wpilib::BufferedPcm> pcm(
+ new ::frc971::wpilib::BufferedPcm());
+ SolenoidWriter solenoid_writer(pcm);
+ solenoid_writer.set_drivetrain_high(pcm->MakeSolenoid(0));
+ solenoid_writer.set_drivetrain_low(pcm->MakeSolenoid(2));
+ solenoid_writer.set_s1(pcm->MakeSolenoid(1));
+ solenoid_writer.set_s2(pcm->MakeSolenoid(3));
+ solenoid_writer.set_s3(pcm->MakeSolenoid(4));
+
+ ::std::thread solenoid_thread(::std::ref(solenoid_writer));
+
+ // Wait forever. Not much else to do...
+ PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
+
+ LOG(ERROR, "Exiting WPILibRobot\n");
+
+ joystick_sender.Quit();
+ joystick_thread.join();
+ reader.Quit();
+ reader_thread.join();
+
+ drivetrain_writer.Quit();
+ drivetrain_writer_thread.join();
+ accessories_writer.Quit();
+ accessories_writer_thread.join();
+
+ ::aos::Cleanup();
+ }
+};
+
+} // namespace wpilib
+} // namespace y2012
+
+
+START_ROBOT_CLASS(::y2012::wpilib::WPILibRobot);