added support for reading sensors from the gyro board
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index e330cfe..606f2e6 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -28,6 +28,7 @@
'../output/output.gyp:CameraServer',
#'camera/camera.gyp:frc971',
'../../gyro_board/src/libusb-driver/libusb-driver.gyp:get',
+ '../input/input.gyp:gyro_board_reader',
],
'copies': [
{
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 99fe9b2..015130f 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -1,7 +1,6 @@
BinaryLogReader
MotorWriter
JoystickReader
-sensor_receiver
drivetrain
CRIOLogReader
angle_adjust
@@ -9,3 +8,4 @@
index
shooter
auto
+gyro_board_reader
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 15a8c74..5b64898 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -22,7 +22,7 @@
// Multiply meters by this to get a drivetrain goal distance.
static const double kDrivetrainCorrectionFactor =
- ((32.0 / 44.0) / (64.0 /24.0 * 19.0 / 50.0));
+ ((32.0 / 44.0) / (64.0 / 24.0 * 19.0 / 50.0));
bool ShouldExitAuto() {
::frc971::autonomous::autonomous.FetchLatest();
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
new file mode 100644
index 0000000..ac39cac
--- /dev/null
+++ b/frc971/input/gyro_board_data.h
@@ -0,0 +1,72 @@
+#ifndef FRC971_INPUT_GYRO_BOARD_DATA_H_
+#define FRC971_INPUT_GYRO_BOARD_DATA_H_
+
+#include "aos/common/byteorder.h"
+
+namespace frc971 {
+
+// The struct that the gyro board sends out with all of the data in it.
+struct GyroBoardData {
+ int64_t gyro_angle;
+
+ int32_t right_drive;
+ int32_t left_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+
+ union {
+ struct {
+ uint8_t wrist_hall_effect : 1;
+ uint8_t angle_adjust_bottom_hall_effect : 1;
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ };
+ uint32_t digitals;
+ };
+
+ void NetworkToHost() {
+ using ::aos::ntoh;
+
+ gyro_angle = ntoh(gyro_angle);
+
+ right_drive = ntoh(right_drive);
+ left_drive = ntoh(left_drive);
+ shooter_angle = ntoh(shooter_angle);
+ shooter = ntoh(shooter);
+ indexer = ntoh(indexer);
+ wrist = ntoh(wrist);
+
+ capture_top_rise = ntoh(capture_top_rise);
+ capture_top_fall = ntoh(capture_top_fall);
+ capture_bottom_fall_delay = ntoh(capture_bottom_fall_delay);
+ capture_wrist_rise = ntoh(capture_wrist_rise);
+ capture_shooter_angle_rise = ntoh(capture_shooter_angle_rise);
+
+ digitals = ntoh(digitals);
+ }
+} __attribute__((__packed__));
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_GYRO_BOARD_DATA_H_
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
new file mode 100644
index 0000000..f123b8e
--- /dev/null
+++ b/frc971/input/gyro_board_reader.cc
@@ -0,0 +1,163 @@
+#include <libusb-1.0/libusb.h>
+#include <memory>
+
+#include "aos/common/inttypes.h"
+#include "aos/atom_code/init.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/wrist/wrist_motor.q.h"
+#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/input/gyro_board_data.h"
+#include "gyro_board/src/libusb-driver/libusb_wrap.h"
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::wrist;
+using ::frc971::control_loops::angle_adjust;
+using ::frc971::control_loops::shooter;
+using ::frc971::control_loops::index_loop;
+
+namespace frc971 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
+}
+
+inline double wrist_translate(int32_t in) {
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
+ (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
+}
+
+inline double angle_adjust_translate(int32_t in) {
+ static const double kCableDiameter = 0.060;
+ return -static_cast<double>(in) / (256.0 /*cpr*/ * 2.0 /*2x*/) *
+ ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
+ (2 * M_PI);
+}
+
+inline double shooter_translate(int32_t in) {
+ return -static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
+ (15.0 / 34.0) /*gears*/ * (2 * M_PI);
+}
+
+inline double index_translate(int32_t in) {
+ return -static_cast<double>(in) / (128.0 /*cpr*/ * 2.0 /*2x*/) *
+ (1.0) /*gears*/ * (2 * M_PI);
+}
+
+} // namespace
+
+class GyroBoardReader {
+ public:
+ void Run() {
+ LibUSB libusb;
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle(
+ libusb.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle) {
+ LOG(FATAL, "couldn't find device\n");
+ }
+
+ uint8_t data[64];
+ GyroBoardData *real_data;
+ static_assert(sizeof(*real_data) <= sizeof(data), "it doesn't fit");
+
+ uint8_t *data_pointer = data;
+ memcpy(&real_data, &data_pointer, sizeof(data_pointer));
+ while (true) {
+ int read_bytes;
+ int r = dev_handle->interrupt_transfer(
+ kEndpoint, data, sizeof(data), &read_bytes, kReadTimeout);
+
+ if (r != 0) {
+ if (r == LIBUSB_ERROR_TIMEOUT) {
+ LOG(ERROR, "read timed out\n");
+ continue;
+ }
+ LOG(FATAL, "libusb gave error %d\n", r);
+ }
+
+ if (read_bytes != sizeof(data)) {
+ LOG(ERROR, "read %d bytes instead of %zu\n",
+ read_bytes, sizeof(data));
+ continue;
+ }
+
+ ProcessData(real_data);
+ }
+ }
+
+ private:
+ static const unsigned char kEndpoint = 0x81;
+ // in ms
+ // 0 is unlimited
+ static const unsigned int kReadTimeout = 1000;
+
+ // vendor ID
+ static const int32_t kVid = 0x1424;
+ // product ID
+ static const int32_t kPid = 0xd243;
+
+ void ProcessData(GyroBoardData *data) {
+ data->NetworkToHost();
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(data->right_drive)
+ .left_encoder(data->left_drive)
+ .Send();
+
+ wrist.position.MakeWithBuilder()
+ .pos(wrist_translate(data->wrist))
+ .hall_effect(!data->wrist_hall_effect)
+ .calibration(wrist_translate(data->capture_wrist_rise))
+ .Send();
+
+ angle_adjust.position.MakeWithBuilder()
+ .angle(angle_adjust_translate(data->shooter_angle))
+ .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
+ .middle_hall_effect(false)
+ .bottom_calibration(angle_adjust_translate(
+ data->capture_shooter_angle_rise))
+ .middle_calibration(angle_adjust_translate(
+ 0))
+ .Send();
+
+ shooter.position.MakeWithBuilder()
+ .position(shooter_translate(data->shooter))
+ .Send();
+
+ index_loop.position.MakeWithBuilder()
+ .index_position(index_translate(data->indexer))
+ .top_disc_detect(!data->top_disc)
+ .top_disc_posedge_count(data->top_rise_count)
+ .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_negedge_count(data->top_fall_count)
+ .top_disc_negedge_position(index_translate(data->capture_top_fall))
+ .bottom_disc_detect(!data->bottom_disc)
+ .bottom_disc_posedge_count(data->bottom_rise_count)
+ .bottom_disc_negedge_count(data->bottom_fall_count)
+ .bottom_disc_negedge_wait_position(index_translate(
+ data->capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(data->bottom_fall_delay_count)
+ .Send();
+ }
+};
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::GyroBoardReader reader;
+ reader.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 5ac253a..ece9d14 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -51,6 +51,25 @@
],
},
{
+ 'target_name': 'gyro_board_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_board_reader.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
+ '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(EXTERNALS):libusb',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ {
'target_name': 'sensor_receiver',
'type': 'executable',
'sources': [
@@ -60,7 +79,6 @@
'<(AOS)/atom_code/atom_code.gyp:init',
'sensor_unpacker',
'<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
- '<(AOS)/atom_code/atom_code.gyp:init',
],
},
{
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
index 873c33b..3537fd2 100644
--- a/frc971/input/sensor_unpacker.cc
+++ b/frc971/input/sensor_unpacker.cc
@@ -80,7 +80,7 @@
angle_adjust.position.MakeWithBuilder()
.angle(angle_adjust_translate(values->angle_adjust_position))
- .bottom_hall_effect(!values->angle_adjust_bottom_hall_effect)
+ .bottom_hall_effect(!values->angle_adjust_bottom_hall_effect)
.middle_hall_effect(!values->angle_adjust_middle_hall_effect && false)
.bottom_calibration(angle_adjust_translate(
values->angle_adjust_bottom_edge_position))