Update vacuum logic for mechanical change
Change-Id: I78ea0c48df5a472ee2c38a5e072c2b0e540cb315
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 4efb95f..0cce4a6 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -350,9 +350,9 @@
CheckCollisions();
}
- const double loop_time =
- chrono::duration_cast<chrono::duration<double>>(
- monotonic_clock::now() - loop_start_time).count();
+ const double loop_time = chrono::duration_cast<chrono::duration<double>>(
+ monotonic_clock::now() - loop_start_time)
+ .count();
const double elevator_acceleration =
(superstructure_plant_.elevator_velocity() -
@@ -718,9 +718,8 @@
ASSERT_TRUE(goal.Send());
}
- RunForTime(
- Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10),
- true, false);
+ RunForTime(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10), true,
+ false);
// Verify that at 0 pressure after short time voltage is still 12
superstructure_plant_.set_simulated_pressure(0.0);
@@ -743,9 +742,8 @@
// Verify that at 0 pressure after short time voltage is still high
superstructure_plant_.set_simulated_pressure(0.0);
- RunForTime(
- Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10),
- true, false);
+ RunForTime(Vacuum::kTimeAtHigherVoltage - chrono::milliseconds(10), true,
+ false);
superstructure_queue_.output.FetchLatest();
EXPECT_EQ(superstructure_queue_.output->pump_voltage, Vacuum::kPumpVoltage);
@@ -757,8 +755,8 @@
Vacuum::kPumpHasPieceVoltage);
}
-// Tests the Vacuum stays on for a bit after getting a no suck goal
-TEST_F(SuperstructureTest, VacuumStaysOn) {
+// Tests the Vacuum stops immediately after getting a no suck goal
+TEST_F(SuperstructureTest, VacuumStopsQuickly) {
WaitUntilZeroed();
// Turn on suction
{
@@ -784,16 +782,8 @@
}
superstructure_plant_.set_simulated_pressure(1.0);
- // Run for a short while and make sure we still ask for non-zero volts
- RunForTime(Vacuum::kTimeToKeepPumpRunning -
- chrono::milliseconds(10),
- true, false);
- superstructure_queue_.output.FetchLatest();
- EXPECT_GE(superstructure_queue_.output->pump_voltage,
- Vacuum::kPumpHasPieceVoltage);
-
- // Wait and make sure the vacuum actually turns off
- RunForTime(chrono::seconds(2));
+ // Run for a short while and check that voltage dropped to 0
+ RunForTime(chrono::milliseconds(10), true, false);
superstructure_queue_.output.FetchLatest();
EXPECT_EQ(superstructure_queue_.output->pump_voltage, 0.0);
}
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index d4c1c6b..7bf2e94 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -7,14 +7,13 @@
constexpr double Vacuum::kPumpVoltage;
constexpr double Vacuum::kPumpHasPieceVoltage;
constexpr aos::monotonic_clock::duration Vacuum::kTimeAtHigherVoltage;
-constexpr aos::monotonic_clock::duration Vacuum::kTimeToKeepPumpRunning;
+constexpr aos::monotonic_clock::duration Vacuum::kReleaseTime;
void Vacuum::Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
SuperstructureQueue::Output *output, bool *has_piece,
aos::EventLoop *event_loop) {
auto monotonic_now = event_loop->monotonic_now();
bool low_pump_voltage = false;
- bool no_goal_for_a_bit = false;
// implement a simple low-pass filter on the pressure
filtered_pressure_ = kSuctionAlpha * suction_pressure +
@@ -29,26 +28,31 @@
monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage &&
new_has_piece;
- // if we've had the piece for enought time, go to lower pump_voltage
+ // If we've had the piece for enough time, go to lower pump_voltage
low_pump_voltage = *has_piece;
- no_goal_for_a_bit =
- monotonic_now > time_at_last_evacuate_goal_ + kTimeToKeepPumpRunning;
if (unsafe_goal && output) {
- const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
- if (evacuate) {
- time_at_last_evacuate_goal_ = monotonic_now;
+ const bool release = !unsafe_goal->top && !unsafe_goal->bottom;
+
+ if (release) {
+ last_release_time_ = monotonic_now;
}
// Once the vacuum evacuates, the pump speeds up because there is no
// resistance. So, we want to turn it down to save the pump from
// overheating.
output->pump_voltage =
- (no_goal_for_a_bit) ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage
- : kPumpVoltage);
+ release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
output->intake_suction_top = unsafe_goal->top;
output->intake_suction_bottom = unsafe_goal->bottom;
+
+ // If we intend to release, or recently released, set has_piece to false so
+ // that we give the part of the vacuum circuit with the pressure sensor time
+ // to equilibrate with the rest of the suction cup.
+ if (release || monotonic_now < last_release_time_ + kReleaseTime) {
+ *has_piece = false;
+ }
}
had_piece_ = new_has_piece;
}
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index d2acf97..f0e97e9 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -1,8 +1,8 @@
#ifndef Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
#define Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_VACUUM_H_
-#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "aos/controls/control_loop.h"
+#include "y2019/control_loops/superstructure/superstructure.q.h"
namespace y2019 {
namespace control_loops {
@@ -15,7 +15,6 @@
SuperstructureQueue::Output *output, bool *has_piece,
aos::EventLoop *event_loop);
-
// Voltage to the vaccum pump when we are attempting to acquire a piece
static constexpr double kPumpVoltage = 8.0;
@@ -26,13 +25,13 @@
static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
std::chrono::milliseconds(100);
- // Time to continue the pump after getting a no suck goal
- static constexpr aos::monotonic_clock::duration kTimeToKeepPumpRunning =
- std::chrono::milliseconds(750);
+ // Time required for the game piece to be released from a vacuum
+ static constexpr aos::monotonic_clock::duration kReleaseTime =
+ std::chrono::milliseconds(250);
private:
bool had_piece_ = false;
- aos::monotonic_clock::time_point time_at_last_evacuate_goal_ =
+ aos::monotonic_clock::time_point last_release_time_ =
aos::monotonic_clock::epoch();
aos::monotonic_clock::time_point time_at_last_acquisition_ =
aos::monotonic_clock::epoch();
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 221bb51..aa29bc9 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -383,10 +383,10 @@
if (superstructure_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *superstructure_);
- big_suction_cup0_->Set(superstructure_->intake_suction_top);
- big_suction_cup1_->Set(superstructure_->intake_suction_top);
- small_suction_cup0_->Set(superstructure_->intake_suction_bottom);
- small_suction_cup1_->Set(superstructure_->intake_suction_bottom);
+ big_suction_cup0_->Set(!superstructure_->intake_suction_bottom);
+ big_suction_cup1_->Set(!superstructure_->intake_suction_bottom);
+ small_suction_cup0_->Set(superstructure_->intake_suction_top);
+ small_suction_cup1_->Set(superstructure_->intake_suction_top);
intake_rollers_talon_->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,