Make arm zeroing safer.
Now it forces the elevator to go to a predefined "safe" height
before it zeroes the arm to avoid crashing the fridge.
Change-Id: I8b5197a6a1e3d75372756015026c5ea8d1ddddef
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 7596a75..1bf1a4c 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -18,6 +18,8 @@
namespace {
constexpr double kZeroingVoltage = 4.0;
constexpr double kElevatorZeroingVelocity = 0.10;
+// What speed we move to our safe height at.
+constexpr double kElevatorSafeHeightVelocity = 0.2;
constexpr double kArmZeroingVelocity = 0.20;
} // namespace
@@ -308,6 +310,20 @@
SetElevatorOffset(left_elevator_estimator_.offset(),
right_elevator_estimator_.offset());
LOG(DEBUG, "Zeroed the elevator!\n");
+
+ if (elevator() > values.fridge.arm_zeroing_height &&
+ state_ != INITIALIZING) {
+ // Move the elevator to a safe height before we start zeroing the arm,
+ // so that we don't crash anything.
+ LOG(DEBUG, "Moving elevator to safe height.\n");
+ elevator_goal_ += kElevatorSafeHeightVelocity *
+ ::aos::controls::kLoopFrequency.ToSeconds();
+ elevator_goal_velocity = kElevatorSafeHeightVelocity;
+
+ state_ = ZEROING_ELEVATOR;
+ break;
+ }
+
} else if (!disable) {
elevator_goal_velocity = elevator_zeroing_velocity();
elevator_goal_ += elevator_goal_velocity *
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index 54dc032..fc10594 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -18,6 +18,7 @@
class FridgeTest_ElevatorGoalPositiveWindupTest_Test;
class FridgeTest_ArmGoalNegativeWindupTest_Test;
class FridgeTest_ElevatorGoalNegativeWindupTest_Test;
+class FridgeTest_SafeArmZeroing_Test;
}
class CappedStateFeedbackLoop : public StateFeedbackLoop<4, 2, 2> {
@@ -74,6 +75,7 @@
friend class testing::FridgeTest_ArmGoalPositiveWindupTest_Test;
friend class testing::FridgeTest_ElevatorGoalNegativeWindupTest_Test;
friend class testing::FridgeTest_ArmGoalNegativeWindupTest_Test;
+ friend class testing::FridgeTest_SafeArmZeroing_Test;
// Sets state_ to the correct state given the current state of the zeroing
// estimators.
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 657c07e..0e6de14 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -331,7 +331,8 @@
constants::GetValues().fridge.arm.lower_hard_limit,
constants::GetValues().fridge.arm.lower_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
- RunForTime(Time::InMS(4000));
+ // We have to wait for it to put the elevator in a safe position as well.
+ RunForTime(Time::InMS(8000));
VerifyNearGoal();
}
@@ -582,6 +583,33 @@
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
}
+// Tests that if we start at the bottom, the elevator moves to a safe height
+// before zeroing the arm.
+TEST_F(FridgeTest, SafeArmZeroing) {
+ auto &values = constants::GetValues();
+ fridge_plant_.InitializeElevatorPosition(
+ values.fridge.elevator.lower_hard_limit);
+ fridge_plant_.InitializeArmPosition(M_PI / 4.0);
+
+ const auto start_time = Time::Now();
+ double last_arm_goal = fridge_.arm_goal_;
+ while (Time::Now() < start_time + Time::InMS(4000)) {
+ RunIteration();
+
+ if (fridge_.state() != Fridge::ZEROING_ELEVATOR) {
+ // Wait until we are zeroing the elevator.
+ continue;
+ }
+
+ fridge_queue_.status.FetchLatest();
+ ASSERT_TRUE(fridge_queue_.status.get() != nullptr);
+ if (fridge_queue_.status->height > values.fridge.arm_zeroing_height) {
+ // We had better not be trying to zero the arm...
+ EXPECT_EQ(last_arm_goal, fridge_.arm_goal_);
+ last_arm_goal = fridge_.arm_goal_;
+ }
+ }
+}
// Phil:
// TODO(austin): Check that we e-stop if encoder index pulse is not n revolutions away from last one. (got extra counts from noise, etc).