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/constants.cc b/frc971/constants.cc
index c68990f..43f5adf 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -55,6 +55,8 @@
kElevatorGearboxOutputPulleyTeeth * kElevatorGearboxOutputPitch /
(2.0 * M_PI);
+const double kArmZeroingHeight = 0.2;
+
const double kMaxAllowedLeftRightArmDifference = 0.04; // radians
const double kMaxAllowedLeftRightElevatorDifference = 0.01; // meters
@@ -147,6 +149,8 @@
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
0.0, 0.0, 0.0, 0.0,
+
+ kArmZeroingHeight,
},
// End "sensor" values.
@@ -214,6 +218,8 @@
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
{kZeroingSampleSize, kArmEncoderIndexDifference, 0.0},
0.0, 0.0, 0.0, 0.0,
+
+ kArmZeroingHeight,
},
// End "sensor" values.
@@ -280,6 +286,8 @@
-0.078959636363636357 - 0.024646,
-3.4952331578947375 + 0.011776,
3.5263507647058816 - 0.018921 + 0.006545,
+
+ kArmZeroingHeight,
},
// TODO(sensors): End "sensor" values.
diff --git a/frc971/constants.h b/frc971/constants.h
index 9e2ccf6..e12a27f 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -102,6 +102,9 @@
double right_elevator_potentiometer_offset;
double left_arm_potentiometer_offset;
double right_arm_potentiometer_offset;
+
+ // How high the elevator has to be before we start zeroing the arm.
+ double arm_zeroing_height;
};
Fridge fridge;
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).