Add battery compensation for the vacuum and tune it down
This means we continue to pump slowly even when pushing hard.
Change-Id: I332ffb90606f18137bd3fbb915e63ec0b08e4944
diff --git a/y2019/control_loops/superstructure/vacuum.h b/y2019/control_loops/superstructure/vacuum.h
index 599e2de..3fd5a49 100644
--- a/y2019/control_loops/superstructure/vacuum.h
+++ b/y2019/control_loops/superstructure/vacuum.h
@@ -17,18 +17,18 @@
// Voltage to the vaccum pump when we are attempting to acquire a piece
- static constexpr double kPumpVoltage = 12.0;
+ static constexpr double kPumpVoltage = 8.0;
// Voltage to the vaccum pump when we have a piece
- static constexpr double kPumpHasPieceVoltage = 8.0;
+ static constexpr double kPumpHasPieceVoltage = 2.0;
// Time to continue at the higher pump voltage after getting a gamepiece
static constexpr aos::monotonic_clock::duration kTimeAtHigherVoltage =
- std::chrono::milliseconds(200);
+ 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(1000);
+ std::chrono::milliseconds(750);
private:
bool had_piece_ = false;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index b3969fb..d473b0f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -23,11 +23,11 @@
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
#include "aos/make_unique.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -311,9 +311,17 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- suction_victor_->SetSpeed(
- ::aos::Clip(queue->pump_voltage, -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ ::aos::robot_state.FetchLatest();
+ const double battery_voltage =
+ ::aos::robot_state.get() ? ::aos::robot_state->voltage_battery : 12.0;
+
+ // Throw a fast low pass filter on the battery voltage so we don't respond
+ // too fast to noise.
+ filtered_battery_voltage_ =
+ 0.5 * filtered_battery_voltage_ + 0.5 * battery_voltage;
+
+ suction_victor_->SetSpeed(::aos::Clip(
+ queue->pump_voltage / filtered_battery_voltage_, -1.0, 1.0));
}
void Stop() override {
@@ -328,6 +336,8 @@
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
wrist_victor_, stilts_victor_, suction_victor_;
+
+ double filtered_battery_voltage_ = 12.0;
};
class SolenoidWriter {