implement reading the gyro over SPI
Change-Id: Iee3041f45e5df9f079a60eb48726659d88513417
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 7b2c3a3..6f13205 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -32,6 +32,7 @@
#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 "Encoder.h"
#include "Talon.h"
@@ -46,8 +47,6 @@
using ::aos::util::SimpleLogInterval;
using ::frc971::control_loops::drivetrain;
-using ::frc971::sensors::other_sensors;
-using ::frc971::sensors::gyro_reading;
using ::aos::util::WrappingCounter;
namespace frc971 {
@@ -355,10 +354,6 @@
static const double kVcc = 5.15;
-double gyro_translate(int64_t in) {
- return in / 16.0 / 1000.0 / (180.0 / M_PI);
-}
-
float hall_translate(const constants::ShifterHallEffect &k, float in_low,
float in_high) {
const float low_ratio =
@@ -617,9 +612,6 @@
filter_.Add(claw_bottom_front_hall_.get());
filter_.Add(claw_bottom_calibration_hall_.get());
filter_.Add(claw_bottom_back_hall_.get());
- printf("Filtering all hall effect sensors with a %" PRIu64
- " nanosecond window\n",
- filter_.GetPeriodNanoSeconds());
}
void operator()() {
@@ -687,31 +679,6 @@
//::aos::time::TimeFreezer time_freezer;
DriverStation *ds = DriverStation::GetInstance();
- bool bad_gyro = true;
- // TODO(brians): Switch to LogInterval for these things.
- /*
- if (data->uninitialized_gyro) {
- LOG(DEBUG, "uninitialized gyro\n");
- bad_gyro = true;
- } else if (data->zeroing_gyro) {
- LOG(DEBUG, "zeroing gyro\n");
- bad_gyro = true;
- } else if (data->bad_gyro) {
- LOG(ERROR, "bad gyro\n");
- bad_gyro = true;
- } else if (data->old_gyro_reading) {
- LOG(WARNING, "old/bad gyro reading\n");
- bad_gyro = true;
- } else {
- bad_gyro = false;
- }
- */
-
- if (!bad_gyro) {
- // TODO(austin): Read the gyro.
- gyro_reading.MakeWithBuilder().angle(0).Send();
- }
-
if (ds->IsSysActive()) {
auto message = ::aos::controls::output_check_received.MakeMessage();
// TODO(brians): Actually read a pulse value from the roboRIO.
@@ -976,6 +943,8 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::SensorReader reader;
::std::thread reader_thread(::std::ref(reader));
+ ::frc971::wpilib::GyroSender gyro_sender;
+ ::std::thread gyro_thread(::std::ref(gyro_sender));
::std::unique_ptr<Compressor> compressor(new Compressor());
compressor->SetClosedLoopControl(true);
@@ -986,7 +955,6 @@
::std::unique_ptr<Talon>(new Talon(2)));
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
-
::frc971::wpilib::ClawWriter claw_writer;
claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
@@ -1013,10 +981,13 @@
PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));
LOG(ERROR, "Exiting WPILibRobot\n");
+
joystick_sender.Quit();
joystick_thread.join();
reader.Quit();
reader_thread.join();
+ gyro_sender.Quit();
+ gyro_thread.join();
drivetrain_writer.Quit();
drivetrain_writer_thread.join();