Added most of Ben's changes in.
diff --git a/frc971/constants.cc b/frc971/constants.cc
index 9893f90..bbc1966 100644
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -53,11 +53,27 @@
true,
control_loops::MakeVClutchDrivetrainLoop,
control_loops::MakeClutchDrivetrainLoop,
+ 2.05,
+ 2.05,
0.0,
0.0,
+ 1.0,
+ 0.1,
0.0,
- 0.0,
- 0.0,
+ 1.57,
+
+ -0.1,
+ 0.05,
+ 1.0,
+ 1.1,
+ 2.0,
+ 2.1,
+ -0.1,
+ 0.05,
+ 1.0,
+ 1.1,
+ 2.0,
+ 2.1,
};
break;
case kPracticeTeamNumber:
@@ -70,11 +86,27 @@
false,
control_loops::MakeVDogDrivetrainLoop,
control_loops::MakeDogDrivetrainLoop,
+ 2.05,
+ 2.05,
0.0,
0.0,
+ 1.0,
+ 0.1,
0.0,
- 0.0,
- 0.0,
+ 1.57,
+
+ -0.1,
+ 0.05,
+ 1.0,
+ 1.1,
+ 2.0,
+ 2.1,
+ -0.1,
+ 0.05,
+ 1.0,
+ 1.1,
+ 2.0,
+ 2.1,
};
break;
default:
diff --git a/frc971/constants.h b/frc971/constants.h
index 9691bb2..2e8af5a 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -40,11 +40,30 @@
::std::function<StateFeedbackLoop<2, 2, 2>()> make_v_drivetrain_loop;
::std::function<StateFeedbackLoop<4, 2, 2>()> make_drivetrain_loop;
- double claw_lower_limit;
- double claw_upper_limit;
- double claw_hall_effect_start_angle;
+ double upper_claw_lower_limit;
+ double upper_claw_upper_limit;
+ double lower_claw_lower_limit;
+ double lower_claw_upper_limit;
double claw_zeroing_off_speed;
double claw_zeroing_speed;
+
+ // claw seperation that would be considered a collision
+ double claw_min_seperation;
+ double claw_max_seperation;
+
+ // Three hall effects are known as front, calib and back
+ double upper_claw_front_heffect_lower_angle;
+ double upper_claw_front_heffect_upper_angle;
+ double upper_claw_calib_heffect_lower_angle;
+ double upper_claw_calib_heffect_upper_angle;
+ double upper_claw_back_heffect_lower_angle;
+ double upper_claw_back_heffect_upper_angle;
+ double lower_claw_front_heffect_lower_angle;
+ double lower_claw_front_heffect_upper_angle;
+ double lower_claw_calib_heffect_lower_angle;
+ double lower_claw_calib_heffect_upper_angle;
+ double lower_claw_back_heffect_lower_angle;
+ double lower_claw_back_heffect_upper_angle;
};
// Creates (once) a Values instance and returns a reference to it.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 55cbcc5..44df982 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -46,8 +46,8 @@
namespace frc971 {
namespace control_loops {
-ClawMotor::ClawMotor(control_loops::ClawLoop *my_claw)
- : aos::control_loops::ControlLoop<control_loops::ClawLoop>(my_claw),
+ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
+ : aos::control_loops::ControlLoop<control_loops::ClawGroup>(my_claw),
zeroed_joint_(MakeTopClawLoop()) {
{
using ::frc971::constants::GetValues;
@@ -68,9 +68,9 @@
}
// Positive angle is up, and positive power is up.
-void ClawMotor::RunIteration(const control_loops::ClawLoop::Goal *goal,
- const control_loops::ClawLoop::Position *position,
- control_loops::ClawLoop::Output *output,
+void ClawMotor::RunIteration(const control_loops::ClawGroup::Goal *goal,
+ const control_loops::ClawGroup::Position *position,
+ control_loops::ClawGroup::Output *output,
::aos::control_loops::Status *status) {
// Disable the motors now so that all early returns will return with the
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
index 8852fd3..03f1930 100644
--- a/frc971/control_loops/claw/claw.q
+++ b/frc971/control_loops/claw/claw.q
@@ -3,19 +3,22 @@
import "aos/common/control_loop/control_loops.q";
// All angles here are 0 horizontal, positive up.
-queue_group ClawLoop {
+queue_group ClawGroup {
implements aos.control_loops.ControlLoop;
message Goal {
- // The angle of the bottom wrist.
+ // The angle of the bottom claw.
double bottom_angle;
- // How much higher the top wrist is.
+ // How much higher the top claw is.
double seperation_angle;
bool intake;
};
message Position {
+ // Top claw position relative to power on.
double top_position;
+
+ // Three Hall Effects with respect to the top claw
bool top_front_hall_effect;
int32_t top_front_hall_effect_posedge_count;
int32_t top_front_hall_effect_negedge_count;
@@ -25,10 +28,18 @@
bool top_back_hall_effect;
int32_t top_back_hall_effect_posedge_count;
int32_t top_back_hall_effect_negedge_count;
+
+ // The encoder value at the last posedge of any of the top claw hall effect
+ // sensors.
double top_posedge_value;
+ // The encoder value at the last negedge of any of the top claw hall effect
+ // sensors.
double top_negedge_value;
+ // bottom claw relative position
double bottom_position;
+
+ // Three Hall Effects with respect to the bottom claw
bool bottom_front_hall_effect;
int32_t bottom_front_hall_effect_posedge_count;
int32_t bottom_front_hall_effect_negedge_count;
@@ -38,7 +49,12 @@
bool bottom_back_hall_effect;
int32_t bottom_back_hall_effect_posedge_count;
int32_t bottom_back_hall_effect_negedge_count;
+
+ // The encoder value at the last posedge of any of the bottom claw hall
+ // effect sensors.
double bottom_posedge_value;
+ // The encoder value at the last negedge of any of the bottom claw hall
+ // effect sensors.
double bottom_negedge_value;
};
@@ -54,4 +70,4 @@
queue aos.control_loops.Status status;
};
-queue_group ClawLoop claw;
+queue_group ClawGroup claw_queue_group;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index e881ef2..c0480a5 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -8,6 +8,8 @@
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw.h"
#include "frc971/constants.h"
+#include "frc971/control_loops/claw/unaugmented_top_claw_motor_plant.h"
+#include "frc971/control_loops/claw/unaugmented_bottom_claw_motor_plant.h"
using ::aos::time::Time;
@@ -16,100 +18,159 @@
namespace control_loops {
namespace testing {
+typedef enum {
+ TOP_CLAW = 0,
+ BOTTOM_CLAW,
+
+ CLAW_COUNT
+} ClawType;
+
// Class which simulates the wrist and sends out queue messages containing the
// position.
class ClawMotorSimulation {
public:
// Constructs a motor simulation. initial_position is the inital angle of the
// wrist, which will be treated as 0 by the encoder.
- ClawMotorSimulation(double initial_position)
- : wrist_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawClawPlant())),
- my_wrist_loop_(".frc971.control_loops.claw",
- 0x1a7b7094, ".frc971.control_loops.claw.goal",
- ".frc971.control_loops.claw.position",
- ".frc971.control_loops.claw.output",
- ".frc971.control_loops.claw.status") {
- Reinitialize(initial_position);
+ ClawMotorSimulation(double initial_top_position,
+ double initial_bottom_position)
+ : top_claw_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawTopClawPlant())),
+ bottom_claw_plant_(
+ new StateFeedbackPlant<2, 1, 1>(MakeRawBottomClawPlant())),
+ claw_queue_group(".frc971.control_loops.claw", 0x1a7b7094,
+ ".frc971.control_loops.claw.goal",
+ ".frc971.control_loops.claw.position",
+ ".frc971.control_loops.claw.output",
+ ".frc971.control_loops.claw.status") {
+ Reinitialize(TOP_CLAW, initial_top_position);
+ Reinitialize(BOTTOM_CLAW, initial_bottom_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;
- last_voltage_ = 0.0;
+ void Reinitialize(ClawType type, double initial_position) {
+ StateFeedbackPlant<2, 1, 1>* plant;
+ if (type == TOP_CLAW) {
+ plant = top_claw_plant_.get();
+ } else {
+ plant = bottom_claw_plant_.get();
+ }
+ initial_position_[type] = initial_position;
+ plant->X(0, 0) = initial_position_[type];
+ plant->X(1, 0) = 0.0;
+ plant->Y = plant->C() * plant->X;
+ last_position_[type] = plant->Y(0, 0);
+ calibration_value_[type] = 0.0;
+ last_voltage_[type] = 0.0;
}
// Returns the absolute angle of the wrist.
- double GetAbsolutePosition() const {
- return wrist_plant_->Y(0, 0);
+ double GetAbsolutePosition(ClawType type) const {
+ if (type == TOP_CLAW) {
+ return top_claw_plant_->Y(0, 0);
+ } else {
+ return bottom_claw_plant_->Y(0, 0);
+ }
}
// Returns the adjusted angle of the wrist.
- double GetPosition() const {
- return GetAbsolutePosition() - initial_position_;
+ double GetPosition(ClawType type) const {
+ return GetAbsolutePosition(type) - initial_position_[type];
+ }
+
+ // Makes sure pos is inside range (inclusive)
+ bool CheckRange(double pos, double low_limit, double hi_limit) {
+ return (pos >= low_limit && pos <= hi_limit);
+ }
+
+ double CheckCalibration(ClawType type, bool hall_effect, double start_angle,
+ double stop_angle) {
+ if ((last_position_[type] < start_angle ||
+ last_position_[type] > stop_angle) &&
+ hall_effect) {
+ calibration_value_[type] = start_angle - initial_position_[type];
+ }
+ return calibration_value_[type];
}
// Sends out the position queue messages.
void SendPositionMessage() {
- const double angle = GetPosition();
-
- ::aos::ScopedMessagePtr<control_loops::ClawLoop::Position> position =
- my_wrist_loop_.position.MakeMessage();
- position->pos = angle;
+ ::aos::ScopedMessagePtr<control_loops::ClawGroup::Position> position =
+ claw_queue_group.position.MakeMessage();
+ position->top_position = GetPosition(TOP_CLAW);
+ position->bottom_position = GetPosition(BOTTOM_CLAW);
// Signal that the hall effect sensor has been triggered if it is within
// the correct range.
- double abs_position = GetAbsolutePosition();
- if (abs_position >= constants::GetValues().wrist_hall_effect_start_angle &&
- abs_position <= constants::GetValues().wrist_hall_effect_stop_angle) {
- position->hall_effect = true;
- } else {
- position->hall_effect = false;
- }
+ double pos[2] = {GetAbsolutePosition(TOP_CLAW),
+ GetAbsolutePosition(BOTTOM_CLAW)};
+ const frc971::constants::Values& v = constants::GetValues();
+ position->top_front_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_front_heffect_start_angle,
+ v.claw_front_heffect_stop_angle);
+ position->top_calibration_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_calib_heffect_start_angle,
+ v.claw_calib_heffect_stop_angle);
+ position->top_back_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_back_heffect_start_angle,
+ v.claw_back_heffect_stop_angle);
+ position->bottom_front_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_front_heffect_start_angle,
+ v.claw_front_heffect_stop_angle);
+ position->bottom_calibration_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_calib_heffect_start_angle,
+ v.claw_calib_heffect_stop_angle);
+ position->bottom_back_hall_effect =
+ CheckRange(pos[TOP_CLAW], v.claw_back_heffect_start_angle,
+ v.claw_back_heffect_stop_angle);
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- if ((last_position_ <
- constants::GetValues().wrist_hall_effect_start_angle ||
- last_position_ >
- constants::GetValues().wrist_hall_effect_stop_angle) &&
- position->hall_effect) {
- calibration_value_ =
- constants::GetValues().wrist_hall_effect_start_angle -
- initial_position_;
- }
-
- position->calibration = calibration_value_;
+ position->top_calibration = CheckCalibration(
+ TOP_CLAW, position->top_calibration_hall_effect,
+ v.claw_calib_heffect_start_angle, v.claw_calib_heffect_stop_angle);
+ position->bottom_calibration = CheckCalibration(
+ BOTTOM_CLAW, position->bottom_calibration_hall_effect,
+ v.claw_calib_heffect_start_angle, v.claw_calib_heffect_stop_angle);
position.Send();
}
- // Simulates the wrist moving for one timestep.
+ // Simulates the claw moving for one timestep.
void Simulate() {
- last_position_ = wrist_plant_->Y(0, 0);
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- wrist_plant_->U << last_voltage_;
- wrist_plant_->Update();
-
- EXPECT_GE(constants::GetValues().wrist_upper_physical_limit,
- wrist_plant_->Y(0, 0));
- EXPECT_LE(constants::GetValues().wrist_lower_physical_limit,
- wrist_plant_->Y(0, 0));
- last_voltage_ = my_wrist_loop_.output->voltage;
+ const frc971::constants::Values& v = constants::GetValues();
+ EXPECT_TRUE(claw_queue_group.output.FetchLatest());
+ Simulate(TOP_CLAW, top_claw_plant_.get(), v.claw_upper_limit,
+ v.claw_lower_limit, claw_queue_group.output->top_claw_voltage);
+ Simulate(BOTTOM_CLAW, bottom_claw_plant_.get(), v.claw_upper_limit,
+ v.claw_lower_limit, claw_queue_group.output->bottom_claw_voltage);
}
- ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> wrist_plant_;
+ void Simulate(ClawType type, StateFeedbackPlant<2, 1, 1>* plant,
+ double upper_limit, double lower_limit, double nl_voltage) {
+ last_position_[type] = plant->Y(0, 0);
+ plant->U << last_voltage_[type];
+ plant->Update();
+
+ // check top claw inside limits
+ EXPECT_GE(upper_limit, plant->Y(0, 0));
+ EXPECT_LE(lower_limit, plant->Y(0, 0));
+ // check bottom claw inside limits
+ EXPECT_GE(upper_limit, plant->Y(0, 0));
+ EXPECT_LE(lower_limit, plant->Y(0, 0));
+ last_voltage_[type] = nl_voltage;
+ }
+
+ // Top of the claw, the one with rollers
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> top_claw_plant_;
+ // Bottom of the claw, the one with tusks
+ ::std::unique_ptr<StateFeedbackPlant<2, 1, 1>> bottom_claw_plant_;
private:
- ClawLoop my_wrist_loop_;
- double initial_position_;
- double last_position_;
- double calibration_value_;
- double last_voltage_;
+ ClawGroup claw_queue_group;
+ double initial_position_[CLAW_COUNT];
+ double last_position_[CLAW_COUNT];
+ double calibration_value_[CLAW_COUNT];
+ double last_voltage_[CLAW_COUNT];
};
+
class ClawTest : public ::testing::Test {
protected:
::aos::common::testing::GlobalCoreInstance my_core;
@@ -117,19 +178,25 @@
// 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.
- ClawLoop my_wrist_loop_;
+ ClawGroup claw_queue_group;
// Create a loop and simulation plant.
- ClawMotor wrist_motor_;
- ClawMotorSimulation wrist_motor_plant_;
+ ClawMotor claw_motor_;
+ ClawMotorSimulation claw_motor_plant_;
- ClawTest() : 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) {
+ // Minimum amount of acceptable seperation between the top and bottom of the
+ // claw.
+ double min_seperation_;
+
+ ClawTest()
+ : claw_queue_group(".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"),
+ claw_motor_(&claw_queue_group),
+ claw_motor_plant_(0.5, 1.0),
+ min_seperation_(constants::GetValues().claw_min_seperation) {
// Flush the robot state queue so we can use clean shared memory for this
// test.
::aos::robot_state.Clear();
@@ -144,11 +211,14 @@
}
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);
+ claw_queue_group.goal.FetchLatest();
+ claw_queue_group.position.FetchLatest();
+ double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
+ double seperation =
+ claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
+ EXPECT_NEAR(claw_queue_group.goal->bottom_angle, bottom, 1e-4);
+ EXPECT_NEAR(claw_queue_group.goal->seperation_angle, seperation, 1e-4);
+ EXPECT_TRUE(min_seperation_ <= seperation);
}
virtual ~ClawTest() {
@@ -158,11 +228,15 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 400; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ bottom_claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
SendDSPacket(true);
}
VerifyNearGoal();
@@ -170,13 +244,19 @@
// Tests that the wrist zeros correctly starting on the hall effect sensor.
TEST_F(ClawTest, ZerosStartingOn) {
- wrist_motor_plant_.Reinitialize(90 * M_PI / 180.0);
+ claw_motor_plant_.Reinitialize(TOP_CLAW, 90 * M_PI / 180.0);
+ claw_motor_plant_.Reinitialize(BOTTOM_CLAW, 100 * M_PI / 180.0);
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ bottom_claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
SendDSPacket(true);
}
VerifyNearGoal();
@@ -184,13 +264,18 @@
// Tests that missing positions are correctly handled.
TEST_F(ClawTest, HandleMissingPosition) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 400; ++i) {
if (i % 23) {
- wrist_motor_plant_.SendPositionMessage();
+ claw_motor_plant_.SendPositionMessage();
}
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ claw_motor_.Iterate();
+ bottom_claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
SendDSPacket(true);
}
VerifyNearGoal();
@@ -198,26 +283,36 @@
// Tests that loosing the encoder for a second triggers a re-zero.
TEST_F(ClawTest, RezeroWithMissingPos) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .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();
+ claw_motor_plant_.SendPositionMessage();
} else {
if (i > 310) {
// Should be re-zeroing now.
- EXPECT_TRUE(wrist_motor_.is_uninitialized());
+ EXPECT_TRUE(claw_motor_.is_uninitialized());
+ EXPECT_TRUE(bottom_claw_motor_.is_uninitialized());
}
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.2).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.2)
+ .seperation_angle(0.2)
+ .Send();
}
if (i == 410) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_TRUE(claw_motor_.is_zeroing());
+ // TODO(austin): Expose if the top and bototm is zeroing through
+ // functions.
+ // EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
}
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
SendDSPacket(true);
}
VerifyNearGoal();
@@ -226,19 +321,24 @@
// Tests that disabling while zeroing sends the state machine into the
// uninitialized state.
TEST_F(ClawTest, DisableGoesUninitialized) {
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 800; ++i) {
- wrist_motor_plant_.SendPositionMessage();
+ claw_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_TRUE(wrist_motor_.is_uninitialized());
+ EXPECT_TRUE(top_claw_motor_.is_uninitialized());
+ EXPECT_TRUE(bottom_claw_motor_.is_uninitialized());
// When disabled, we should be applying 0 power.
- EXPECT_TRUE(my_wrist_loop_.output.FetchLatest());
- EXPECT_EQ(0, my_wrist_loop_.output->voltage);
+ EXPECT_TRUE(claw_queue_group.output.FetchLatest());
+ EXPECT_EQ(0, claw_queue_group.output->top_claw_voltage);
+ EXPECT_EQ(0, claw_queue_group.output->bottom_claw_voltage);
}
} else {
SendDSPacket(true);
@@ -246,10 +346,12 @@
if (i == 202) {
// Verify that we are zeroing after the bot gets enabled again.
EXPECT_TRUE(wrist_motor_.is_zeroing());
+ // TODO(austin): Expose if the top and bottom is zeroing through
+ // functions.
}
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
}
VerifyNearGoal();
}
@@ -257,76 +359,123 @@
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(ClawTest, NoWindupNegative) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ int capped_count[2] = {0, 0};
+ double saved_zeroing_position[2] = {0, 0};
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
+ claw_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ saved_zeroing_position[TOP_CLAW] =
+ top_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = -100.0;
+ saved_zeroing_position[BOTTOM_CLAW] =
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_;
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_ = -100.0;
} else if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position,
- wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
+ top_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
+ if (!top_claw_motor_.is_ready()) {
+ if (top_claw_motor_.capped_goal()) {
+ ++capped_count[TOP_CLAW];
// 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.
- ASSERT_LT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_LT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
} else {
- ASSERT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_GT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+ if (!bottom_claw_motor_.is_ready()) {
+ if (bottom_claw_motor_.capped_goal()) {
+ ++capped_count[BOTTOM_CLAW];
+ // 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.
+ ASSERT_LT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
}
}
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ top_claw_motor_.Iterate();
+ bottom_claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
SendDSPacket(true);
}
VerifyNearGoal();
- EXPECT_GT(3, capped_count);
+ EXPECT_GT(3, capped_count[TOP_CLAW]);
+ EXPECT_GT(3, capped_count[BOTTOM_CLAW]);
}
// Tests that the wrist can't get too far away from the zeroing position if the
// zeroing position is saturating the goal.
TEST_F(ClawTest, NoWindupPositive) {
- int capped_count = 0;
- double saved_zeroing_position = 0;
- my_wrist_loop_.goal.MakeWithBuilder().goal(0.1).Send();
+ int capped_count[2] = {0, 0};
+ double saved_zeroing_position[2] = {0, 0};
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
for (int i = 0; i < 500; ++i) {
- wrist_motor_plant_.SendPositionMessage();
+ claw_motor_plant_.SendPositionMessage();
if (i == 50) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
// Move the zeroing position far away and verify that it gets moved back.
- saved_zeroing_position = wrist_motor_.zeroed_joint_.zeroing_position_;
- wrist_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ saved_zeroing_position[TOP_CLAW] =
+ top_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = 100.0;
+ saved_zeroing_position[BOTTOM_CLAW] =
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_;
+ top_claw_motor_.zeroed_joint_.zeroing_position_ = 100.0;
} else {
if (i == 51) {
- EXPECT_TRUE(wrist_motor_.is_zeroing());
- EXPECT_NEAR(saved_zeroing_position, wrist_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ EXPECT_TRUE(top_claw_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
+ top_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
+ EXPECT_TRUE(bottom_claw_motor_.is_zeroing());
+ EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
+ bottom_claw_motor_.zeroed_joint_.zeroing_position_, 0.4);
}
}
- if (!wrist_motor_.is_ready()) {
- if (wrist_motor_.capped_goal()) {
- ++capped_count;
+ if (!top_claw_motor_.is_ready()) {
+ if (top_claw_motor_.capped_goal()) {
+ ++capped_count[TOP_CLAW];
// 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_.zeroed_joint_.U_uncapped()));
+ ASSERT_LT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
} else {
- EXPECT_GT(5, ::std::abs(wrist_motor_.zeroed_joint_.U_uncapped()));
+ ASSERT_GT(5, ::std::abs(top_claw_motor_.zeroed_joint_.U_uncapped()));
+ }
+ }
+ if (!bottom_claw_motor_.is_ready()) {
+ if (bottom_claw_motor_.capped_goal()) {
+ ++capped_count[BOTTOM_CLAW];
+ // 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.
+ ASSERT_LT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
+ } else {
+ ASSERT_GT(5, ::std::abs(bottom_claw_motor_.zeroed_joint_.U_uncapped()));
}
}
- wrist_motor_.Iterate();
- wrist_motor_plant_.Simulate();
+ top_claw_motor_.Iterate();
+ bottom_claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
SendDSPacket(true);
}
VerifyNearGoal();
- EXPECT_GT(3, capped_count);
+ EXPECT_GT(3, capped_count[TOP_CLAW]);
+ EXPECT_GT(3, capped_count[BOTTOM_CLAW]);
}
} // namespace testing