implement reading the gyro over SPI
Change-Id: Iee3041f45e5df9f079a60eb48726659d88513417
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 8cc1e62..db2d31e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -17,7 +17,7 @@
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/queues/gyro.q.h"
#include "frc971/shifter_hall_effect.h"
using frc971::sensors::gyro_reading;
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index 3854fe1..8205a25 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -43,7 +43,7 @@
'<(AOS)/common/controls/controls.gyp:polytope',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/queues/queues.gyp:gyro',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/logging/logging.gyp:matrix_logging',
@@ -68,7 +68,7 @@
'drivetrain_lib',
'<(AOS)/common/controls/controls.gyp:control_loop_test',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/queues/queues.gyp:gyro',
'<(AOS)/common/common.gyp:queues',
],
},
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 3b607fe..0823306 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -13,7 +13,7 @@
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/queues/gyro.q.h"
namespace frc971 {
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 69340d3..8473fe5 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -27,7 +27,7 @@
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/util/util.gyp:log_interval',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/queues/queues.gyp:gyro',
'<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
@@ -58,6 +58,7 @@
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/common/controls/controls.gyp:output_check',
+ '<(DEPTH)/frc971/queues/queues.gyp:gyro',
],
},
],
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 9156509..771a06c 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -12,7 +12,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/constants.h"
-#include "frc971/queues/other_sensors.q.h"
+#include "frc971/queues/gyro.q.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index f5b2047..1cdf201 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/other_sensors.q.h"
+#include "frc971/queues/gyro.q.h"
#include "frc971/constants.h"
#include "frc971/queues/to_log.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
diff --git a/frc971/queues/gyro.q b/frc971/queues/gyro.q
new file mode 100644
index 0000000..7b234a1
--- /dev/null
+++ b/frc971/queues/gyro.q
@@ -0,0 +1,13 @@
+package frc971.sensors;
+
+message GyroReading {
+ // Positive is counter-clockwise (Austin says "it's Positive").
+ // Right-hand coordinate system around the Z-axis going up.
+ double angle;
+};
+queue GyroReading gyro_reading;
+
+message Uid {
+ uint32_t uid;
+};
+queue Uid gyro_part_id;
diff --git a/frc971/queues/other_sensors.q b/frc971/queues/other_sensors.q
index 7a33567..85f2ad6 100644
--- a/frc971/queues/other_sensors.q
+++ b/frc971/queues/other_sensors.q
@@ -6,13 +6,6 @@
};
queue OtherSensors other_sensors;
-message GyroReading {
- // Positive is counter-clockwise (Austin says "it's Positive").
- // Right-hand coordinate system around the Z-axis going up.
- double angle;
-};
-queue GyroReading gyro_reading;
-
message AutoMode {
double voltage;
};
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 9c6c38c..e503e46 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -18,6 +18,17 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'gyro',
+ 'type': 'static_library',
+ 'sources': [
+ 'gyro.q',
+ ],
+ 'variables': {
+ 'header_path': 'frc971/queues',
+ },
+ 'includes': ['../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'queues',
'type': 'static_library',
'sources': ['<@(queue_files)'],
diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
new file mode 100644
index 0000000..4b440a9
--- /dev/null
+++ b/frc971/wpilib/gyro_interface.cc
@@ -0,0 +1,144 @@
+#include "frc971/wpilib/gyro_interface.h"
+
+#include <inttypes.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace frc971 {
+namespace wpilib {
+
+GyroInterface::GyroInterface() : gyro_(new SPI(SPI::kOnboardCS0)) {
+ // The gyro goes up to 8.08MHz.
+ // The myRIO goes up to 4MHz, so the roboRIO probably does too.
+ gyro_->SetClockRate(4e6);
+ gyro_->SetChipSelectActiveLow();
+ gyro_->SetClockActiveHigh();
+ gyro_->SetSampleDataOnRising();
+ gyro_->SetMSBFirst();
+}
+
+bool GyroInterface::InitializeGyro() {
+ uint32_t result;
+ if (!DoTransaction(0x20000003, &result)) {
+ LOG(WARNING, "failed to start a self-check\n");
+ return false;
+ }
+ if (result != 1) {
+ // We might have hit a parity error or something and are now retrying, so
+ // this isn't a very big deal.
+ LOG(INFO, "gyro unexpected initial response 0x%" PRIx32 "\n", result);
+ }
+
+ // Wait for it to assert the fault conditions before reading them.
+ ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+
+ if (!DoTransaction(0x20000000, &result)) {
+ LOG(WARNING, "failed to clear latched non-fault data\n");
+ return false;
+ }
+ LOG(DEBUG, "gyro dummy response is 0x%" PRIx32 "\n", result);
+
+ if (!DoTransaction(0x20000000, &result)) {
+ LOG(WARNING, "failed to read self-test data\n");
+ return false;
+ }
+ if (ExtractStatus(result) != 2) {
+ LOG(WARNING, "gyro first value 0x%" PRIx32 " not self-test data\n", result);
+ return false;
+ }
+ if (ExtractErrors(result) != 0x7F) {
+ LOG(WARNING, "gyro first value 0x%" PRIx32 " does not have all errors\n",
+ result);
+ return false;
+ }
+
+ if (!DoTransaction(0x20000000, &result)) {
+ LOG(WARNING, "failed to clear latched self-test data\n");
+ return false;
+ }
+ if (ExtractStatus(result) != 2) {
+ LOG(WARNING, "gyro second value 0x%" PRIx32 " not self-test data\n",
+ result);
+ return false;
+ }
+
+ return true;
+}
+
+bool GyroInterface::DoTransaction(uint32_t to_write, uint32_t *result) {
+ static const uint8_t kBytes = 4;
+ static_assert(kBytes == sizeof(to_write),
+ "need the same number of bytes as sizeof(the data)");
+
+ if (__builtin_parity(to_write & ~1) == 0) to_write |= 1;
+
+ uint8_t to_send[kBytes], to_receive[kBytes];
+ const uint32_t to_write_flipped = __builtin_bswap32(to_write);
+ memcpy(to_send, &to_write_flipped, kBytes);
+
+ switch (gyro_->Transaction(to_send, to_receive, kBytes)) {
+ case -1:
+ LOG(INFO, "SPI::Transaction failed\n");
+ return false;
+ case kBytes:
+ break;
+ default:
+ LOG(FATAL, "SPI::Transaction returned something weird\n");
+ }
+
+ memcpy(result, to_receive, kBytes);
+ if (__builtin_parity(*result & 0xFFFF) != 1) {
+ LOG(INFO, "high byte parity failure\n");
+ return false;
+ }
+ if (__builtin_parity(*result) != 1) {
+ LOG(INFO, "whole value parity failure\n");
+ return false;
+ }
+
+ *result = __builtin_bswap32(*result);
+ return true;
+}
+
+uint16_t GyroInterface::DoRead(uint8_t address) {
+ const uint32_t command = (0x8 << 28) | (address << 17);
+ uint32_t response;
+ while (true) {
+ if (!DoTransaction(command, &response)) {
+ LOG(WARNING, "reading 0x%" PRIx8 " failed\n", address);
+ continue;
+ }
+ if ((response & 0xEFE00000) != 0x4E000000) {
+ LOG(WARNING, "gyro read from 0x%" PRIx8
+ " gave unexpected response 0x%" PRIx32 "\n",
+ address, response);
+ continue;
+ }
+ return (response >> 5) & 0xFFFF;
+ }
+}
+
+double GyroInterface::ExtractAngle(uint32_t value) {
+ const int16_t reading = -(int16_t)(value >> 10 & 0xFFFF);
+ return static_cast<double>(reading) * 2.0 * M_PI / 360.0 / 80.0;
+}
+
+uint32_t GyroInterface::ReadPartID() {
+ return (DoRead(0x0E) << 16) | DoRead(0x10);
+}
+
+uint32_t GyroInterface::GetReading() {
+ uint32_t result;
+ if (!DoTransaction(0x20000000, &result)) {
+ return 0;
+ }
+ return result;
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/gyro_interface.h b/frc971/wpilib/gyro_interface.h
new file mode 100644
index 0000000..baba248
--- /dev/null
+++ b/frc971/wpilib/gyro_interface.h
@@ -0,0 +1,55 @@
+#ifndef FRC971_WPILIB_GYRO_INTERFACE_H_
+#define FRC971_WPILIB_GYRO_INTERFACE_H_
+
+#include <memory>
+
+#include "SPI.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class GyroInterface {
+ public:
+ GyroInterface();
+
+ // Runs the recommended gyro startup procedure including checking all of the
+ // self-test bits.
+ // Returns true if it succeeds.
+ bool InitializeGyro();
+
+ // Reads one of the gyro's "registers" and returns the value.
+ // Retries until it succeeds.
+ uint16_t DoRead(uint8_t address);
+
+ // Returns all of the non-data bits in the "header" except the parity from
+ // value.
+ uint8_t ExtractStatus(uint32_t value) { return (value >> 26) & ~4; }
+ // Returns all of the error bits in the "footer" from value.
+ uint8_t ExtractErrors(uint32_t value) { return (value >> 1) & 0x7F; }
+
+ // Returns the anglular rate contained in value.
+ double ExtractAngle(uint32_t value);
+
+ // Performs a transaction with the gyro.
+ // to_write is the value to write. This function handles setting the checksum
+ // bit.
+ // result is where to stick the result. This function verifies the parity bit.
+ // Returns true for success.
+ bool DoTransaction(uint32_t to_write, uint32_t *result);
+
+ // Returns the part ID from the gyro.
+ // Retries until it succeeds.
+ uint32_t ReadPartID();
+
+ // Gets a reading from the gyro.
+ // Returns a value to be passed to the Extract* methods or 0 for error.
+ uint32_t GetReading();
+
+ private:
+ ::std::unique_ptr<SPI> gyro_;
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_GYRO_INTERFACE_H_
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
new file mode 100644
index 0000000..a155b7e
--- /dev/null
+++ b/frc971/wpilib/gyro_sender.cc
@@ -0,0 +1,136 @@
+#include "frc971/wpilib/gyro_sender.h"
+
+#include <inttypes.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/util/phased_loop.h"
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+
+#include "frc971/queues/gyro.q.h"
+
+namespace frc971 {
+namespace wpilib {
+
+GyroSender::GyroSender() {}
+
+void GyroSender::operator()() {
+ ::aos::SetCurrentThreadName("Gyro");
+
+ // Try to initialize repeatedly as long as we're supposed to be running.
+ while (run_ && !gyro_.InitializeGyro()) {
+ ::aos::time::SleepFor(::aos::time::Time::InMS(50));
+ }
+ LOG(INFO, "gyro initialized successfully\n");
+
+ auto message = ::frc971::sensors::gyro_part_id.MakeMessage();
+ message->uid = gyro_.ReadPartID();
+ LOG_STRUCT(INFO, "gyro ID", *message);
+ message.Send();
+
+ // In radians, ready to send out.
+ double angle = 0;
+
+ int startup_cycles_left = 2 * kReadingRate;
+
+ double zeroing_data[6 * kReadingRate];
+ size_t zeroing_index = 0;
+ bool zeroed = false;
+ bool have_zeroing_data = false;
+ double zero_offset = 0;
+
+ while (run_) {
+ ::aos::time::PhasedLoopXMS(1000 / kReadingRate, 0);
+
+ const uint32_t result = gyro_.GetReading();
+ if (result == 0) {
+ LOG(WARNING, "normal gyro read failed\n");
+ continue;
+ }
+ switch (gyro_.ExtractStatus(result)) {
+ case 0:
+ LOG(WARNING, "gyro says data is bad\n");
+ continue;
+ case 1:
+ break;
+ default:
+ LOG(WARNING, "gyro gave weird status 0x%" PRIx8 "\n",
+ gyro_.ExtractStatus(result));
+ continue;
+ }
+ if (gyro_.ExtractErrors(result) != 0) {
+ const uint8_t errors = gyro_.ExtractErrors(result);
+ if (errors & (1 << 6)) {
+ LOG(WARNING, "gyro gave PLL error\n");
+ }
+ if (errors & (1 << 5)) {
+ LOG(WARNING, "gyro gave quadrature error\n");
+ }
+ if (errors & (1 << 4)) {
+ LOG(WARNING, "gyro gave non-volatile memory error\n");
+ }
+ if (errors & (1 << 3)) {
+ LOG(WARNING, "gyro gave volatile memory error\n");
+ }
+ if (errors & (1 << 2)) {
+ LOG(WARNING, "gyro gave power error\n");
+ }
+ if (errors & (1 << 1)) {
+ LOG(WARNING, "gyro gave continuous self-test error\n");
+ }
+ if (errors & 1) {
+ LOG(WARNING, "gyro gave unexpected self-test mode\n");
+ }
+ continue;
+ }
+
+ if (startup_cycles_left > 0) {
+ --startup_cycles_left;
+ continue;
+ }
+
+ const double new_angle =
+ gyro_.ExtractAngle(result) / static_cast<double>(kReadingRate);
+ auto message = ::frc971::sensors::gyro_reading.MakeMessage();
+ if (zeroed) {
+ angle += new_angle;
+ angle += zero_offset;
+ message->angle = angle;
+ LOG_STRUCT(DEBUG, "sending", *message);
+ message.Send();
+ } else {
+ // TODO(brian): Don't break without 6 seconds of standing still before
+ // enabling. Ideas:
+ // Don't allow driving until we have at least some data?
+ // Some kind of indicator light?
+ {
+ message->angle = new_angle;
+ LOG_STRUCT(DEBUG, "collected", *message);
+ }
+ zeroing_data[zeroing_index] = new_angle;
+ ++zeroing_index;
+ if (zeroing_index >= sizeof(zeroing_data) / sizeof(zeroing_data[0])) {
+ zeroing_index = 0;
+ have_zeroing_data = true;
+ }
+
+ ::aos::robot_state.FetchLatest();
+ if (::aos::robot_state.get() && ::aos::robot_state->enabled &&
+ have_zeroing_data) {
+ zero_offset = 0;
+ for (size_t i = 0; i < sizeof(zeroing_data) / sizeof(zeroing_data[0]);
+ ++i) {
+ zero_offset -= zeroing_data[i];
+ }
+ zero_offset /= sizeof(zeroing_data) / sizeof(zeroing_data[0]);
+ LOG(INFO, "total zero offset %f\n", zero_offset);
+ zeroed = true;
+ }
+ }
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
new file mode 100644
index 0000000..029ace9
--- /dev/null
+++ b/frc971/wpilib/gyro_sender.h
@@ -0,0 +1,41 @@
+#ifndef FRC971_WPILIB_GYRO_H_
+#define FRC971_WPILIB_GYRO_H_
+
+#include <stdint.h>
+
+#include <atomic>
+
+#include "frc971/wpilib/gyro_interface.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Handles reading the gyro over SPI and sending out angles on a queue.
+//
+// This is designed to be passed into ::std::thread's constructor so it will run
+// as a separate thread.
+class GyroSender {
+ public:
+ GyroSender();
+
+ // For ::std::thread to call.
+ //
+ // Initializes the gyro and then loops forever taking readings.
+ void operator()();
+
+ void Quit() { run_ = false; }
+
+ private:
+
+ // Readings per second.
+ static const int kReadingRate = 200;
+
+ GyroInterface gyro_;
+
+ ::std::atomic<bool> run_{true};
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_GYRO_H_
diff --git a/frc971/wpilib/wpilib.gyp b/frc971/wpilib/wpilib.gyp
index d69a0ed..a7c7dff 100644
--- a/frc971/wpilib/wpilib.gyp
+++ b/frc971/wpilib/wpilib.gyp
@@ -22,10 +22,12 @@
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/common/controls/controls.gyp:output_check',
'<(AOS)/common/messages/messages.gyp:robot_state',
+ '<(AOS)/common/util/util.gyp:phased_loop',
'hall_effect',
'joystick_sender',
'loop_output_handler',
'buffered_pcm',
+ 'gyro_sender',
],
},
{
@@ -44,6 +46,41 @@
],
},
{
+ 'target_name': 'gyro_interface',
+ 'type': 'static_library',
+ 'sources': [
+ 'gyro_interface.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):WPILib_roboRIO',
+ ],
+ },
+ {
+ 'target_name': 'gyro_sender',
+ 'type': 'static_library',
+ 'sources': [
+ 'gyro_sender.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/queues/queues.gyp:gyro',
+ 'gyro_interface',
+ '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/logging/logging.gyp:queue_logging',
+ '<(AOS)/common/util/util.gyp:phased_loop',
+ '<(AOS)/common/messages/messages.gyp:robot_state',
+ '<(AOS)/linux_code/linux_code.gyp:init',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ 'export_dependent_settings': [
+ 'gyro_interface',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
+ {
'target_name': 'loop_output_handler',
'type': 'static_library',
'sources': [
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();