Fixed broken tests.
Change-Id: I04d64d0f1f8f93dc7fb7750c1a439afb9cc3a75e
diff --git a/frc971/control_loops/fridge/fridge.cc b/frc971/control_loops/fridge/fridge.cc
index 1bf1a4c..9452637 100644
--- a/frc971/control_loops/fridge/fridge.cc
+++ b/frc971/control_loops/fridge/fridge.cc
@@ -311,7 +311,7 @@
right_elevator_estimator_.offset());
LOG(DEBUG, "Zeroed the elevator!\n");
- if (elevator() > values.fridge.arm_zeroing_height &&
+ 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.
@@ -517,6 +517,8 @@
status->zeroed = state_ == RUNNING;
status->angle = arm_loop_->X_hat(0, 0);
status->height = elevator_loop_->X_hat(0, 0);
+ status->goal_angle = arm_goal_;
+ status->goal_height = elevator_goal_;
if (unsafe_goal) {
status->grabbers = unsafe_goal->grabbers;
} else {
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
index 09d2a42..be54ee7 100644
--- a/frc971/control_loops/fridge/fridge.q
+++ b/frc971/control_loops/fridge/fridge.q
@@ -55,6 +55,11 @@
// state of the grabber pistons
GrabberPistons grabbers;
+ // Goal angle of the arm.
+ double goal_angle;
+ // Goal height of the elevator.
+ double goal_height;
+
// If true, we have aborted.
bool estopped;
diff --git a/frc971/control_loops/fridge/fridge_lib_test.cc b/frc971/control_loops/fridge/fridge_lib_test.cc
index 0e6de14..1dff2ef 100644
--- a/frc971/control_loops/fridge/fridge_lib_test.cc
+++ b/frc971/control_loops/fridge/fridge_lib_test.cc
@@ -346,7 +346,7 @@
constants::GetValues().fridge.arm.upper_hard_limit,
constants::GetValues().fridge.arm.upper_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.0).height(0.4).Send();
- RunForTime(Time::InMS(4000));
+ RunForTime(Time::InMS(5000));
VerifyNearGoal();
}
@@ -466,14 +466,14 @@
constants::GetValues().fridge.arm.upper_hard_limit,
constants::GetValues().fridge.arm.upper_hard_limit);
fridge_queue_.goal.MakeWithBuilder().angle(0.03).height(0.45).Send();
- RunForTime(Time::InMS(4000));
+ RunForTime(Time::InMS(5000));
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
VerifyNearGoal();
SimulateSensorReset();
RunForTime(Time::InMS(100));
EXPECT_NE(Fridge::RUNNING, fridge_.state());
- RunForTime(Time::InMS(4000));
+ RunForTime(Time::InMS(6000));
EXPECT_EQ(Fridge::RUNNING, fridge_.state());
VerifyNearGoal();
}
@@ -487,7 +487,7 @@
EXPECT_EQ(0.0, fridge_.arm_goal_);
// Now make sure they move correctly
- RunForTime(Time::InMS(1000), true);
+ RunForTime(Time::InMS(4000), true);
EXPECT_NE(0.0, fridge_.elevator_goal_);
EXPECT_NE(0.0, fridge_.arm_goal_);
}