Merge remote-tracking branch 'brian/devel' into claw
diff --git a/frc971/constants.cc b/frc971/constants.cc
index e933d59..672d5ef 100755
--- a/frc971/constants.cc
+++ b/frc971/constants.cc
@@ -79,6 +79,7 @@
{0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
+ 4.0,
}
};
break;
@@ -111,6 +112,7 @@
{0.0, 2.05, 0.02, 2.02, {-0.1, 0.05}, {1.0, 1.1}, {2.0, 2.1}},
0.01, // claw_unimportant_epsilon
0.9, // start_fine_tune_pos
+ 4.0,
}
};
break;
diff --git a/frc971/constants.h b/frc971/constants.h
index 3f848ac..68dfea4 100755
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -93,6 +93,7 @@
double claw_unimportant_epsilon;
double start_fine_tune_pos;
+ double max_zeroing_voltage;
};
Claws claw;
};
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 667d2a4..cfe61a0 100755
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -46,7 +46,25 @@
namespace control_loops {
void ClawLimitedLoop::CapU() {
- double max_value = ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
+ uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
+ if (is_zeroing_) {
+ const frc971::constants::Values &values = constants::GetValues();
+ if (uncapped_average_voltage_ > values.claw.max_zeroing_voltage) {
+ const double difference =
+ uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
+ U(0, 0) -= difference;
+ U(1, 0) -= difference;
+ } else if (uncapped_average_voltage_ < -values.claw.max_zeroing_voltage) {
+ const double difference =
+ -uncapped_average_voltage_ - values.claw.max_zeroing_voltage;
+ U(0, 0) += difference;
+ U(1, 0) += difference;
+ }
+ }
+
+ double max_value =
+ ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
+
if (max_value > 12.0) {
LOG(DEBUG, "Capping U because max is %f\n", max_value);
U = U * 12.0 / max_value;
@@ -64,7 +82,8 @@
bottom_claw_(this),
claw_(MakeClawLoop()),
was_enabled_(false),
- doing_calibration_fine_tune_(false) {}
+ doing_calibration_fine_tune_(false),
+ capped_goal_(false) {}
const int ZeroedStateFeedbackLoop::kZeroingMaxVoltage;
@@ -91,7 +110,6 @@
return true;
}
if (front_hall_effect_negedge_count_changed()) {
- LOG(INFO, "%s Value is %f last is %f\n", name_, negedge_value_, last_encoder());
if (negedge_value_ - last_encoder() > 0) {
*edge_angle = claw_values.front.upper_angle;
LOG(INFO, "%s Negedge front upper edge -> %f\n", name_, *edge_angle);
@@ -388,17 +406,11 @@
}
// TODO(austin): Limit the goals here.
- // Need to prevent windup, limit voltage, deal with windup on only 1 claw,
- // ...
- if (top_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
- }
- if (bottom_claw_.zeroing_state() ==
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
- }
- if (bottom_claw_.zeroing_state() !=
- ZeroedStateFeedbackLoop::UNKNOWN_POSITION) {
+ if ((bottom_claw_.zeroing_state() !=
+ ZeroedStateFeedbackLoop::UNKNOWN_POSITION ||
+ bottom_claw_.front_hall_effect() || top_claw_.front_hall_effect()) &&
+ !top_claw_.back_hall_effect() && !bottom_claw_.back_hall_effect()) {
if (enabled) {
// Time to slowly move back up to find any position to narrow down the
// zero.
@@ -435,22 +447,6 @@
// TODO(austin): Handle disabled properly everwhere... Restart and all that
// jazz.
- // TODO(Joe): Write this.
- if (bottom_claw_goal_ < values.bottom.lower_limit) {
- bottom_claw_goal_ = values.bottom.lower_limit;
- }
- if (bottom_claw_goal_ > values.bottom.upper_limit) {
- bottom_claw_goal_ = values.bottom.upper_limit;
- }
-
- if (top_claw_goal_ < values.bottom.lower_limit) {
- top_claw_goal_ = values.bottom.lower_limit;
- }
- if (top_claw_goal_ > values.top.upper_limit) {
- top_claw_goal_ = values.top.upper_limit;
- }
-
- // TODO(austin): ...
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, 0, 0;
double separation = -971;
@@ -460,40 +456,39 @@
LOG(DEBUG, "Goal is %f (bottom) %f, separation is %f\n", claw_.R(0, 0),
claw_.R(1, 0), separation);
+ // Only cap power when one of the halves of the claw is unknown.
+ claw_.set_is_zeroing(mode == UNKNOWN_LOCATION);
claw_.Update(output == nullptr);
} else {
claw_.Update(true);
}
- (void) mode;
- /*
+ capped_goal_ = false;
switch (mode) {
case READY:
break;
case FINE_TUNE:
break;
- case UNKNOWN_LOCATION:
- if (top_claw_->uncapped_voltage() > values.max_zeroing_voltage) {
- double dx =
- (top_claw_->uncapped_voltage() - values.max_zeroing_voltage) /
- top_claw_->K(0, 0);
- zeroing_position_ -= dx;
+ case UNKNOWN_LOCATION: {
+ if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
+ double dx = (claw_.uncapped_average_voltage() -
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ bottom_claw_goal_ -= dx;
+ top_claw_goal_ -= dx;
capped_goal_ = true;
- } else if (top_claw_->uncapped_voltage() < -values.max_zeroing_voltage) {
- double dx =
- (top_claw_->uncapped_voltage() + values.max_zeroing_voltage) /
- top_claw_->K(0, 0);
- zeroing_position_ -= dx;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
+ } else if (claw_.uncapped_average_voltage() <
+ -values.claw.max_zeroing_voltage) {
+ double dx = (claw_.uncapped_average_voltage() +
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ bottom_claw_goal_ -= dx;
+ top_claw_goal_ -= dx;
capped_goal_ = true;
+ LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
- break;
- }
- */
-
- if (position) {
- //LOG(DEBUG, "pos: %f hall: %s absolute: %f\n", position->top_position,
- //position->top_calibration_hall_effect ? "true" : "false",
- //zeroed_joint_.absolute_position());
+ } break;
}
if (output) {
@@ -501,8 +496,6 @@
output->bottom_claw_voltage = claw_.U(0, 0);
}
status->done = false;
- //::std::abs(zeroed_joint_.absolute_position() - goal->bottom_angle -
- //goal->seperation_angle) < 0.004;
was_enabled_ = ::aos::robot_state->enabled;
}
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index bef130b..20b30b6 100755
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -24,11 +24,21 @@
class ClawLimitedLoop : public StateFeedbackLoop<4, 2, 2> {
public:
ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
- : StateFeedbackLoop<4, 2, 2>(loop) {}
+ : StateFeedbackLoop<4, 2, 2>(loop),
+ uncapped_average_voltage_(0.0),
+ is_zeroing_(true) {}
virtual void CapU();
+ void set_is_zeroing(bool is_zeroing) { is_zeroing_ = is_zeroing; }
+
void ChangeTopOffset(double doffset);
void ChangeBottomOffset(double doffset);
+
+ double uncapped_average_voltage() const { return uncapped_average_voltage_; }
+
+ private:
+ double uncapped_average_voltage_;
+ bool is_zeroing_;
};
class ClawMotor;
@@ -229,6 +239,9 @@
&control_loops::claw_queue_group);
// True if the state machine is ready.
+ bool capped_goal() const { return capped_goal_; }
+
+ // True if the state machine is ready.
bool is_ready() const { return false; }
void ChangeTopOffset(double doffset);
@@ -269,6 +282,8 @@
// distance.
double initial_seperation_;
+ bool capped_goal_;
+
DISALLOW_COPY_AND_ASSIGN(ClawMotor);
};
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index c6c7e89..0d4a7be 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -451,8 +451,8 @@
}
// Tests that the wrist zeros correctly starting on the hall effect sensor.
-TEST_F(ClawTest, ZerosStartingOn) {
- claw_motor_plant_.Reinitialize(100 * M_PI / 180.0, 90 * M_PI / 180.0);
+TEST_F(ClawTest, ZerosInTheMiddle) {
+ claw_motor_plant_.Reinitialize(0.5, 0.4);
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.1)
@@ -468,13 +468,37 @@
VerifyNearGoal();
}
-// Tests that missing positions are correctly handled.
-TEST_F(ClawTest, HandleMissingPosition) {
+class ZeroingClawTest
+ : public ClawTest,
+ public ::testing::WithParamInterface< ::std::pair<double, double>> {};
+
+// Tests that the wrist zeros correctly starting on the hall effect sensor.
+TEST_P(ZeroingClawTest, ParameterizedZero) {
+ claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
+
claw_queue_group.goal.MakeWithBuilder()
.bottom_angle(0.1)
.seperation_angle(0.2)
.Send();
- for (int i = 0; i < 500; ++i) {
+ for (int i = 0; i < 600; ++i) {
+ claw_motor_plant_.SendPositionMessage();
+ claw_motor_.Iterate();
+ claw_motor_plant_.Simulate();
+
+ SendDSPacket(true);
+ }
+ VerifyNearGoal();
+}
+
+// Tests that missing positions are correctly handled.
+TEST_P(ZeroingClawTest, HandleMissingPosition) {
+ claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
+
+ claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(0.1)
+ .seperation_angle(0.2)
+ .Send();
+ for (int i = 0; i < 600; ++i) {
if (i % 23) {
claw_motor_plant_.SendPositionMessage();
}
@@ -486,6 +510,30 @@
VerifyNearGoal();
}
+INSTANTIATE_TEST_CASE_P(ZeroingClawTest, ZeroingClawTest,
+ ::testing::Values(::std::make_pair(0.04, 0.02),
+ ::std::make_pair(0.2, 0.1),
+ ::std::make_pair(0.3, 0.2),
+ ::std::make_pair(0.4, 0.3),
+ ::std::make_pair(0.5, 0.4),
+ ::std::make_pair(0.6, 0.5),
+ ::std::make_pair(0.7, 0.6),
+ ::std::make_pair(0.8, 0.7),
+ ::std::make_pair(0.9, 0.8),
+ ::std::make_pair(1.0, 0.9),
+ ::std::make_pair(1.1, 1.0),
+ ::std::make_pair(1.15, 1.05),
+ ::std::make_pair(1.05, 0.95),
+ ::std::make_pair(1.1, 1.0),
+ ::std::make_pair(1.2, 1.1),
+ ::std::make_pair(1.3, 1.2),
+ ::std::make_pair(1.4, 1.3),
+ ::std::make_pair(1.5, 1.4),
+ ::std::make_pair(1.6, 1.5),
+ ::std::make_pair(1.7, 1.6),
+ ::std::make_pair(1.8, 1.7),
+ ::std::make_pair(2.015, 2.01)));
+
/*
// Tests that loosing the encoder for a second triggers a re-zero.
TEST_F(ClawTest, RezeroWithMissingPos) {