Fridge handles null goal.
Change-Id: I4c9bf6a6e51f351aad3957ed22e8f4eedbb6ea35
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 61152e4..83645e9 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -56,15 +56,14 @@
}
Fridge::Fridge(control_loops::FridgeQueue *fridge)
- : aos::controls::ControlLoop<control_loops::FridgeQueue>(fridge),
+ : aos::controls::ControlLoop<control_loops::FridgeQueue, false>(fridge),
arm_loop_(new CappedStateFeedbackLoop(
StateFeedbackLoop<4, 2, 2>(MakeArmLoop()))),
elevator_loop_(new CappedStateFeedbackLoop(
StateFeedbackLoop<4, 2, 2>(MakeElevatorLoop()))),
left_arm_estimator_(constants::GetValues().fridge.left_arm_zeroing),
right_arm_estimator_(constants::GetValues().fridge.right_arm_zeroing),
- left_elevator_estimator_(
- constants::GetValues().fridge.left_elev_zeroing),
+ left_elevator_estimator_(constants::GetValues().fridge.left_elev_zeroing),
right_elevator_estimator_(
constants::GetValues().fridge.right_elev_zeroing) {}
@@ -231,7 +230,7 @@
return arm_zeroing_velocity_;
}
-void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *goal,
+void Fridge::RunIteration(const control_loops::FridgeQueue::Goal *unsafe_goal,
const control_loops::FridgeQueue::Position *position,
control_loops::FridgeQueue::Output *output,
control_loops::FridgeQueue::Status *status) {
@@ -343,14 +342,18 @@
case RUNNING:
LOG(DEBUG, "Running!\n");
- arm_goal_velocity = goal->angular_velocity;
- elevator_goal_velocity = goal->velocity;
+ if (unsafe_goal) {
+ arm_goal_velocity = unsafe_goal->angular_velocity;
+ elevator_goal_velocity = unsafe_goal->velocity;
+ }
// Update state_ to accurately represent the state of the zeroing
// estimators.
UpdateZeroingState();
- arm_goal_ = goal->angle;
- elevator_goal_ = goal->height;
+ if (unsafe_goal) {
+ arm_goal_ = unsafe_goal->angle;
+ elevator_goal_ = unsafe_goal->height;
+ }
if (state_ != RUNNING && state_ != ESTOP) {
state_ = UNINITIALIZED;
@@ -492,7 +495,14 @@
output->right_arm = arm_loop_->U(1, 0);
output->left_elevator = elevator_loop_->U(0, 0);
output->right_elevator = elevator_loop_->U(1, 0);
- output->grabbers = goal->grabbers;
+ if (unsafe_goal) {
+ output->grabbers = unsafe_goal->grabbers;
+ } else {
+ output->grabbers.top_front = false;
+ output->grabbers.top_back = false;
+ output->grabbers.bottom_front = false;
+ output->grabbers.bottom_back = false;
+ }
}
// TODO(austin): Populate these fully.
@@ -500,8 +510,16 @@
status->done = false;
status->angle = arm_loop_->X_hat(0, 0);
status->height = elevator_loop_->X_hat(0, 0);
- status->grabbers = goal->grabbers;
+ if (unsafe_goal) {
+ status->grabbers = unsafe_goal->grabbers;
+ } else {
+ status->grabbers.top_front = false;
+ status->grabbers.top_back = false;
+ status->grabbers.bottom_front = false;
+ status->grabbers.bottom_back = false;
+ }
status->estopped = (state_ == ESTOP);
+ status->state = state_;
last_state_ = state_;
}
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index 87dbfa4..eecfe1b 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -40,7 +40,7 @@
};
class Fridge
- : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue, false> {
public:
explicit Fridge(
control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index 07a0646..90cebd4 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -60,6 +60,9 @@
// TODO(austin): Internal state.
bool estopped;
+
+ // The internal state of the state machine.
+ int32_t state;
};
message Output {
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 2902f19..2af5e2c 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -207,6 +207,8 @@
void VerifyNearGoal() {
fridge_queue_.goal.FetchLatest();
fridge_queue_.status.FetchLatest();
+ EXPECT_TRUE(fridge_queue_.goal.get() != nullptr);
+ EXPECT_TRUE(fridge_queue_.status.get() != nullptr);
EXPECT_NEAR(fridge_queue_.goal->angle, fridge_queue_.status->angle, 0.001);
EXPECT_NEAR(fridge_queue_.goal->height, fridge_queue_.status->height,
0.001);
@@ -573,6 +575,14 @@
EXPECT_EQ(fridge_.arm_loop_->U(), fridge_.arm_loop_->U_uncapped());
}
+// Tests that the loop zeroes when run for a while.
+TEST_F(FridgeTest, ZeroNoGoal) {
+ RunForTime(Time::InMS(4000));
+
+ EXPECT_EQ(Fridge::RUNNING, fridge_.state());
+}
+
+
// 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).
// TODO(austin): Check that we e-stop if pot disagrees too much with encoder after we are zeroed.