Cleaned stuff up and made claw tests actually pass.
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
index 1910989..1934032 100644
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -51,14 +51,14 @@
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;
+ 1 / 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;
+ 1 / uncapped_average_voltage_ * values.claw.max_zeroing_voltage;
+ U(0, 0) *= difference;
+ U(1, 0) *= difference;
}
}
@@ -68,14 +68,14 @@
bool bottom_big = (::std::abs(U(0, 0)) > 12.0) &&
(::std::abs(U(0, 0)) > ::std::abs(U(1, 0)));
bool top_big = (::std::abs(U(1, 0)) > 12.0) && (!bottom_big);
- double separation_voltage = U(1, 0) - U(0, 0) / 3.0000;
+ double separation_voltage = U(1, 0) - U(0, 0) * kClawMomentOfInertiaRatio;
double u_top = U(1, 0);
double u_bottom = U(0, 0);
if (bottom_big) {
LOG(DEBUG, "Capping U because bottom is %f\n", max_value);
u_bottom *= scalar;
- u_top = separation_voltage + u_bottom / 3.0000;
+ u_top = separation_voltage + u_bottom * kClawMomentOfInertiaRatio;
// If we can't maintain the separation, just clip it.
if (u_top > 12.0) u_top = 12.0;
else if (u_top < -12.0) u_top = -12.0;
@@ -83,7 +83,7 @@
else if (top_big) {
LOG(DEBUG, "Capping U because top is %f\n", max_value);
u_top *= scalar;
- u_bottom = (u_top - separation_voltage) * 3.0000;
+ u_bottom = (u_top - separation_voltage) / kClawMomentOfInertiaRatio;
if (u_bottom > 12.0) u_bottom = 12.0;
else if (u_bottom < -12.0) u_bottom = -12.0;
}
@@ -93,7 +93,7 @@
LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
LOG(DEBUG, "Separation Voltage was %f, is now %f\n", separation_voltage,
- U(1, 0) - U(0, 0) / 3.0000);
+ U(1, 0) - U(0, 0) * kClawMomentOfInertiaRatio);
}
ClawMotor::ClawMotor(control_loops::ClawGroup *my_claw)
@@ -132,12 +132,12 @@
if (sensor.negedge_count_changed()) {
if (negedge_value_ > last_encoder()) {
*edge_angle = angles.upper_angle;
- LOG(INFO, "%s Negedge upper of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
+ hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
} else {
*edge_angle = angles.lower_angle;
- LOG(INFO, "%s Negedge lower of %s -> %f\n", name_,
- hall_effect_name, *edge_angle);
+ LOG(INFO, "%s Negedge lower of %s -> %f, last_encoder: %f, negedge_value: %f\n", name_,
+ hall_effect_name, *edge_angle, last_encoder(), negedge_value_);
}
*edge_encoder = negedge_value_;
return true;
@@ -417,7 +417,7 @@
if (!doing_calibration_fine_tune_) {
if (::std::abs(top_absolute_position() -
values.claw.start_fine_tune_pos) <
- values.claw.claw_unimportant_epsilon) {//HERE
+ values.claw.claw_unimportant_epsilon) {
doing_calibration_fine_tune_ = true;
top_claw_goal_ += values.claw.claw_zeroing_speed * dt;
top_claw_velocity_ = bottom_claw_velocity_ =
@@ -455,7 +455,7 @@
// calibrated so we are done fine tuning top
doing_calibration_fine_tune_ = false;
LOG(DEBUG, "Calibrated the top correctly!\n");
- } else { //HERE
+ } else {
doing_calibration_fine_tune_ = false;
top_claw_goal_ = values.claw.start_fine_tune_pos;
top_claw_velocity_ = bottom_claw_velocity_ = 0.0;
@@ -553,12 +553,20 @@
case FINE_TUNE_BOTTOM:
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
+ Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
if (claw_.uncapped_average_voltage() > values.claw.max_zeroing_voltage) {
- double dx = (claw_.uncapped_average_voltage() -
+ double dx_bot = (U(0, 0) -
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
+ double dx_top = (U(1, 0) -
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::max(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
+ U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
LOG(DEBUG, "Uncapped is %f, max is %f, difference is %f\n",
@@ -567,11 +575,18 @@
values.claw.max_zeroing_voltage));
} else if (claw_.uncapped_average_voltage() <
-values.claw.max_zeroing_voltage) {
- double dx = (claw_.uncapped_average_voltage() +
+ double dx_bot = (U(0, 0) +
values.claw.max_zeroing_voltage) /
claw_.K(0, 0);
+ double dx_top = (U(1, 0) +
+ values.claw.max_zeroing_voltage) /
+ claw_.K(0, 0);
+ double dx = ::std::min(dx_top, dx_bot);
bottom_claw_goal_ -= dx;
top_claw_goal_ -= dx;
+ Eigen::Matrix<double, 4, 1> R;
+ R << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_, claw_.R(2, 0), claw_.R(3, 0);
+ U = claw_.K() * (R - claw_.X_hat);
capped_goal_ = true;
LOG(DEBUG, "Moving the goal by %f to prevent windup\n", dx);
}
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index 7917703..3ec3b25 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -82,9 +82,14 @@
return GetAbsolutePosition(type) - initial_position_[type];
}
- // Makes sure pos is inside range (inclusive)
+ // Makes sure pos is inside range (exclusive)
bool CheckRange(double pos, const constants::Values::Claws::AnglePair &pair) {
- return (pos >= pair.lower_angle && pos <= pair.upper_angle);
+ // Note: If the >= and <= signs are used, then the there exists a case
+ // where the wrist starts precisely on the edge and because initial
+ // position and the *edge_value_ are the same, then the comparison
+ // in ZeroedStateFeedbackLoop::DoGetPositionOfEdge will return that
+ // the lower, rather than upper, edge of the hall effect was passed.
+ return (pos > pair.lower_angle && pos < pair.upper_angle);
}
void SetHallEffect(double pos,
@@ -440,28 +445,27 @@
}
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),
+ ::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)
+ ::std::make_pair(1.15, 1.05),
+ ::std::make_pair(1.05, 0.95),
+ ::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)
));
/*
@@ -547,7 +551,7 @@
const frc971::constants::Values& values = constants::GetValues();
bool kicked = false;
bool measured = false;
- for (int i = 0; i < 600; ++i) {
+ for (int i = 0; i < 700; ++i) {
claw_motor_plant_.SendPositionMessage();
if (i >= start_time && mode == claw_motor_.mode() && !kicked) {
EXPECT_EQ(mode, claw_motor_.mode());
diff --git a/frc971/control_loops/claw/claw_motor_plant.h b/frc971/control_loops/claw/claw_motor_plant.h
index 988cc20..80164d8 100644
--- a/frc971/control_loops/claw/claw_motor_plant.h
+++ b/frc971/control_loops/claw/claw_motor_plant.h
@@ -14,6 +14,8 @@
StateFeedbackLoop<4, 2, 2> MakeClawLoop();
+const double kClawMomentOfInertiaRatio = 0.333333;
+
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 0610225..90faf9f 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -62,8 +62,10 @@
"""Returns a template name for StateFeedbackPlantCoefficients."""
return self._GenericType('StateFeedbackPlantCoefficients')
- def WriteHeader(self, header_file):
- """Writes the header file to the file named header_file."""
+ def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
+ """Writes the header file to the file named header_file.
+ Set double_appendage to true in order to include a ratio of
+ moments of inertia constant. Currently, only used for 2014 claw."""
with open(header_file, 'w') as fd:
header_guard = self._HeaderGuard(header_file)
fd.write('#ifndef %s\n'
@@ -85,6 +87,10 @@
fd.write('%s Make%sLoop();\n\n' %
(self._LoopType(), self._gain_schedule_name))
+ fd.write('const double k%sMomentOfInertiaRatio = %f;\n\n' %
+ (self._gain_schedule_name,
+ self._loops[0].J_top / self._loops[0].J_bottom))
+
fd.write(self._namespace_end)
fd.write('\n\n')
fd.write("#endif // %s\n" % header_guard)