Clean up a few after-merge idiosyncrasies.
- Get the bot3 input code up to date. (Again.)
- Get rid of old gyro_reader.cc file, which somehow survived.
diff --git a/bot3/atom_code/atom_code.gyp b/bot3/atom_code/atom_code.gyp
index 337e298..1b1ba1e 100644
--- a/bot3/atom_code/atom_code.gyp
+++ b/bot3/atom_code/atom_code.gyp
@@ -11,13 +11,11 @@
#'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto',
'<(DEPTH)/bot3/input/input.gyp:joystick_reader',
- '<(DEPTH)/bot3/input/input.gyp:gyro_reader',
#'../input/input.gyp:AutoMode',
'<(DEPTH)/bot3/output/output.gyp:MotorWriter',
'<(DEPTH)/bot3/output/output.gyp:CameraServer',
#'camera/camera.gyp:frc971',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:get',
- '<(DEPTH)/bot3/input/input.gyp:gyro_board_reader',
'<(DEPTH)/bot3/input/input.gyp:gyro_sensor_receiver',
],
'copies': [
diff --git a/bot3/input/gyro_board_reader.cc b/bot3/input/gyro_board_reader.cc
deleted file mode 100644
index a5f961f..0000000
--- a/bot3/input/gyro_board_reader.cc
+++ /dev/null
@@ -1,187 +0,0 @@
-#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 "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "bot3/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 ::bot3::control_loops::drivetrain;
-using ::frc971::sensors::gyro;
-
-namespace bot3 {
-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);
-}
-
-// TODO(daniel): This might have to change if I find out that the gear ratios are different.
-inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (15.0 / 34.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) {
- }
-
- 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_);
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
- .Send();
-
- //TODO (danielp): Add stuff for bot3 shooter.
- }
-
- ::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_;
-};
-
-} // namespace bot3
-
-int main() {
- ::aos::Init();
- ::bot3::GyroBoardReader reader;
- reader.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/bot3/input/gyro_sensor_receiver.cc b/bot3/input/gyro_sensor_receiver.cc
index 6553e3a..f53f15b 100644
--- a/bot3/input/gyro_sensor_receiver.cc
+++ b/bot3/input/gyro_sensor_receiver.cc
@@ -1,17 +1,10 @@
-#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 "aos/common/util/wrapping_counter.h"
#include "bot3/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/input/gyro_board_data.h"
-#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/input/usb_receiver.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -19,7 +12,8 @@
using ::bot3::control_loops::drivetrain;
using ::frc971::sensors::gyro;
-using ::frc971::GyroBoardData;
+using ::aos::util::WrappingCounter;
+using ::frc971::USBReceiver;
namespace bot3 {
namespace {
@@ -54,203 +48,40 @@
} // 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");
+class GyroSensorReceiver : public USBReceiver {
+ virtual void ProcessData() override {
+ if (data()->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n",
+ data()->robot_id, data()->base_status & 0xF);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n",
+ data()->base_status & 0xF);
}
gyro.MakeWithBuilder()
- .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .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))
+ .right_encoder(drivetrain_translate(data()->main.right_drive))
+ .left_encoder(-drivetrain_translate(data()->main.left_drive))
.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_;
+ WrappingCounter top_rise_;
+ WrappingCounter top_fall_;
+ WrappingCounter bottom_rise_;
+ WrappingCounter bottom_fall_delay_;
+ WrappingCounter bottom_fall_;
};
} // namespace bot3
int main() {
::aos::Init();
- ::bot3::GyroSensorUnpacker unpacker;
- ::bot3::GyroSensorReceiver receiver(&unpacker);
+ ::bot3::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
}
diff --git a/bot3/input/input.gyp b/bot3/input/input.gyp
index 5beb3d0..b428dc2 100644
--- a/bot3/input/input.gyp
+++ b/bot3/input/input.gyp
@@ -10,6 +10,7 @@
'<(AOS)/atom_code/input/input.gyp:joystick_input',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
+
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(DEPTH)/bot3/autonomous/autonomous.gyp:auto_queue',
@@ -25,40 +26,28 @@
'<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(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',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ 'usb_receiver',
],
},
{
- 'target_name': 'gyro_board_reader',
- 'type': 'executable',
+ 'target_name': 'usb_receiver',
+ 'type': 'static_library',
'sources': [
- 'gyro_board_reader.cc',
+ '<(DEPTH)/frc971/input/usb_receiver.cc',
],
'dependencies': [
- '<(DEPTH)/bot3/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(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/util/util.gyp:wrapping_counter',
'<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:timing',
+ '<(AOS)/common/common.gyp:controls',
],
- },
- {
- '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',
+ 'export_dependent_settings': [
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ '<(AOS)/common/common.gyp:time',
],
},
],