redid gyro_sensor_receiver so it should work with the new isoc stuff
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 023bc5f..c166a13 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -1,13 +1,15 @@
+#include <string.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/glibusb/glibusb.h"
#include "aos/common/glibusb/gbuffer.h"
+#include "aos/common/util/wrapping_counter.h"
+#include "aos/common/control_loop/ControlLoop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
@@ -27,6 +29,7 @@
using ::frc971::control_loops::shooter;
using ::frc971::control_loops::index_loop;
using ::frc971::sensors::gyro;
+using ::aos::util::WrappingCounter;
namespace frc971 {
namespace {
@@ -61,168 +64,184 @@
} // namespace
-class GyroSensorUnpacker :
- public ::aos::sensors::SensorUnpackerInterface<GyroBoardData> {
+class GyroSensorReceiver {
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) {
+ GyroSensorReceiver() {
+ Reset();
}
- void UnpackFrom(GyroBoardData *data) {
- 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;
+ void RunIteration() {
+ if (ReceiveData()) {
+ Reset();
} else {
- LOG(DEBUG, "processing a packet dip switches %x\n",
- data->base_status & 0xF);
+ const ::aos::time::Time received_time = ::aos::time::Time::Now();
+ if (phase_locker_.IsCurrentPacketGood(received_time, sequence_.count())) {
+ ProcessData();
+ }
}
-
- 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->main.top_rise_count,
- &last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->main.top_fall_count,
- &last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->main.bottom_rise_count,
- &last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->main.bottom_fall_delay_count,
- &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->main.bottom_fall_count,
- &last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->main.wrist_rise_count,
- &last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->main.shooter_angle_rise_count,
- &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->main.right_drive))
- .left_encoder(-drivetrain_translate(data->main.left_drive))
- .Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->main.wrist))
- .hall_effect(!data->main.wrist_hall_effect)
- .calibration(wrist_translate(data->main.capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->main.shooter_angle))
- .bottom_hall_effect(!data->main.angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->main.capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->main.shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->main.indexer))
- .top_disc_detect(!data->main.top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->main.capture_top_rise))
- .top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->main.capture_top_fall))
- .bottom_disc_detect(!data->main.bottom_disc)
- .bottom_disc_posedge_count(bottom_rise_count_)
- .bottom_disc_negedge_count(bottom_fall_count_)
- .bottom_disc_negedge_wait_position(index_translate(
- data->main.capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
- .loader_top(data->main.loader_top)
- .loader_bottom(data->main.loader_bottom)
- .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
+ static const unsigned char kEndpoint = 0x83;
// 0 is unlimited
- static const unsigned int kReadTimeout = 1000;
+ static constexpr ::aos::time::Time kReadTimeout =
+ ::aos::time::Time::InSeconds(1.5);
+ static constexpr ::glibusb::VendorProductId kDeviceId =
+ ::glibusb::VendorProductId(0x1424 /* vendor ID */,
+ 0xd243 /* product ID */);
- static constexpr glibusb::VendorProductId kDeviceId =
- glibusb::VendorProductId(0x1424 /* vendor ID */,
- 0xd243 /* product ID */);
+ static const int kPacketsPerLoopCycle = 10;
- // How big of a buffer to give the function.
- static const size_t kDataLength = 64;
+ // Contains all of the complicated state and logic for locking onto the the
+ // correct phase.
+ class {
+ public:
+ void Reset() {
+ last_guessed_time_ = ::aos::time::Time(0, 0);
+ good_phase_ = guess_phase_ = kUnknownPhase;
+ guess_phase_good_ = guess_phase_bad_ = 0;
+ good_phase_early_ = good_phase_late_ = 0;
+ }
- virtual bool DoReceiveData() {
+ // Gets called for every packet received.
+ // Returns whether or not to process the values from this packet.
+ bool IsCurrentPacketGood(const ::aos::time::Time &received_time,
+ int32_t sequence) {
+ // How often we (should) receive packets.
+ static const ::aos::time::Time kPacketFrequency =
+ ::aos::control_loops::kLoopFrequency / kPacketsPerLoopCycle;
+
+ // When we want to receive a packet for the next cycle of control loops.
+ const ::aos::time::Time next_desired =
+ ::aos::control_loops::NextLoopTime(received_time + kDesiredOffset);
+ // How far off of when we want the next packet this one is.
+ const ::aos::time::Time offset = next_desired - received_time;
+
+ const int received_phase = sequence % kPacketsPerLoopCycle;
+
+ assert(!(good_phase_early_ != 0 && good_phase_late_ != 0));
+
+ if (guess_phase_good_ > kMinGoodGuessCycles) {
+ good_phase_ = guess_phase_;
+ if (guess_phase_offset_ < kPacketFrequency * -0.5) {
+ ++good_phase_;
+ } else if (guess_phase_offset_ > kPacketFrequency * 0.5) {
+ --good_phase_;
+ }
+ } else if (guess_phase_bad_ > kMaxBadGuessCycles) {
+ Reset();
+ }
+ if (good_phase_early_ > kSwitchCycles) {
+ good_phase_early_ = 0;
+ --good_phase_;
+ } else if (good_phase_late_ > kSwitchCycles) {
+ good_phase_late_ = 0;
+ ++good_phase_;
+ }
+ if (good_phase_ == kUnknownPhase) {
+ LOG(INFO, "guessing which packet is good\n");
+
+ // If we're going to call this packet a good guess.
+ bool guess_is_good = false;
+ // If it's close to the right time.
+ if (offset.abs() < kPacketFrequency * 0.65) {
+ // If we didn't (also) guess that the previous one was good.
+ if (received_time - last_guessed_time_ > kPacketFrequency * 2) {
+ guess_is_good = true;
+ }
+ if (guess_phase_ == kUnknownPhase) {
+ if (offset.abs() < kPacketFrequency * 0.55) {
+ guess_phase_ = received_phase;
+ guess_phase_offset_ = offset;
+ }
+ } else if (received_phase == guess_phase_) {
+ ++guess_phase_good_;
+ guess_phase_bad_ = 0;
+ guess_phase_offset_ = (guess_phase_offset_ * 9 + offset) / 10;
+ }
+ } else if (guess_phase_ != kUnknownPhase &&
+ received_phase == guess_phase_) {
+ ++guess_phase_bad_;
+ guess_phase_good_ = ::std::max(0, guess_phase_good_ -
+ (kMinGoodGuessCycles / 10));
+ }
+ if (guess_is_good) {
+ last_guessed_time_ = received_time;
+ return true;
+ } else {
+ return false;
+ }
+ } else { // we know what phase we're looking for
+ // Deal with it if the above logic for tweaking the phase that we're
+ // using wrapped it around.
+ if (good_phase_ == -1) {
+ good_phase_ = kPacketsPerLoopCycle;
+ } else if (good_phase_ == kPacketsPerLoopCycle) {
+ good_phase_ = 0;
+ }
+ assert(good_phase_ >= 0);
+ assert(good_phase_ < kPacketsPerLoopCycle);
+
+ if (received_phase == good_phase_) {
+ if (offset < kPacketFrequency * -0.6) {
+ ++good_phase_early_;
+ good_phase_late_ = 0;
+ } else if (offset > kPacketFrequency * 0.6) {
+ ++good_phase_late_;
+ good_phase_early_ = 0;
+ } else {
+ good_phase_early_ = good_phase_late_ = 0;
+ }
+ return true;
+ } else {
+ return false;
+ }
+ }
+ }
+
+ private:
+ // How long before the control loops run we want to use a packet.
+ static constexpr ::aos::time::Time kDesiredOffset =
+ ::aos::time::Time::InSeconds(-0.0025);
+
+ // How many times the packet we guessed has to be close to right to use the
+ // guess.
+ static const int kMinGoodGuessCycles = 30;
+ // How many times in a row we have to guess the wrong packet before trying
+ // again.
+ static const int kMaxBadGuessCycles = 3;
+
+ // How many times in a row a different packet has to be better than the one
+ // that we're using befor switching to it.
+ static const int kSwitchCycles = 15;
+
+ ::aos::time::Time last_guessed_time_{0, 0};
+
+ const int kUnknownPhase = -11;
+ // kUnknownPhase or the sequence number (%kPacketsPerLoopCycle) to
+ // use or think about using.
+ // If not kUnknownPhase, 0 <= these < kPacketsPerLoopCycle.
+ int good_phase_, guess_phase_;
+ int guess_phase_good_, guess_phase_bad_;
+ ::aos::time::Time guess_phase_offset_{0, 0};
+ int good_phase_early_, good_phase_late_;
+ } phase_locker_;
+
+ // Returns true if receiving failed and we should try a Reset().
+ bool ReceiveData() {
// Loop and then return once we get a good one.
while (true) {
- using glibusb::UsbEndpoint;
+ using ::glibusb::UsbEndpoint;
UsbEndpoint::IoStatus result =
endpoint_->ReadAtMostWithTimeout(sizeof(GyroBoardData),
- kReadTimeout,
+ kReadTimeout.ToMSec(),
&buffer_);
switch (result) {
case UsbEndpoint::kSuccess:
break;
case UsbEndpoint::kTimeout:
LOG(WARNING, "read timed out\n");
- continue;
+ return true;
case UsbEndpoint::kNoDevice:
LOG(ERROR, "no device\n");
return true;
@@ -232,58 +251,119 @@
LOG(ERROR, "read failed\n");
continue;
}
- if (buffer_.Length() < sizeof(GyroBoardData)) {
- LOG(ERROR, "read %zd bytes instead of at least %zd\n",
- buffer_.Length(), sizeof(GyroBoardData));
- continue;
- }
-
- memcpy(&data()->values, buffer_.GetBufferPointer(sizeof(GyroBoardData)),
- 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 false;
}
+ sequence_.Update(data()->sequence);
+ return false;
}
- virtual void Reset() {
- device_ = ::std::unique_ptr<glibusb::UsbDevice>(
+ GyroBoardData *data() {
+ return static_cast<GyroBoardData *>(
+ buffer_.GetBufferPointer(sizeof(GyroBoardData)));
+ }
+
+ void Reset() {
+ // Make sure to delete the endpoint before its device.
+ endpoint_.reset();
+ device_ = ::std::unique_ptr< ::glibusb::UsbDevice>(
libusb_.FindSingleMatchingDeviceOrLose(kDeviceId));
CHECK(device_);
- endpoint_ = ::std::unique_ptr<glibusb::UsbInEndpoint>(
+ endpoint_ = ::std::unique_ptr< ::glibusb::UsbInEndpoint>(
device_->InEndpoint(kEndpoint));
-
- data()->count = 0;
+ sequence_.Reset();
+ phase_locker_.Reset();
}
- 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;
+ void ProcessData() {
+ 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);
+ }
+
+ static ::aos::time::Time last_time = ::aos::time::Time::Now();
+ if ((last_time - ::aos::time::Time::Now()) >
+ ::aos::time::Time::InMS(0.0011)) {
+ LOG(INFO, "missed one\n");
+ }
+
+ gyro.MakeWithBuilder()
+ .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .Send();
+
+ drivetrain.position.MakeWithBuilder()
+ .right_encoder(drivetrain_translate(data()->main.right_drive))
+ .left_encoder(-drivetrain_translate(data()->main.left_drive))
+ .Send();
+
+ wrist.position.MakeWithBuilder()
+ .pos(wrist_translate(data()->main.wrist))
+ .hall_effect(!data()->main.wrist_hall_effect)
+ .calibration(wrist_translate(data()->main.capture_wrist_rise))
+ .Send();
+
+ angle_adjust.position.MakeWithBuilder()
+ .angle(angle_adjust_translate(data()->main.shooter_angle))
+ .bottom_hall_effect(!data()->main.angle_adjust_bottom_hall_effect)
+ .middle_hall_effect(false)
+ .bottom_calibration(angle_adjust_translate(
+ data()->main.capture_shooter_angle_rise))
+ .middle_calibration(angle_adjust_translate(
+ 0))
+ .Send();
+
+ shooter.position.MakeWithBuilder()
+ .position(shooter_translate(data()->main.shooter))
+ .Send();
+
+ index_loop.position.MakeWithBuilder()
+ .index_position(index_translate(data()->main.indexer))
+ .top_disc_detect(!data()->main.top_disc)
+ .top_disc_posedge_count(top_rise_.Update(data()->main.top_rise_count))
+ .top_disc_posedge_position(
+ index_translate(data()->main.capture_top_rise))
+ .top_disc_negedge_count(top_fall_.Update(data()->main.top_fall_count))
+ .top_disc_negedge_position(
+ index_translate(data()->main.capture_top_fall))
+ .bottom_disc_detect(!data()->main.bottom_disc)
+ .bottom_disc_posedge_count(
+ bottom_rise_.Update(data()->main.bottom_rise_count))
+ .bottom_disc_negedge_count(
+ bottom_fall_.Update(data()->main.bottom_fall_count))
+ .bottom_disc_negedge_wait_position(index_translate(
+ data()->main.capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(
+ bottom_fall_delay_.Update(data()->main.bottom_fall_delay_count))
+ .loader_top(data()->main.loader_top)
+ .loader_bottom(data()->main.loader_bottom)
+ .Send();
}
- ::std::unique_ptr<glibusb::UsbDevice> device_;
- ::std::unique_ptr<glibusb::UsbInEndpoint> endpoint_;
- glibusb::Buffer buffer_;
+ ::std::unique_ptr< ::glibusb::UsbDevice> device_;
+ ::std::unique_ptr< ::glibusb::UsbInEndpoint> endpoint_;
+ ::glibusb::Buffer buffer_;
- ::aos::time::Time start_time_;
+ WrappingCounter sequence_;
- glibusb::Libusb libusb_;
+ ::glibusb::Libusb libusb_;
+
+ WrappingCounter top_rise_;
+ WrappingCounter top_fall_;
+ WrappingCounter bottom_rise_;
+ WrappingCounter bottom_fall_delay_;
+ WrappingCounter bottom_fall_;
};
-constexpr glibusb::VendorProductId GyroSensorReceiver::kDeviceId;
+constexpr ::glibusb::VendorProductId GyroSensorReceiver::kDeviceId;
+constexpr ::aos::time::Time GyroSensorReceiver::kReadTimeout;
} // namespace frc971
int main() {
::aos::Init();
- ::frc971::GyroSensorUnpacker unpacker;
- ::frc971::GyroSensorReceiver receiver(&unpacker);
+ ::frc971::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
}