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/ahal/Spark.cc b/frc971/wpilib/ahal/Spark.cc
index deb891b..502ff31 100644
--- a/frc971/wpilib/ahal/Spark.cc
+++ b/frc971/wpilib/ahal/Spark.cc
@@ -26,7 +26,7 @@
* 0.999ms = full "reverse"
*/
SetBounds(2.003, 1.55, 1.50, 1.46, .999);
- SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
diff --git a/frc971/wpilib/ahal/Talon.cc b/frc971/wpilib/ahal/Talon.cc
index e3af567..7caabc0 100644
--- a/frc971/wpilib/ahal/Talon.cc
+++ b/frc971/wpilib/ahal/Talon.cc
@@ -32,7 +32,7 @@
* 0.989ms = full "reverse"
*/
SetBounds(2.037, 1.539, 1.513, 1.487, .989);
- SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
diff --git a/frc971/wpilib/ahal/TalonFX.cc b/frc971/wpilib/ahal/TalonFX.cc
index 93dc62e..fe29089 100644
--- a/frc971/wpilib/ahal/TalonFX.cc
+++ b/frc971/wpilib/ahal/TalonFX.cc
@@ -33,7 +33,7 @@
* 0.997ms = full "reverse"
*/
SetBounds(2.004, 1.52, 1.50, 1.48, .997);
- SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
diff --git a/frc971/wpilib/ahal/VictorSP.cc b/frc971/wpilib/ahal/VictorSP.cc
index fee03a9..249202d 100644
--- a/frc971/wpilib/ahal/VictorSP.cc
+++ b/frc971/wpilib/ahal/VictorSP.cc
@@ -33,7 +33,7 @@
* 0.997ms = full "reverse"
*/
SetBounds(2.004, 1.52, 1.50, 1.48, .997);
- SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetPeriodMultiplier(kPeriodMultiplier_2X);
SetSpeed(0.0);
SetZeroLatch();
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_);
}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp b/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
index 19a3b83..83a0ca5 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
@@ -430,6 +430,7 @@
void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
int32_t* status) {
+ pwmSystem->writeConfig_Period(5050 / 2, status);
auto port = digitalChannelHandles->Get(pwmPortHandle, HAL_HandleEnum::PWM);
if (port == nullptr) {
*status = HAL_HANDLE_ERROR;
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index b1f386c..e538c2c 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -664,6 +664,7 @@
// Thread 3.
::aos::ShmEventLoop sensor_reader_event_loop(&config.message());
SensorReader sensor_reader(&sensor_reader_event_loop, values);
+ sensor_reader.set_pwm_trigger(true);
sensor_reader.set_drivetrain_left_encoder(make_encoder(1));
sensor_reader.set_drivetrain_right_encoder(make_encoder(0));