got it so that reading usb packets reliably works "sometimes"
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
index dc9f17d..e3840c4 100644
--- a/aos/common/sensors/sensor_receiver-tmpl.h
+++ b/aos/common/sensors/sensor_receiver-tmpl.h
@@ -6,7 +6,7 @@
template<class Values>
const time::Time SensorReceiver<Values>::kJitterDelay =
- time::Time::InSeconds(0.0035);
+ time::Time::InSeconds(0.00225);
// Not a multiple of kSensorSendFrequency to unwedge ourself if we hit some bug
// where it needs to get off of that frequency to work.
template<class Values>
@@ -185,6 +185,7 @@
}
}
+ Synchronized(goal_time + kLoopFrequency * kTestCycles);
return true;
}
@@ -197,13 +198,6 @@
int old_count = data_.count;
DoReceiveData();
- data_.checksum = ntoh(data_.checksum);
- if (!data_.CheckChecksum()) {
- LOG(WARNING, "got a bad packet\n");
- return ReceiveData();
- }
-
- data_.NetworkToHost();
if (data_.count < 0) {
LOG(FATAL, "data count overflowed. currently %"PRId32"\n", data_.count);
}
@@ -251,6 +245,13 @@
while (true) {
if (socket_.Receive(this->data(), sizeof(*this->data())) ==
sizeof(*this->data())) {
+ this->data()->checksum = ntoh(this->data()->checksum);
+ if (!this->data()->CheckChecksum()) {
+ LOG(WARNING, "got a bad packet\n");
+ continue;
+ }
+
+ this->data()->NetworkToHost();
return;
}
LOG(WARNING, "received incorrect amount of data\n");
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
index c829d53..9cd24e1 100644
--- a/aos/common/sensors/sensor_receiver.h
+++ b/aos/common/sensors/sensor_receiver.h
@@ -53,6 +53,8 @@
// Subclasses need to implement this to read 1 set of data (blocking until it
// is available) into data().
+ // It needs to have the correct byte order etc and not be corrupted
+ // (subclasses can check the checksum if they want).
virtual void DoReceiveData() = 0;
// Optional: if subclasses can do anything to reinitialize after there are
@@ -60,6 +62,10 @@
// This will be called right before calling DoReceiveData() the first time.
virtual void Reset() {}
+ // Optional: if subclasses want to be notified when this is first convinced
+ // that it has a good packet, they should do whatever here.
+ virtual void Synchronized(time::Time /*last_packet_ideal_time*/) {}
+
// Returns whether the current packet looks like a good one to use.
bool GoodPacket();
diff --git a/aos/common/time.cc b/aos/common/time.cc
index 995011a..75ea196 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -120,6 +120,9 @@
const Time Time::operator/(int32_t rhs) const {
return Time(*this) /= rhs;
}
+double Time::operator/(const Time &rhs) const {
+ return ToSeconds() / rhs.ToSeconds();
+}
Time &Time::operator%=(int32_t rhs) {
nsec_ = ToNSec() % rhs;
const int wraps = nsec_ / kNSecInSec;
diff --git a/aos/common/time.h b/aos/common/time.h
index ea703f7..276cf1c 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -160,6 +160,8 @@
const Time operator-(const Time &rhs) const;
const Time operator*(int32_t rhs) const;
const Time operator/(int32_t rhs) const;
+ // TODO(brians) test this
+ double operator/(const Time &rhs) const;
const Time operator%(int32_t rhs) const;
bool operator==(const Time &rhs) const;
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 710a5d4..e9cfeff 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -28,6 +28,7 @@
#'camera/camera.gyp:frc971',
'../../gyro_board/src/libusb-driver/libusb-driver.gyp:get',
'../input/input.gyp:gyro_board_reader',
+ '../input/input.gyp:gyro_sensor_receiver',
],
'copies': [
{
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
new file mode 100644
index 0000000..aedc9a3
--- /dev/null
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -0,0 +1,270 @@
+#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) <= sizeof(data_),
+ "the buffer is 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;
+
+ virtual void DoReceiveData() {
+ // Loop and then return once we get a good one.
+ 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 < static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ read_bytes, sizeof(GyroBoardData));
+ continue;
+ }
+
+ memcpy(&data()->values, 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() {
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb_.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(FATAL, "couldn't find device\n");
+ }
+ data()->count = 0;
+ }
+
+ virtual void Synchronized(::aos::time::Time start_time) {
+ LOG(INFO, "offset %f\n", (::aos::time::Time::Now() - start_time).ToSeconds());
+ start_time_ = start_time -
+ ::aos::sensors::kSensorSendFrequency * data()->count;
+ }
+
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+
+ ::aos::time::Time start_time_;
+
+ LibUSB libusb_;
+
+ uint8_t data_[64];
+};
+
+} // namespace frc971
+
+int main() {
+ ::aos::Init();
+ ::frc971::GyroSensorUnpacker unpacker;
+ ::frc971::GyroSensorReceiver receiver(&unpacker);
+ while (true) {
+ receiver.RunIteration();
+ }
+ ::aos::Cleanup();
+}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 447ded3..45f68a2 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -49,6 +49,27 @@
],
},
{
+ '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': [
@@ -65,8 +86,8 @@
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
'<(EXTERNALS):libusb',
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:timing',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:timing',
],
},
{
diff --git a/gyro_board/src/libusb-driver/libusb_wrap.cc b/gyro_board/src/libusb-driver/libusb_wrap.cc
index 72fc58e..13a5e31 100644
--- a/gyro_board/src/libusb-driver/libusb_wrap.cc
+++ b/gyro_board/src/libusb-driver/libusb_wrap.cc
@@ -81,8 +81,7 @@
libusb_device_handle *dev_handle) : dev_handle_(dev_handle) { }
LibUSBDeviceHandle::~LibUSBDeviceHandle() {
- int r;
- r = libusb_release_interface(dev_handle_, 0);
+ int r = libusb_release_interface(dev_handle_, 0);
if (r != 0) {
LOG(FATAL, "Cannot Release Interface\n");
}