frc971/control_loops/wrist_lib_test.cc - RealtimeRoboticsGroup/test - Gitiles

 #include <unistd.h>

 #include <memory>

 #include "gtest/gtest.h"
 #include "aos/common/queue.h"
 #include "aos/common/queue_testutils.h"
 #include "frc971/control_loops/wrist_motor.q.h"
 #include "frc971/control_loops/wrist.h"
 #include "frc971/constants.h"


 using ::aos::time::Time;

 namespace frc971 {
 namespace control_loops {
 namespace testing {


 // Class which simulates the wrist and sends out queue messages containing the
 // position.
 class WristMotorSimulation {
  public:
   // Constructs a motor simulation.  initial_position is the inital angle of the
   // wrist, which will be treated as 0 by the encoder.
   WristMotorSimulation(double initial_position)
       : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeWristPlant())),
         my_wrist_loop_(".frc971.control_loops.wrist",
                        0x1a7b7094, ".frc971.control_loops.wrist.goal",
                        ".frc971.control_loops.wrist.position",
                        ".frc971.control_loops.wrist.output",
                        ".frc971.control_loops.wrist.status") {
     Reinitialize(initial_position);
   }

   // Resets the plant so that it starts at initial_position.
   void Reinitialize(double initial_position) {
     initial_position_ = initial_position;
     wrist_plant_->X(0, 0) = initial_position_;
     wrist_plant_->X(1, 0) = 0.0;
     wrist_plant_->Y = wrist_plant_->C * wrist_plant_->X;
     last_position_ = wrist_plant_->Y(0, 0);
     calibration_value_ = 0.0;
   }

   // Returns the absolute angle of the wrist.
   double GetAbsolutePosition() const {
     return wrist_plant_->Y(0, 0);
   }

   // Returns the adjusted angle of the wrist.
   double GetPosition() const {
     return GetAbsolutePosition() - initial_position_;
   }

   // Sends out the position queue messages.
   void SendPositionMessage() {
     const double angle = GetPosition();

     double horizontal_hall_effect_start_angle;
     ASSERT_TRUE(constants::horizontal_hall_effect_start_angle(
                     &horizontal_hall_effect_start_angle));
     double horizontal_hall_effect_stop_angle;
     ASSERT_TRUE(constants::horizontal_hall_effect_stop_angle(
                     &horizontal_hall_effect_stop_angle));

     ::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
         my_wrist_loop_.position.MakeMessage();
     position->pos = angle;

     // Signal that the hall effect sensor has been triggered if it is within
     // the correct range.
     double abs_position = GetAbsolutePosition();
     if (abs_position >= horizontal_hall_effect_start_angle &&
         abs_position <= horizontal_hall_effect_stop_angle) {
       position->hall_effect = true;
     } else {
       position->hall_effect = false;
     }

     // Only set calibration if it changed last cycle.  Calibration starts out
     // with a value of 0.
     if ((last_position_ < horizontal_hall_effect_start_angle ||
          last_position_ > horizontal_hall_effect_stop_angle) &&
          position->hall_effect) {
       calibration_value_ =
           horizontal_hall_effect_start_angle - initial_position_;
     }

     position->calibration = calibration_value_;
     position.Send();
   }

   // Simulates the wrist moving for one timestep.
   void Simulate() {
     last_position_ = wrist_plant_->Y(0, 0);
     EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
     wrist_plant_->U << my_wrist_loop_.output->voltage;
     wrist_plant_->Update();

     // Assert that we are in the right physical range.
     double horizontal_upper_physical_limit;
     ASSERT_TRUE(constants::horizontal_upper_physical_limit(
                     &horizontal_upper_physical_limit));
     double horizontal_lower_physical_limit;
     ASSERT_TRUE(constants::horizontal_lower_physical_limit(
                     &horizontal_lower_physical_limit));

     EXPECT_GE(horizontal_upper_physical_limit, wrist_plant_->Y(0, 0));
     EXPECT_LE(horizontal_lower_physical_limit, wrist_plant_->Y(0, 0));
   }

   ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
  private:
   WristLoop my_wrist_loop_;
   double initial_position_;
   double last_position_;
   double calibration_value_;
 };

 class WristTest : public ::testing::Test {
  protected:
   ::aos::common::testing::GlobalCoreInstance my_core;

   // Create a new instance of the test queue so that it invalidates the queue
   // that it points to.  Otherwise, we will have a pointer to shared memory that
   // is no longer valid.
   WristLoop my_wrist_loop_;

   // Create a loop and simulation plant.
   WristMotor wrist_motor_;
   WristMotorSimulation wrist_motor_plant_;

   WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
                                0x1a7b7094, ".frc971.control_loops.wrist.goal",
                                ".frc971.control_loops.wrist.position",
                                ".frc971.control_loops.wrist.output",
                                ".frc971.control_loops.wrist.status"),
                 wrist_motor_(&my_wrist_loop_),
                 wrist_motor_plant_(0.5) {
     // Flush the robot state queue so we can use clean shared memory for this
     // test.
     ::aos::robot_state.Clear();
     SendDSPacket(true);
   }

   void SendDSPacket(bool enabled) {
     ::aos::robot_state.MakeWithBuilder().enabled(enabled)
                                         .autonomous(false)
                                         .team_id(971).Send();
     ::aos::robot_state.FetchLatest();
   }

   void VerifyNearGoal() {
     my_wrist_loop_.goal.FetchLatest();
     my_wrist_loop_.position.FetchLatest();
     EXPECT_NEAR(my_wrist_loop_.goal->goal,
                 wrist_motor_plant_.GetAbsolutePosition(),
                 1e-4);
   }

   virtual ~WristTest() {
     ::aos::robot_state.Clear();
   }
 };

 // Tests that the wrist zeros correctly and goes to a position.
 TEST_F(WristTest, ZerosCorrectly) {
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 400; ++i) {
     wrist_motor_plant_.SendPositionMessage();
     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 // Tests that the wrist zeros correctly starting on the hall effect sensor.
 TEST_F(WristTest, ZerosStartingOn) {
   wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);

   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 500; ++i) {
     wrist_motor_plant_.SendPositionMessage();
     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 // Tests that missing positions are correctly handled.
 TEST_F(WristTest, HandleMissingPosition) {
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 400; ++i) {
     if (i % 23) {
       wrist_motor_plant_.SendPositionMessage();
     }
     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 // Tests that loosing the encoder for a second triggers a re-zero.
 TEST_F(WristTest, RezeroWithMissingPos) {
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 800; ++i) {
     // After 3 seconds, simulate the encoder going missing.
     // This should trigger a re-zero.  To make sure it works, change the goal as
     // well.
     if (i < 300 || i > 400) {
       wrist_motor_plant_.SendPositionMessage();
     } else {
       if (i > 310) {
         // Should be re-zeroing now.
         EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
       }
       my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
     }
     if (i == 430) {
       EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
     }

     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 // Tests that disabling while zeroing sends the state machine into the
 // uninitialized state.
 TEST_F(WristTest, DisableGoesUninitialized) {
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 800; ++i) {
     wrist_motor_plant_.SendPositionMessage();
     // After 0.5 seconds, disable the robot.
     if (i > 50 && i < 200) {
       SendDSPacket(false);
       if (i > 100) {
         // Give the loop a couple cycled to get the message and then verify that
         // it is in the correct state.
         EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
         // When disabled, we should be applying 0 power.
         EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
         EXPECT_EQ(0, my_wrist_loop_.output->voltage);
       }
     } else {
       SendDSPacket(true);
     }
     if (i == 202) {
       // Verify that we are zeroing after the bot gets enabled again.
       EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
     }

     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
   }
   VerifyNearGoal();
 }

 // 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) {
   double saved_zeroing_position = 0;
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 500; ++i) {
     wrist_motor_plant_.SendPositionMessage();
     if (i == 50) {
       EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
       // Move the zeroing position far away and verify that it gets moved back.
       saved_zeroing_position = wrist_motor_.zeroing_position_;
       wrist_motor_.zeroing_position_ = -100.0;
     } else if (i == 51) {
       EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
       EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
     }
     if (wrist_motor_.state_ != WristMotor::READY) {
       if (i == 51) {
         // 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)));
       } else {
         EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
       }
     }


     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 // 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) {
   double saved_zeroing_position = 0;
   my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
   for (int i = 0; i < 500; ++i) {
     wrist_motor_plant_.SendPositionMessage();
     if (i == 50) {
       EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
       // Move the zeroing position far away and verify that it gets moved back.
       saved_zeroing_position = wrist_motor_.zeroing_position_;
       wrist_motor_.zeroing_position_ = 100.0;
     } else {
       if (i == 51) {
         EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
         EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
       }
     }
     if (wrist_motor_.state_ != WristMotor::READY) {
       if (i == 51) {
         // 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)));
       } else {
         EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
       }
     }

     wrist_motor_.Iterate();
     wrist_motor_plant_.Simulate();
     SendDSPacket(true);
   }
   VerifyNearGoal();
 }

 }  // namespace testing
 }  // namespace control_loops
 }  // namespace frc971
	#include <unistd.h>

	#include <memory>

	#include "gtest/gtest.h"
	#include "aos/common/queue.h"
	#include "aos/common/queue_testutils.h"
	#include "frc971/control_loops/wrist_motor.q.h"
	#include "frc971/control_loops/wrist.h"
	#include "frc971/constants.h"


	using ::aos::time::Time;

	namespace frc971 {
	namespace control_loops {
	namespace testing {


	// Class which simulates the wrist and sends out queue messages containing the
	// position.
	class WristMotorSimulation {
	public:
	// Constructs a motor simulation. initial_position is the inital angle of the
	// wrist, which will be treated as 0 by the encoder.
	WristMotorSimulation(double initial_position)
	: wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeWristPlant())),
	my_wrist_loop_(".frc971.control_loops.wrist",
	0x1a7b7094, ".frc971.control_loops.wrist.goal",
	".frc971.control_loops.wrist.position",
	".frc971.control_loops.wrist.output",
	".frc971.control_loops.wrist.status") {
	Reinitialize(initial_position);
	}

	// Resets the plant so that it starts at initial_position.
	void Reinitialize(double initial_position) {
	initial_position_ = initial_position;
	wrist_plant_->X(0, 0) = initial_position_;
	wrist_plant_->X(1, 0) = 0.0;
	wrist_plant_->Y = wrist_plant_->C * wrist_plant_->X;
	last_position_ = wrist_plant_->Y(0, 0);
	calibration_value_ = 0.0;
	}

	// Returns the absolute angle of the wrist.
	double GetAbsolutePosition() const {
	return wrist_plant_->Y(0, 0);
	}

	// Returns the adjusted angle of the wrist.
	double GetPosition() const {
	return GetAbsolutePosition() - initial_position_;
	}

	// Sends out the position queue messages.
	void SendPositionMessage() {
	const double angle = GetPosition();

	double horizontal_hall_effect_start_angle;
	ASSERT_TRUE(constants::horizontal_hall_effect_start_angle(
	&horizontal_hall_effect_start_angle));
	double horizontal_hall_effect_stop_angle;
	ASSERT_TRUE(constants::horizontal_hall_effect_stop_angle(
	&horizontal_hall_effect_stop_angle));

	::aos::ScopedMessagePtr<control_loops::WristLoop::Position> position =
	my_wrist_loop_.position.MakeMessage();
	position->pos = angle;

	// Signal that the hall effect sensor has been triggered if it is within
	// the correct range.
	double abs_position = GetAbsolutePosition();
	if (abs_position >= horizontal_hall_effect_start_angle &&
	abs_position <= horizontal_hall_effect_stop_angle) {
	position->hall_effect = true;
	} else {
	position->hall_effect = false;
	}

	// Only set calibration if it changed last cycle. Calibration starts out
	// with a value of 0.
	if ((last_position_ < horizontal_hall_effect_start_angle \|\|
	last_position_ > horizontal_hall_effect_stop_angle) &&
	position->hall_effect) {
	calibration_value_ =
	horizontal_hall_effect_start_angle - initial_position_;
	}

	position->calibration = calibration_value_;
	position.Send();
	}

	// Simulates the wrist moving for one timestep.
	void Simulate() {
	last_position_ = wrist_plant_->Y(0, 0);
	EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
	wrist_plant_->U << my_wrist_loop_.output->voltage;
	wrist_plant_->Update();

	// Assert that we are in the right physical range.
	double horizontal_upper_physical_limit;
	ASSERT_TRUE(constants::horizontal_upper_physical_limit(
	&horizontal_upper_physical_limit));
	double horizontal_lower_physical_limit;
	ASSERT_TRUE(constants::horizontal_lower_physical_limit(
	&horizontal_lower_physical_limit));

	EXPECT_GE(horizontal_upper_physical_limit, wrist_plant_->Y(0, 0));
	EXPECT_LE(horizontal_lower_physical_limit, wrist_plant_->Y(0, 0));
	}

	::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
	private:
	WristLoop my_wrist_loop_;
	double initial_position_;
	double last_position_;
	double calibration_value_;
	};

	class WristTest : public ::testing::Test {
	protected:
	::aos::common::testing::GlobalCoreInstance my_core;

	// Create a new instance of the test queue so that it invalidates the queue
	// that it points to. Otherwise, we will have a pointer to shared memory that
	// is no longer valid.
	WristLoop my_wrist_loop_;

	// Create a loop and simulation plant.
	WristMotor wrist_motor_;
	WristMotorSimulation wrist_motor_plant_;

	WristTest() : my_wrist_loop_(".frc971.control_loops.wrist",
	0x1a7b7094, ".frc971.control_loops.wrist.goal",
	".frc971.control_loops.wrist.position",
	".frc971.control_loops.wrist.output",
	".frc971.control_loops.wrist.status"),
	wrist_motor_(&my_wrist_loop_),
	wrist_motor_plant_(0.5) {
	// Flush the robot state queue so we can use clean shared memory for this
	// test.
	::aos::robot_state.Clear();
	SendDSPacket(true);
	}

	void SendDSPacket(bool enabled) {
	::aos::robot_state.MakeWithBuilder().enabled(enabled)
	.autonomous(false)
	.team_id(971).Send();
	::aos::robot_state.FetchLatest();
	}

	void VerifyNearGoal() {
	my_wrist_loop_.goal.FetchLatest();
	my_wrist_loop_.position.FetchLatest();
	EXPECT_NEAR(my_wrist_loop_.goal->goal,
	wrist_motor_plant_.GetAbsolutePosition(),
	1e-4);
	}

	virtual ~WristTest() {
	::aos::robot_state.Clear();
	}
	};

	// Tests that the wrist zeros correctly and goes to a position.
	TEST_F(WristTest, ZerosCorrectly) {
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 400; ++i) {
	wrist_motor_plant_.SendPositionMessage();
	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	// Tests that the wrist zeros correctly starting on the hall effect sensor.
	TEST_F(WristTest, ZerosStartingOn) {
	wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);

	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 500; ++i) {
	wrist_motor_plant_.SendPositionMessage();
	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	// Tests that missing positions are correctly handled.
	TEST_F(WristTest, HandleMissingPosition) {
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 400; ++i) {
	if (i % 23) {
	wrist_motor_plant_.SendPositionMessage();
	}
	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	// Tests that loosing the encoder for a second triggers a re-zero.
	TEST_F(WristTest, RezeroWithMissingPos) {
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 800; ++i) {
	// After 3 seconds, simulate the encoder going missing.
	// This should trigger a re-zero. To make sure it works, change the goal as
	// well.
	if (i < 300 \|\| i > 400) {
	wrist_motor_plant_.SendPositionMessage();
	} else {
	if (i > 310) {
	// Should be re-zeroing now.
	EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
	}
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
	}
	if (i == 430) {
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	}

	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	// Tests that disabling while zeroing sends the state machine into the
	// uninitialized state.
	TEST_F(WristTest, DisableGoesUninitialized) {
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 800; ++i) {
	wrist_motor_plant_.SendPositionMessage();
	// After 0.5 seconds, disable the robot.
	if (i > 50 && i < 200) {
	SendDSPacket(false);
	if (i > 100) {
	// Give the loop a couple cycled to get the message and then verify that
	// it is in the correct state.
	EXPECT_EQ(WristMotor::UNINITIALIZED, wrist_motor_.state_);
	// When disabled, we should be applying 0 power.
	EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
	EXPECT_EQ(0, my_wrist_loop_.output->voltage);
	}
	} else {
	SendDSPacket(true);
	}
	if (i == 202) {
	// Verify that we are zeroing after the bot gets enabled again.
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	}

	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	}
	VerifyNearGoal();
	}

	// 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) {
	double saved_zeroing_position = 0;
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 500; ++i) {
	wrist_motor_plant_.SendPositionMessage();
	if (i == 50) {
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	// Move the zeroing position far away and verify that it gets moved back.
	saved_zeroing_position = wrist_motor_.zeroing_position_;
	wrist_motor_.zeroing_position_ = -100.0;
	} else if (i == 51) {
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
	}
	if (wrist_motor_.state_ != WristMotor::READY) {
	if (i == 51) {
	// 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)));
	} else {
	EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
	}
	}


	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	// 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) {
	double saved_zeroing_position = 0;
	my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
	for (int i = 0; i < 500; ++i) {
	wrist_motor_plant_.SendPositionMessage();
	if (i == 50) {
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	// Move the zeroing position far away and verify that it gets moved back.
	saved_zeroing_position = wrist_motor_.zeroing_position_;
	wrist_motor_.zeroing_position_ = 100.0;
	} else {
	if (i == 51) {
	EXPECT_EQ(WristMotor::ZEROING, wrist_motor_.state_);
	EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroing_position_, 0.4);
	}
	}
	if (wrist_motor_.state_ != WristMotor::READY) {
	if (i == 51) {
	// 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)));
	} else {
	EXPECT_GT(5, ::std::abs(wrist_motor_.loop_->U_uncapped(0, 0)));
	}
	}

	wrist_motor_.Iterate();
	wrist_motor_plant_.Simulate();
	SendDSPacket(true);
	}
	VerifyNearGoal();
	}

	} // namespace testing
	} // namespace control_loops
	} // namespace frc971