Remove 1 cycle delay in FPGA writes
The FPGA is only taking updates to lengthen the cycle before the next
cycle starts. This is adding 1 cycle of delay when decelerating our
catapult.
The FPGA supports changing the period. So, we can set the period to
1/2, and then skip every other period. This produces the same waveform,
but effectively updates the active FPGA value more frequently.
Change-Id: Ib239f09ff4da652a9fbdf0aa0e552eb71b1d2054
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 6c47214..bb99dc8 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -14,6 +14,9 @@
#include "frc971/wpilib/wpilib_interface.h"
#include "hal/PWM.h"
+DEFINE_int32(pwm_offset, 5050 / 2,
+ "Offset of reading the sensors from the start of the PWM cycle");
+
namespace frc971 {
namespace wpilib {
@@ -125,12 +128,17 @@
last_monotonic_now_ = monotonic_now;
monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
+ VLOG(1) << "Start time " << last_tick_timepoint << " period " << period_.count();
if (last_tick_timepoint == monotonic_clock::min_time) {
return;
}
last_tick_timepoint +=
- ((monotonic_now - last_tick_timepoint) / period_) * period_;
+ ((monotonic_now - chrono::microseconds(FLAGS_pwm_offset) -
+ last_tick_timepoint) /
+ period_) *
+ period_ + chrono::microseconds(FLAGS_pwm_offset);
+ VLOG(1) << "Now " << monotonic_now << " tick " << last_tick_timepoint;
// If it's over 1/2 of a period back in time, that's wrong. Move it
// forwards to now.
if (last_tick_timepoint - monotonic_now < -period_ / 2) {
@@ -142,7 +150,7 @@
// errors in waking up. The PWM cycle starts at the falling edge of the
// PWM pulse.
const auto next_time =
- last_tick_timepoint + period_ + chrono::microseconds(50);
+ last_tick_timepoint + period_;
timer_handler_->Setup(next_time, period_);
}