Added Vacuum class and test
Change-Id: I4c25946b71a0054791a2fc1305eb8ef509a147b1
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index f55e1ad..00bacae 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -24,6 +24,7 @@
],
deps = [
":collision_avoidance",
+ ":vacuum",
":superstructure_queue",
"//aos/controls:control_loop",
"//y2019:constants",
@@ -77,6 +78,20 @@
],
)
+cc_library(
+ name = "vacuum",
+ srcs = [
+ "vacuum.cc",
+ ],
+ hdrs = [
+ "vacuum.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/controls:control_loop"
+ ],
+)
+
cc_test(
name = "collision_avoidance_tests",
srcs = [
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 187379b..3443d63 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -8,35 +8,6 @@
namespace control_loops {
namespace superstructure {
-void Superstructure::HandleSuction(const SuctionGoal *unsafe_goal,
- float suction_pressure,
- SuperstructureQueue::Output *output,
- bool *has_piece) {
- constexpr double kPumpVoltage = 12.0;
- constexpr double kPumpHasPieceVoltage = 8.0;
-
- // TODO(austin): Low pass filter on pressure.
- *has_piece = suction_pressure < 0.70;
-
- if (unsafe_goal && output) {
- const bool evacuate = unsafe_goal->top || unsafe_goal->bottom;
- if (evacuate) {
- vacuum_count_ = 200;
- }
- // TODO(austin): High speed pump a bit longer after we detect we have the
- // game piece.
- // 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 =
- (vacuum_count_ > 0) ? (*has_piece ? kPumpHasPieceVoltage : kPumpVoltage)
- : 0.0;
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
- }
- vacuum_count_ = ::std::max(0, vacuum_count_ - 1);
-}
-
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -77,8 +48,9 @@
output != nullptr ? &(output->stilts_voltage) : nullptr,
&(status->stilts));
- HandleSuction(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
- position->suction_pressure, output, &(status->has_piece));
+ vacuum_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->suction) : nullptr,
+ position->suction_pressure, output, &(status->has_piece),
+ event_loop());
status->zeroed = status->elevator.zeroed && status->wrist.zeroed &&
status->intake.zeroed && status->stilts.zeroed;
diff --git a/y2019/control_loops/superstructure/superstructure.h b/y2019/control_loops/superstructure/superstructure.h
index 9879e17..626c84b 100644
--- a/y2019/control_loops/superstructure/superstructure.h
+++ b/y2019/control_loops/superstructure/superstructure.h
@@ -6,6 +6,7 @@
#include "y2019/constants.h"
#include "y2019/control_loops/superstructure/collision_avoidance.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/control_loops/superstructure/vacuum.h"
namespace y2019 {
namespace control_loops {
@@ -32,6 +33,7 @@
const PotAndAbsoluteEncoderSubsystem &wrist() const { return wrist_; }
const AbsoluteEncoderSubsystem &intake() const { return intake_; }
const PotAndAbsoluteEncoderSubsystem &stilts() const { return stilts_; }
+ const Vacuum &vacuum() const { return vacuum_; }
protected:
virtual void RunIteration(const SuperstructureQueue::Goal *unsafe_goal,
@@ -40,16 +42,13 @@
SuperstructureQueue::Status *status) override;
private:
- void HandleSuction(const SuctionGoal *unsafe_goal, float suction_pressure,
- SuperstructureQueue::Output *output, bool *has_piece);
-
PotAndAbsoluteEncoderSubsystem elevator_;
PotAndAbsoluteEncoderSubsystem wrist_;
AbsoluteEncoderSubsystem intake_;
PotAndAbsoluteEncoderSubsystem stilts_;
+ Vacuum vacuum_;
CollisionAvoidance collision_avoidance_;
- int vacuum_count_ = 0;
static constexpr double kMinIntakeAngleForRollers = -0.7;
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index 4176057..f87e4f2 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -25,8 +25,8 @@
.frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal wrist;
// Distance stilts extended out of the bottom of the robot. Positive = down.
- // 0 is the height such that the bottom of the stilts is tangent to the bottom
- // of the middle wheels.
+ // 0 is the height such that the bottom of the stilts is tangent to the
+ // bottom of the middle wheels.
.frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal stilts;
// Positive is rollers intaking inward.
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index bd18b60..4efb95f 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -128,6 +128,8 @@
wrist_pot_encoder_.GetSensorValues(&position->wrist);
intake_pot_encoder_.GetSensorValues(&position->intake_joint);
stilts_pot_encoder_.GetSensorValues(&position->stilts);
+ position->suction_pressure = simulated_pressure_;
+
position.Send();
}
@@ -162,6 +164,10 @@
stilts_plant_->set_voltage_offset(voltage_offset);
}
+ void set_simulated_pressure(double pressure) {
+ simulated_pressure_ = pressure;
+ }
+
// Simulates the superstructure for a single timestep.
void Simulate() {
EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
@@ -266,6 +272,8 @@
::std::unique_ptr<CappedTestPlant> stilts_plant_;
PositionSensorSimulator stilts_pot_encoder_;
+ double simulated_pressure_ = 1.0;
+
SuperstructureQueue superstructure_queue_;
};
@@ -305,7 +313,7 @@
superstructure_.Iterate();
superstructure_plant_.Simulate();
- TickTime(::std::chrono::microseconds(5050));
+ TickTime(chrono::microseconds(5050));
}
void CheckCollisions() {
@@ -698,6 +706,98 @@
VerifyNearGoal();
}
+// Tests the Vacuum detects a gamepiece
+TEST_F(SuperstructureTest, VacuumDetectsPiece) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ 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);
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_TRUE(superstructure_queue_.status->has_piece);
+}
+
+// Tests the Vacuum backs off after acquiring a gamepiece
+TEST_F(SuperstructureTest, VacuumBacksOff) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // 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);
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage, Vacuum::kPumpVoltage);
+
+ // Verify that after waiting with a piece the pump voltage goes to the
+ // has piece voltage
+ RunForTime(chrono::seconds(2), true, false);
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage,
+ Vacuum::kPumpHasPieceVoltage);
+}
+
+// Tests the Vacuum stays on for a bit after getting a no suck goal
+TEST_F(SuperstructureTest, VacuumStaysOn) {
+ WaitUntilZeroed();
+ // Turn on suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = true;
+ goal->suction.bottom = true;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Get a Gamepiece
+ superstructure_plant_.set_simulated_pressure(0.0);
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.status.FetchLatest();
+ EXPECT_TRUE(superstructure_queue_.status->has_piece);
+
+ // Turn off suction
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->suction.top = false;
+ goal->suction.bottom = false;
+ ASSERT_TRUE(goal.Send());
+ }
+
+ 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));
+ superstructure_queue_.output.FetchLatest();
+ EXPECT_EQ(superstructure_queue_.output->pump_voltage, 0.0);
+}
+
// Tests that running disabled, ya know, works
TEST_F(SuperstructureTest, DiasableTest) {
RunForTime(chrono::seconds(2), false, false);
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
new file mode 100644
index 0000000..21acb2e
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -0,0 +1,57 @@
+#include "y2019/control_loops/superstructure/vacuum.h"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+constexpr double Vacuum::kPumpVoltage;
+constexpr double Vacuum::kPumpHasPieceVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeAtHigherVoltage;
+constexpr aos::monotonic_clock::duration Vacuum::kTimeToKeepPumpRunning;
+
+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 +
+ (1 - kSuctionAlpha) * filtered_pressure_;
+
+ *has_piece = filtered_pressure_ < kVacuumThreshold;
+
+ if (*has_piece && !had_piece_) {
+ time_at_last_acquisition_ = monotonic_now;
+ }
+
+ // if we've had the piece for enought time, go to lower pump_voltage
+ low_pump_voltage =
+ *has_piece &&
+ monotonic_now > time_at_last_acquisition_ + kTimeAtHigherVoltage;
+ 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;
+ }
+
+ // 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);
+
+ output->intake_suction_top = unsafe_goal->top;
+ output->intake_suction_bottom = unsafe_goal->bottom;
+ }
+ had_piece_ = *has_piece;
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
new file mode 100644
index 0000000..599e2de
--- /dev/null
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -0,0 +1,55 @@
+#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"
+
+namespace y2019 {
+namespace control_loops {
+namespace superstructure {
+
+class Vacuum {
+ public:
+ Vacuum() {}
+ void Iterate(const SuctionGoal *unsafe_goal, float suction_pressure,
+ 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 = 12.0;
+
+ // Voltage to the vaccum pump when we have a piece
+ static constexpr double kPumpHasPieceVoltage = 8.0;
+
+ // Time to continue at the higher pump voltage after getting a gamepiece
+ static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
+ std::chrono::milliseconds(200);
+
+ // Time to continue the pump after getting a no suck goal
+ static constexpr aos::monotonic_clock::duration kTimeToKeepPumpRunning =
+ std::chrono::milliseconds(1000);
+
+ private:
+ bool had_piece_ = false;
+ aos::monotonic_clock::time_point time_at_last_evacuate_goal_ =
+ aos::monotonic_clock::epoch();
+ aos::monotonic_clock::time_point time_at_last_acquisition_ =
+ aos::monotonic_clock::epoch();
+ double filtered_pressure_ = 1.0;
+
+ static constexpr double kVacuumThreshold = 0.70;
+
+ static constexpr double kFilterTimeConstant = 0.1;
+ static constexpr double dt = .00505;
+ static constexpr double kSuctionAlpha =
+ dt * (1 - kFilterTimeConstant) / (kFilterTimeConstant);
+
+ DISALLOW_COPY_AND_ASSIGN(Vacuum);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2019
+
+#endif // Y2019_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_