Made zeroing test cases more flexible to deal with new constants. We now count the number of capped cycles and verify that it is low.
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index 4e116c7..b49e99c 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -21,7 +21,8 @@
loop_(new WristStateFeedbackLoop(MakeWristLoop(), this)),
state_(UNINITIALIZED),
error_count_(0),
- zero_offset_(0.0) {
+ zero_offset_(0.0),
+ capped_goal_(false) {
}
bool WristMotor::FetchConstants() {
@@ -196,6 +197,7 @@
// Update the observer.
loop_->Update(position != NULL, output == NULL);
+ capped_goal_ = false;
// Verify that the zeroing goal hasn't run away.
switch (state_) {
case UNINITIALIZED:
@@ -210,10 +212,12 @@
double dx = (loop_->U_uncapped(0, 0) -
kMaxZeroingVoltage) / loop_->K(0, 0);
zeroing_position_ -= dx;
+ capped_goal_ = true;
} else if(loop_->U_uncapped(0, 0) < -kMaxZeroingVoltage) {
double dx = (loop_->U_uncapped(0, 0) +
kMaxZeroingVoltage) / loop_->K(0, 0);
zeroing_position_ -= dx;
+ capped_goal_ = true;
}
break;
}
diff --git a/frc971/control_loops/wrist/wrist.h b/frc971/control_loops/wrist/wrist.h
index 250f536..55d14c8 100644
--- a/frc971/control_loops/wrist/wrist.h
+++ b/frc971/control_loops/wrist/wrist.h
@@ -27,6 +27,9 @@
explicit WristMotor(
control_loops::WristLoop *my_wrist = &control_loops::wrist);
+ // True if the goal was moved to avoid goal windup.
+ bool capped_goal() const { return capped_goal_; }
+
protected:
virtual void RunIteration(
const ::aos::control_loops::Goal *goal,
@@ -96,6 +99,9 @@
double wrist_hall_effect_start_angle_;
double wrist_zeroing_speed_;
+ // True if the zeroing goal was capped during this cycle.
+ bool capped_goal_;
+
DISALLOW_COPY_AND_ASSIGN(WristMotor);
};
diff --git a/frc971/control_loops/wrist/wrist_lib_test.cc b/frc971/control_loops/wrist/wrist_lib_test.cc
index 8e852c1..aa0cb78 100644
--- a/frc971/control_loops/wrist/wrist_lib_test.cc
+++ b/frc971/control_loops/wrist/wrist_lib_test.cc
@@ -267,6 +267,7 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(WristTest, NoWindupNegative) {
+ int capped_count = 0;
double saved_zeroing_position = 0;
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 500; ++i) {
@@ -281,12 +282,13 @@
EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
}
if (wrist_motor_.state_ != WristMotor::READY) {
- if (i == 51) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
- EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
} else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
+ ASSERT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
}
}
@@ -296,11 +298,13 @@
SendDSPacket(true);
}
VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
}
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(WristTest, NoWindupPositive) {
+ int capped_count = 0;
double saved_zeroing_position = 0;
my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
for (int i = 0; i < 500; ++i) {
@@ -317,7 +321,8 @@
}
}
if (wrist_motor_.state_ != WristMotor::READY) {
- if (i == 51) {
+ if (wrist_motor_.capped_goal()) {
+ ++capped_count;
// The cycle after we kick the zero position is the only cycle during
// which we should expect to see a high uncapped power during zeroing.
EXPECT_LT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
@@ -331,6 +336,7 @@
SendDSPacket(true);
}
VerifyNearGoal();
+ EXPECT_GT(3, capped_count);
}
} // namespace testing