Made some changes to JoystickReader for third robot.
Changes are by no means finalized!
- Copied input stuff to bot3/input.
(Copying even unmodified files was necessary so git wouldn't build stuff
from frc971/input/input.gyp and see duplicate targets.)
- Modified build system to build this.
- Took the opportunity to fix some name-related stylguide compliance issues.
(I fixed these in frc971 as well.)
diff --git a/bot3/input/gyro_board_data.h b/bot3/input/gyro_board_data.h
new file mode 100644
index 0000000..10135e3
--- /dev/null
+++ b/bot3/input/gyro_board_data.h
@@ -0,0 +1,75 @@
+#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 left_drive;
+ int32_t right_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;
+
+ uint8_t top_rise_count;
+
+ uint8_t top_fall_count;
+
+ uint8_t bottom_rise_count;
+
+ uint8_t bottom_fall_delay_count;
+ uint8_t bottom_fall_count;
+
+ uint8_t wrist_rise_count;
+
+ uint8_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() {
+ // Apparently it sends the information out in little endian.
+#if 0
+ 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);
+#endif
+ }
+} __attribute__((__packed__));
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_GYRO_BOARD_DATA_H_
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
new file mode 100644
index 0000000..06e760c
--- /dev/null
+++ b/bot3/input/gyro_board_reader.cc
@@ -0,0 +1,256 @@
+#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 "aos/common/control_loop/Timing.h"
+#include "aos/common/time.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"
+#include "frc971/queues/GyroAngle.q.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;
+using ::frc971::sensors::gyro;
+
+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*/ * 4.0 /*quad*/) *
+ ((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*/ * 4.0 /*quad*/) *
+ (1.0) /*gears*/ * (2 * M_PI);
+}
+
+} // namespace
+
+class GyroBoardReader {
+ public:
+ GyroBoardReader()
+ : top_rise_count_(0),
+ last_top_rise_count_(0),
+ top_fall_count_(0),
+ last_top_fall_count_(0),
+ bottom_rise_count_(0),
+ last_bottom_rise_count_(0),
+ bottom_fall_delay_count_(0),
+ last_bottom_fall_delay_count_(0),
+ bottom_fall_count_(0),
+ last_bottom_fall_count_(0),
+ wrist_rise_count_(0),
+ last_wrist_rise_count_(0),
+ shooter_angle_rise_count_(0),
+ last_shooter_angle_rise_count_(0) {
+ }
+
+ void Run() {
+ LibUSB libusb;
+
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(ERROR, "couldn't find device. exiting\n");
+ exit(1);
+ }
+
+ 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) {
+ if (false) {
+ // Theoretically need -3ms of offset. Using a slightly larger one to avoid
+ // missing the first control loop in the worst case.
+ ::aos::time::PhasedLoop10MS(
+ ::aos::time::Time::InSeconds(-0.0031).ToUSec());
+ LOG(DEBUG, "starting now\n");
+
+ // Read 2 to make sure that we get fresh data.
+ if (!ReadPacket(data, sizeof(data))) continue;
+ //LOG(DEBUG, "in between\n");
+ if (!ReadPacket(data, sizeof(data))) continue;
+ } else {
+ if (!ReadPacket(data, 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;
+
+ // Returns whether it read a good packet.
+ bool ReadPacket(uint8_t *data, size_t data_size) {
+ int read_bytes;
+ int r = dev_handle_->interrupt_transfer(
+ kEndpoint, data, data_size, &read_bytes, kReadTimeout);
+
+ if (r != 0) {
+ if (r == LIBUSB_ERROR_TIMEOUT) {
+ LOG(ERROR, "read timed out\n");
+ return false;
+ }
+ LOG(FATAL, "libusb gave error %d\n", r);
+ }
+
+ if (read_bytes < static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ read_bytes, sizeof(GyroBoardData));
+ return false;
+ }
+
+ return true;
+ }
+
+ void UpdateWrappingCounter(
+ uint8_t current, uint8_t *last, int32_t *counter) {
+ if (*last > current) {
+ *counter += 0x100;
+ }
+ *counter = (*counter & 0xffffff00) | current;
+ *last = current;
+ }
+
+ void ProcessData(GyroBoardData *data) {
+ data->NetworkToHost();
+ LOG(DEBUG, "processing a packet\n");
+ static ::aos::time::Time last_time = ::aos::time::Time::Now();
+ if ((last_time - ::aos::time::Time::Now()) >
+ ::aos::time::Time::InMS(0.00205)) {
+ LOG(INFO, "missed one\n");
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ UpdateWrappingCounter(data->top_rise_count,
+ &last_top_rise_count_, &top_rise_count_);
+ UpdateWrappingCounter(data->top_fall_count,
+ &last_top_fall_count_, &top_fall_count_);
+ UpdateWrappingCounter(data->bottom_rise_count,
+ &last_bottom_rise_count_, &bottom_rise_count_);
+ UpdateWrappingCounter(data->bottom_fall_delay_count,
+ &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
+ UpdateWrappingCounter(data->bottom_fall_count,
+ &last_bottom_fall_count_, &bottom_fall_count_);
+ UpdateWrappingCounter(data->wrist_rise_count,
+ &last_wrist_rise_count_, &wrist_rise_count_);
+ UpdateWrappingCounter(data->shooter_angle_rise_count,
+ &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->right_drive))
+ .left_encoder(-drivetrain_translate(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(top_rise_count_)
+ .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_negedge_count(top_fall_count_)
+ .top_disc_negedge_position(index_translate(data->capture_top_fall))
+ .bottom_disc_detect(data->bottom_disc)
+ .bottom_disc_posedge_count(bottom_rise_count_)
+ .bottom_disc_negedge_count(bottom_fall_count_)
+ .bottom_disc_negedge_wait_position(index_translate(
+ data->capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .Send();
+ }
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+
+ int32_t top_rise_count_;
+ uint8_t last_top_rise_count_;
+ int32_t top_fall_count_;
+ uint8_t last_top_fall_count_;
+ int32_t bottom_rise_count_;
+ uint8_t last_bottom_rise_count_;
+ int32_t bottom_fall_delay_count_;
+ uint8_t last_bottom_fall_delay_count_;
+ int32_t bottom_fall_count_;
+ uint8_t last_bottom_fall_count_;
+ int32_t wrist_rise_count_;
+ uint8_t last_wrist_rise_count_;
+ int32_t shooter_angle_rise_count_;
+ uint8_t last_shooter_angle_rise_count_;
+};
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::GyroBoardReader reader;
+ reader.Run();
+ ::aos::Cleanup();
+ return 0;
+}
diff --git a/bot3/input/gyro_reader.cc b/bot3/input/gyro_reader.cc
new file mode 100644
index 0000000..bdcd07e
--- /dev/null
+++ b/bot3/input/gyro_reader.cc
@@ -0,0 +1,51 @@
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/atom_code/init.h"
+
+#include "frc971/queues/GyroAngle.q.h"
+
+#define M_PI 3.14159265358979323846264338327
+
+using frc971::sensors::gyro;
+
+int main(){
+ aos::Init();
+ int fd = open("/dev/aschuh0", O_RDONLY);
+ int rate_limit = 0;
+ if (fd < 0) {
+ LOG(ERROR, "No Gyro found.\n");
+ } else {
+ LOG(INFO, "Gyro now connected\n");
+ }
+
+ while (true) {
+ int64_t gyro_value;
+ if (read(fd, (void *)&gyro_value, sizeof(gyro_value)) != sizeof(gyro_value)) {
+ LOG(ERROR, "Could not read gyro errno: %d\n", errno);
+ if (errno == ENODEV || errno == EBADF) {
+ close(fd);
+ while (1) {
+ usleep(1000);
+ fd = open("/dev/aschuh0", O_RDONLY);
+ if (fd > 0) {
+ LOG(INFO, "Found gyro again\n");
+ break;
+ }
+ }
+ }
+ continue;
+ }
+ rate_limit ++;
+ if (rate_limit > 10) {
+ LOG(DEBUG, "Gyro is %d\n", (int)(gyro_value / 16));
+ rate_limit = 0;
+ }
+ gyro.MakeWithBuilder().angle(gyro_value / 16.0 / 1000.0 / 180.0 * M_PI).Send();
+ }
+
+ aos::Cleanup();
+}
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
new file mode 100644
index 0000000..4dec23e
--- /dev/null
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -0,0 +1,300 @@
+#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 "aos/common/time.h"
+#include "aos/common/sensors/sensor_unpacker.h"
+#include "aos/common/sensors/sensor_receiver.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"
+#include "frc971/queues/GyroAngle.q.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;
+using ::frc971::sensors::gyro;
+
+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*/ * 4.0 /*quad*/) *
+ ((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*/ * 4.0 /*quad*/) *
+ (1.0) /*gears*/ * (2 * M_PI);
+}
+
+} // namespace
+
+class GyroSensorUnpacker :
+ public ::aos::sensors::SensorUnpackerInterface<GyroBoardData> {
+ public:
+ GyroSensorUnpacker()
+ : top_rise_count_(0),
+ last_top_rise_count_(0),
+ top_fall_count_(0),
+ last_top_fall_count_(0),
+ bottom_rise_count_(0),
+ last_bottom_rise_count_(0),
+ bottom_fall_delay_count_(0),
+ last_bottom_fall_delay_count_(0),
+ bottom_fall_count_(0),
+ last_bottom_fall_count_(0),
+ wrist_rise_count_(0),
+ last_wrist_rise_count_(0),
+ shooter_angle_rise_count_(0),
+ last_shooter_angle_rise_count_(0) {
+ }
+
+ void UnpackFrom(GyroBoardData *data) {
+ data->NetworkToHost();
+ LOG(DEBUG, "processing a packet\n");
+
+ static ::aos::time::Time last_time = ::aos::time::Time::Now();
+ if ((last_time - ::aos::time::Time::Now()) >
+ ::aos::time::Time::InMS(0.00205)) {
+ LOG(INFO, "missed one\n");
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ UpdateWrappingCounter(data->top_rise_count,
+ &last_top_rise_count_, &top_rise_count_);
+ UpdateWrappingCounter(data->top_fall_count,
+ &last_top_fall_count_, &top_fall_count_);
+ UpdateWrappingCounter(data->bottom_rise_count,
+ &last_bottom_rise_count_, &bottom_rise_count_);
+ UpdateWrappingCounter(data->bottom_fall_delay_count,
+ &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
+ UpdateWrappingCounter(data->bottom_fall_count,
+ &last_bottom_fall_count_, &bottom_fall_count_);
+ UpdateWrappingCounter(data->wrist_rise_count,
+ &last_wrist_rise_count_, &wrist_rise_count_);
+ UpdateWrappingCounter(data->shooter_angle_rise_count,
+ &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data->right_drive))
+ .left_encoder(-drivetrain_translate(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(top_rise_count_)
+ .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_negedge_count(top_fall_count_)
+ .top_disc_negedge_position(index_translate(data->capture_top_fall))
+ .bottom_disc_detect(!data->bottom_disc)
+ .bottom_disc_posedge_count(bottom_rise_count_)
+ .bottom_disc_negedge_count(bottom_fall_count_)
+ .bottom_disc_negedge_wait_position(index_translate(
+ data->capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .Send();
+ }
+
+ private:
+ void UpdateWrappingCounter(
+ uint8_t current, uint8_t *last, int32_t *counter) {
+ if (*last > current) {
+ *counter += 0x100;
+ }
+ *counter = (*counter & 0xffffff00) | current;
+ *last = current;
+ }
+
+ int32_t top_rise_count_;
+ uint8_t last_top_rise_count_;
+ int32_t top_fall_count_;
+ uint8_t last_top_fall_count_;
+ int32_t bottom_rise_count_;
+ uint8_t last_bottom_rise_count_;
+ int32_t bottom_fall_delay_count_;
+ uint8_t last_bottom_fall_delay_count_;
+ int32_t bottom_fall_count_;
+ uint8_t last_bottom_fall_count_;
+ int32_t wrist_rise_count_;
+ uint8_t last_wrist_rise_count_;
+ int32_t shooter_angle_rise_count_;
+ uint8_t last_shooter_angle_rise_count_;
+};
+
+class GyroSensorReceiver :
+ public ::aos::sensors::SensorReceiver<GyroBoardData> {
+ public:
+ GyroSensorReceiver(
+ ::aos::sensors::SensorUnpackerInterface<GyroBoardData> *unpacker)
+ : ::aos::sensors::SensorReceiver<GyroBoardData>(unpacker),
+ start_time_(0, 0) {
+ static_assert(sizeof(GyroBoardData) <= kDataLength,
+ "the buffer will be too small");
+ }
+
+ 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;
+
+ // How big of a buffer to give the function.
+ static const size_t kDataLength = 64;
+
+ virtual void DoReceiveData() {
+ // Loop and then return once we get a good one.
+ while (true) {
+ completed_transfer_ = NULL;
+ while (completed_transfer_ == NULL) {
+ libusb_.HandleEvents();
+ }
+ LOG(DEBUG, "processing transfer %p\n", completed_transfer_);
+
+ if (completed_transfer_->read_bytes() <
+ static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ completed_transfer_->read_bytes(), sizeof(GyroBoardData));
+ continue;
+ }
+
+ memcpy(&data()->values, completed_transfer_->data(),
+ sizeof(GyroBoardData));
+ if (data()->count == 0) {
+ start_time_ = ::aos::time::Time::Now();
+ data()->count = 1;
+ } else {
+ ::aos::time::Time delta_time = ::aos::time::Time::Now() - start_time_;
+ data()->count = static_cast<int32_t>(
+ (delta_time / ::aos::sensors::kSensorSendFrequency) + 0.5);
+ }
+ return;
+ }
+ }
+
+ virtual void Reset() {
+ transfer1_.reset();
+ transfer2_.reset();
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb_.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(ERROR, "couldn't find device. exiting\n");
+ exit(1);
+ }
+ transfer1_ = ::std::unique_ptr<libusb::Transfer>(
+ new libusb::Transfer(kDataLength, StaticTransferCallback, this));
+ transfer2_ = ::std::unique_ptr<libusb::Transfer>(
+ new libusb::Transfer(kDataLength, StaticTransferCallback, this));
+ transfer1_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
+ transfer2_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
+ transfer1_->Submit();
+ transfer2_->Submit();
+
+ data()->count = 0;
+ }
+
+ static void StaticTransferCallback(libusb::Transfer *transfer, void *self) {
+ static_cast<GyroSensorReceiver *>(self)->TransferCallback(transfer);
+ }
+ void TransferCallback(libusb::Transfer *transfer) {
+ if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
+ LOG(DEBUG, "transfer %p completed\n", transfer);
+ completed_transfer_ = transfer;
+ } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
+ LOG(WARNING, "transfer %p timed out\n", transfer);
+ } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
+ LOG(DEBUG, "transfer %p cancelled\n", transfer);
+ } else {
+ LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
+ }
+ transfer->Submit();
+ }
+
+ virtual void Synchronized(::aos::time::Time start_time) {
+ // Subtract off how many packets it read while synchronizing from the time.
+ start_time_ = start_time -
+ ::aos::sensors::kSensorSendFrequency * data()->count;
+ }
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+ ::std::unique_ptr<libusb::Transfer> transfer1_, transfer2_;
+ // Temporary variable for holding a completed transfer to communicate that
+ // information from the callback to the code that wants it.
+ libusb::Transfer *completed_transfer_;
+
+ ::aos::time::Time start_time_;
+
+ LibUSB libusb_;
+};
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::GyroSensorUnpacker unpacker;
+ ::frc971::GyroSensorReceiver receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
new file mode 100644
index 0000000..8d75241
--- /dev/null
+++ b/bot3/input/input.gyp
@@ -0,0 +1,77 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'joystick_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'joystick_reader.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/atom_code/input/input.gyp:joystick_input',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
+ '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
+ '<(DEPTH)/frc971/autonomous/autonomous.gyp:auto_queue',
+ ],
+ },
+ {
+ 'target_name': 'gyro_sensor_receiver',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_sensor_receiver.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',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ ],
+ },
+ {
+ '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',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
+ ],
+ },
+ {
+ 'target_name': 'gyro_reader',
+ 'type': 'executable',
+ 'sources': [
+ 'gyro_reader.cc',
+ ],
+ 'dependencies': [
+ '<(DEPTH)/frc971/queues/queues.gyp:queues',
+ '<(AOS)/atom_code/atom_code.gyp:init',
+ '<(AOS)/build/aos.gyp:logging',
+ ],
+ },
+ ],
+}
diff --git a/bot3/input/joystick_reader.cc b/bot3/input/joystick_reader.cc
new file mode 100644
index 0000000..da8a8e2
--- /dev/null
+++ b/bot3/input/joystick_reader.cc
@@ -0,0 +1,218 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <math.h>
+
+#include "aos/atom_code/init.h"
+#include "aos/atom_code/input/joystick_input.h"
+#include "aos/common/logging/logging.h"
+
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/queues/Piston.q.h"
+#include "frc971/autonomous/auto.q.h"
+#include "frc971/control_loops/index/index_motor.q.h"
+#include "frc971/control_loops/shooter/shooter_motor.q.h"
+#include "frc971/queues/CameraTarget.q.h"
+
+using ::frc971::control_loops::drivetrain;
+using ::frc971::control_loops::shifters;
+using ::frc971::sensors::gyro;
+using ::frc971::control_loops::index_loop;
+using ::frc971::control_loops::shooter;
+using ::frc971::control_loops::hangers;
+using ::frc971::vision::target_angle;
+
+using ::aos::input::driver_station::ButtonLocation;
+using ::aos::input::driver_station::JoystickAxis;
+using ::aos::input::driver_station::ControlBit;
+
+namespace bot3 {
+namespace input {
+namespace joysticks {
+
+const ButtonLocation kDriveControlLoopEnable1(1, 7),
+ kDriveControlLoopEnable2(1, 11);
+const JoystickAxis kSteeringWheel(1, 1), kDriveThrottle(2, 2);
+const ButtonLocation kShiftHigh(2, 1), kShiftLow(2, 3);
+const ButtonLocation kQuickTurn(1, 5);
+
+const ButtonLocation kLongShot(3, 5);
+const ButtonLocation kMediumShot(3, 3);
+const ButtonLocation kShortShot(3, 6);
+const ButtonLocation kPitShot1(2, 7), kPitShot2(2, 10);
+
+const ButtonLocation kFire(3, 11);
+const ButtonLocation kIntake(3, 10);
+const ButtonLocation kForceFire(3, 12);
+const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+
+const ButtonLocation kDeployHangers(3, 1);
+
+class Reader : public ::aos::input::JoystickInput {
+ public:
+ static const bool kWristAlwaysDown = false;
+
+ Reader() {
+ printf("\nRunning Bot3 JoystickReader!\n");
+ shifters.MakeWithBuilder().set(true).Send();
+ }
+
+ virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
+ static bool is_high_gear = false;
+
+ if (data.GetControlBit(ControlBit::kAutonomous)) {
+ if (data.PosEdge(ControlBit::kEnabled)){
+ LOG(INFO, "Starting auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(true).Send();
+ } else if (data.NegEdge(ControlBit::kEnabled)) {
+ LOG(INFO, "Stopping auto mode\n");
+ ::frc971::autonomous::autonomous.MakeWithBuilder()
+ .run_auto(false).Send();
+ }
+ } else { // teleop
+ bool is_control_loop_driving = false;
+ double left_goal = 0.0;
+ double right_goal = 0.0;
+ const double wheel = data.GetAxis(kSteeringWheel);
+ const double throttle = -data.GetAxis(kDriveThrottle);
+ LOG(DEBUG, "wheel %f throttle %f\n", wheel, throttle);
+ const double kThrottleGain = 1.0 / 2.5;
+ if (data.IsPressed(kDriveControlLoopEnable1) ||
+ data.IsPressed(kDriveControlLoopEnable2)) {
+ static double distance = 0.0;
+ static double angle = 0.0;
+ static double filtered_goal_distance = 0.0;
+ if (data.PosEdge(kDriveControlLoopEnable1) ||
+ data.PosEdge(kDriveControlLoopEnable2)) {
+ if (drivetrain.position.FetchLatest() && gyro.FetchLatest()) {
+ distance = (drivetrain.position->left_encoder +
+ drivetrain.position->right_encoder) / 2.0
+ - throttle * kThrottleGain / 2.0;
+ angle = gyro->angle;
+ filtered_goal_distance = distance;
+ }
+ }
+ is_control_loop_driving = true;
+
+ //const double gyro_angle = Gyro.View().angle;
+ const double goal_theta = angle - wheel * 0.27;
+ const double goal_distance = distance + throttle * kThrottleGain;
+ //TODO(danielp) Change this after a look in the CAD.
+ const double robot_width = 22.0 / 100.0 * 2.54;
+ const double kMaxVelocity = 0.6;
+ if (goal_distance > kMaxVelocity * 0.02 + filtered_goal_distance) {
+ filtered_goal_distance += kMaxVelocity * 0.02;
+ } else if (goal_distance < -kMaxVelocity * 0.02 +
+ filtered_goal_distance) {
+ filtered_goal_distance -= kMaxVelocity * 0.02;
+ } else {
+ filtered_goal_distance = goal_distance;
+ }
+ left_goal = filtered_goal_distance - robot_width * goal_theta / 2.0;
+ right_goal = filtered_goal_distance + robot_width * goal_theta / 2.0;
+ is_high_gear = false;
+
+ LOG(DEBUG, "Left goal %f Right goal %f\n", left_goal, right_goal);
+ }
+ if (!(drivetrain.goal.MakeWithBuilder()
+ .steering(wheel)
+ .throttle(throttle)
+ .highgear(is_high_gear).quickturn(data.IsPressed(kQuickTurn))
+ .control_loop_driving(is_control_loop_driving)
+ .left_goal(left_goal).right_goal(right_goal).Send())) {
+ LOG(WARNING, "sending stick values failed\n");
+ }
+
+ if (data.PosEdge(kShiftHigh)) {
+ is_high_gear = false;
+ }
+ if (data.PosEdge(kShiftLow)) {
+ is_high_gear = true;
+ }
+
+ ::aos::ScopedMessagePtr<frc971::control_loops::ShooterLoop::Goal> shooter_goal =
+ shooter.goal.MakeMessage();
+ shooter_goal->velocity = 0;
+ if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
+ shooter_goal->velocity = 131;
+ } else if (data.IsPressed(kLongShot)) {
+#if 0
+ target_angle.FetchLatest();
+ if (target_angle.IsNewerThanMS(500)) {
+ shooter_goal->velocity = target_angle->shooter_speed;
+ angle_adjust_goal = target_angle->shooter_angle;
+ // TODO(brians): do the math right here
+ wrist_up_position = 0.70;
+ } else {
+ LOG(WARNING, "camera frame too old\n");
+ // pretend like no button is pressed
+ }
+#endif
+ shooter_goal->velocity = 360;
+ } else if (data.IsPressed(kMediumShot)) {
+#if 0
+ shooter_goal->velocity = 375;
+ wrist_up_position = 0.70;
+ angle_adjust_goal = 0.564;
+#endif
+ // middle wheel on the back line (same as auto)
+ shooter_goal->velocity = 395;
+ } else if (data.IsPressed(kShortShot)) {
+ shooter_goal->velocity = 375;
+ }
+
+ //TODO (daniel) Modify this for hopper.
+ ::aos::ScopedMessagePtr<frc971::control_loops::IndexLoop::Goal> index_goal =
+ index_loop.goal.MakeMessage();
+ if (data.IsPressed(kFire)) {
+ // FIRE
+ index_goal->goal_state = 4;
+ } else if (shooter_goal->velocity != 0) {
+ // get ready to shoot
+ index_goal->goal_state = 3;
+ } else if (data.IsPressed(kIntake)) {
+ // intake
+ index_goal->goal_state = 2;
+ } else {
+ // get ready to intake
+ index_goal->goal_state = 1;
+ }
+ index_goal->force_fire = data.IsPressed(kForceFire);
+
+ const bool index_up = data.IsPressed(kForceIndexUp);
+ const bool index_down = data.IsPressed(kForceIndexDown);
+ index_goal->override_index = index_up || index_down;
+ if (index_up && index_down) {
+ index_goal->index_voltage = 0.0;
+ } else if (index_up) {
+ index_goal->index_voltage = 12.0;
+ } else if (index_down) {
+ index_goal->index_voltage = -12.0;
+ }
+
+ index_goal.Send();
+ shooter_goal.Send();
+ }
+
+ static int hanger_cycles = 0;
+ if (data.IsPressed(kDeployHangers)) {
+ ++hanger_cycles;
+ } else {
+ hanger_cycles = 0;
+ }
+ hangers.MakeWithBuilder().set(hanger_cycles >= 10).Send();
+ }
+};
+
+} // namespace joysticks
+} // namespace input
+} // namespace bot3
+
+int main() {
+ ::aos::Init();
+ ::bot3::input::joysticks::Reader reader;
+ reader.Run();
+ ::aos::Cleanup();
+}