Hood should not break itself.
Change-Id: Ia099241a354c94b69d4cbaeaa3f7b8ab0758b788
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index b0c02de..c58822a 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -8,6 +8,8 @@
namespace superstructure {
namespace hood {
+namespace chrono = ::std::chrono;
+
constexpr double Hood::kZeroingVoltage;
constexpr double Hood::kOperatingVoltage;
// The tracking error to allow before declaring that we are stuck and reversing
@@ -53,6 +55,8 @@
void Hood::Reset() {
state_ = State::UNINITIALIZED;
profiled_subsystem_.Reset();
+ last_move_time_ = ::aos::monotonic_clock::min_time;
+ last_position_ = 0;
}
void Hood::Iterate(const control_loops::HoodGoal *unsafe_goal,
@@ -165,7 +169,22 @@
const double max_voltage =
(state_ == State::RUNNING) ? kOperatingVoltage : kZeroingVoltage;
- profiled_subsystem_.set_max_voltage({{max_voltage}});
+ // If we have been in the same position for kNumberCyclesTillNotMoving, make
+ // sure that the kNotMovingVoltage is used instead of kOperatingVoltage.
+ double error_voltage = max_voltage;
+ if (::std::abs(profiled_subsystem_.position() - last_position_) >
+ kErrorOnPositionTillNotMoving) {
+ // Currently moving. Update time of last move.
+ last_move_time_ = ::aos::monotonic_clock::now();
+ // Save last position.
+ last_position_ = profiled_subsystem_.position();
+ }
+ if (::aos::monotonic_clock::now() > kTimeTillNotMoving + last_move_time_) {
+ error_voltage = kNotMovingVoltage;
+ }
+
+ profiled_subsystem_.set_max_voltage(
+ {{::std::min(max_voltage, error_voltage)}});
// Calculate the loops for a cycle.
profiled_subsystem_.Update(disable);
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 41cbbd0..cf45850 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -3,6 +3,7 @@
#include "frc971/control_loops/profiled_subsystem.h"
#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/constants.h"
namespace y2017 {
namespace control_loops {
@@ -32,7 +33,17 @@
// The zeroing and operating voltages.
static constexpr double kZeroingVoltage = 2.0;
- static constexpr double kOperatingVoltage = 4.0;
+ static constexpr double kOperatingVoltage = 12.0;
+
+ // Constants needed when determining if the hood is in danger of breaking its
+ // own gears at the normal operating voltage.
+ static constexpr double kErrorOnPositionTillNotMoving =
+ 2.5 * (::y2017::constants::Values::kHoodEncoderIndexDifference /
+ ::y2017::constants::Values::kHoodEncoderCountsPerRevolution);
+
+ static constexpr ::aos::monotonic_clock::duration kTimeTillNotMoving =
+ ::std::chrono::milliseconds(15);
+ static constexpr double kNotMovingVoltage = 2.0;
void Iterate(const control_loops::HoodGoal *unsafe_goal,
const ::frc971::IndexPosition *position, double *output,
@@ -54,6 +65,9 @@
State state_;
IndexPulseProfiledSubsystem profiled_subsystem_;
+ double last_position_ = 0;
+ ::aos::monotonic_clock::time_point last_move_time_ =
+ ::aos::monotonic_clock::min_time;
};
} // namespace hood