Add hysterisis for vacuum.
When disabled, we want to not say that we dropped it for longer to avoid
dropping at the end of auto.
Change-Id: I93ede157b89908fc9b5879491b52d2dad3a96e13
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 497ae5b..956bb79 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -1,9 +1,15 @@
#include "y2019/control_loops/superstructure/vacuum.h"
+#include <chrono>
+
+#include "y2019/control_loops/superstructure/superstructure.q.h"
+
namespace y2019 {
namespace control_loops {
namespace superstructure {
+namespace chrono = ::std::chrono;
+
constexpr double Vacuum::kPumpVoltage;
constexpr double Vacuum::kPumpHasPieceVoltage;
constexpr aos::monotonic_clock::duration Vacuum::kTimeAtHigherVoltage;
@@ -19,7 +25,10 @@
filtered_pressure_ = kSuctionAlpha * suction_pressure +
(1 - kSuctionAlpha) * filtered_pressure_;
- const bool new_has_piece = filtered_pressure_ < kVacuumThreshold;
+ const bool new_has_piece =
+ filtered_pressure_ < (filtered_had_piece_near_disabled_
+ ? kVacuumOffThreshold
+ : kVacuumOnThreshold);
if (new_has_piece && !had_piece_) {
time_at_last_acquisition_ = monotonic_now;
@@ -28,8 +37,13 @@
monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage &&
new_has_piece;
+ if (!output && *has_piece) {
+ last_disable_has_piece_time_ = monotonic_now;
+ }
+
+
// If we've had the piece for enough time, go to lower pump_voltage
- low_pump_voltage = *has_piece;
+ low_pump_voltage = *has_piece && filtered_pressure_ < kVacuumOnThreshold;
if (unsafe_goal && output) {
const bool release = !unsafe_goal->grab_piece;
@@ -63,6 +77,10 @@
}
}
had_piece_ = new_has_piece;
+
+ filtered_had_piece_near_disabled_ =
+ *has_piece &&
+ monotonic_now < last_disable_has_piece_time_ + chrono::milliseconds(250);
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index f0e97e9..e793530 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -1,7 +1,7 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
-#include "aos/controls/control_loop.h"
+#include "aos/events/event-loop.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
namespace y2019 {
@@ -33,11 +33,17 @@
bool had_piece_ = false;
aos::monotonic_clock::time_point last_release_time_ =
aos::monotonic_clock::epoch();
+ // Time since the last time we had a game piece while disabled.
+ aos::monotonic_clock::time_point last_disable_has_piece_time_ =
+ aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point time_at_last_acquisition_ =
aos::monotonic_clock::epoch();
double filtered_pressure_ = 1.0;
- static constexpr double kVacuumThreshold = 0.70;
+ bool filtered_had_piece_near_disabled_ = false;
+
+ static constexpr double kVacuumOnThreshold = 0.70;
+ static constexpr double kVacuumOffThreshold = 0.85;
static constexpr double kFilterTimeConstant = 0.1;
static constexpr double dt = .00505;