merging in new and improved claw code
diff --git a/frc971/control_loops/claw/claw.cc b/frc971/control_loops/claw/claw.cc
old mode 100755
new mode 100644
index 417df9f..58b2ca0
--- a/frc971/control_loops/claw/claw.cc
+++ b/frc971/control_loops/claw/claw.cc
@@ -1,7 +1,5 @@
#include "frc971/control_loops/claw/claw.h"
-#include <stdio.h>
-
#include <algorithm>
#include "aos/common/control_loop/control_loops.q.h"
@@ -53,33 +51,71 @@
ClawLimitedLoop::ClawLimitedLoop(StateFeedbackLoop<4, 2, 2> loop)
: StateFeedbackLoop<4, 2, 2>(loop),
uncapped_average_voltage_(0.0),
- is_zeroing_(true) {}
+ is_zeroing_(true),
+ U_Poly_((Eigen::Matrix<double, 4, 2>() << 1, 0,
+ -1, 0,
+ 0, 1,
+ 0, -1).finished(),
+ (Eigen::Matrix<double, 4, 1>() << kMaxVoltage, kMaxVoltage,
+ kMaxVoltage, kMaxVoltage).finished()) {
+ ::aos::controls::HPolytope<0>::Init();
+}
+// Caps the voltage prioritizing reducing velocity error over reducing
+// positional error.
+// Uses the polytope libararies which we used to just use for the drivetrain.
+// Uses a region representing the maximum voltage and then transforms it such
+// that the points represent different amounts of positional error and
+// constrains the region such that, if at all possible, it will maintain its
+// current efforts to reduce velocity error.
void ClawLimitedLoop::CapU() {
- uncapped_average_voltage_ = U(0, 0) + U(1, 0) / 2.0;
- if (is_zeroing_) {
- LOG(DEBUG, "zeroing\n");
- 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;
- } 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;
- }
+ Eigen::Matrix<double, 4, 1> error = R - X_hat;
+
+ double u_top = U(1, 0);
+ double u_bottom = U(0, 0);
+
+ uncapped_average_voltage_ = (u_top + u_bottom) / 2;
+
+ double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
+
+ if (::std::abs(u_bottom) > max_voltage or ::std::abs(u_top) > max_voltage) {
+ // H * U <= k
+ // U = UPos + UVel
+ // H * (UPos + UVel) <= k
+ // H * UPos <= k - H * UVel
+
+ // Now, we can do a coordinate transformation and say the following.
+
+ // UPos = position_K * position_error
+ // (H * position_K) * position_error <= k - H * UVel
+
+ Eigen::Matrix<double, 2, 2> position_K;
+ position_K << K(0, 0), K(0, 1),
+ K(1, 0), K(1, 1);
+ Eigen::Matrix<double, 2, 2> velocity_K;
+ velocity_K << K(0, 2), K(0, 3),
+ K(1, 2), K(1, 3);
+
+ Eigen::Matrix<double, 2, 1> position_error;
+ position_error << error(0, 0), error(1, 0);
+ Eigen::Matrix<double, 2, 1> velocity_error;
+ velocity_error << error(2, 0), error(3, 0);
+
+ Eigen::Matrix<double, 4, 1> pos_poly_k =
+ U_Poly_.k() - U_Poly_.H() * velocity_K * velocity_error;
+ Eigen::Matrix<double, 4, 2> pos_poly_H = U_Poly_.H() * position_K;
+ ::aos::controls::HPolytope<2> pos_poly(pos_poly_H, pos_poly_k);
+
+
+ Eigen::Matrix<double, 2, 1> adjusted_pos_error = CoerceGoal(
+ pos_poly, (Eigen::Matrix<double, 1, 2>() << position_error(1, 0),
+ -position_error(0, 0)).finished(),
+ 0.0, position_error);
+
+ LOG(DEBUG, "Capping U is now %f %f\n", U(0, 0), U(1, 0));
+ U = velocity_K * velocity_error + position_K * adjusted_pos_error;
}
- double max_value =
- ::std::max(::std::abs(U(0, 0)), ::std::abs(U(1, 0) + U(0, 0)));
-
- const double k_max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
- if (max_value > k_max_voltage) {
- U = U * k_max_voltage / max_value;
- LOG(DEBUG, "Capping U is now %f %f (max is %f)\n",
- U(0, 0), U(1, 0), max_value);
- }
}
ZeroedStateFeedbackLoop::ZeroedStateFeedbackLoop(const char *name,
@@ -705,11 +741,21 @@
case FINE_TUNE_BOTTOM:
case FINE_TUNE_TOP:
case UNKNOWN_LOCATION: {
+ Eigen::Matrix<double, 2, 1> U = claw_.K() * (claw_.R - claw_.X_hat);
+ LOG(DEBUG, "Uncapped voltages: Top: %f, Bottom: %f\n", U(1, 0), U(0, 0));
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);
+ 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."
" Uncapped is %f, max is %f, difference is %f\n",
@@ -719,10 +765,18 @@
values.claw.max_zeroing_voltage));
} 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);
+ 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);
}
@@ -741,7 +795,7 @@
? -12.0
: goal->centering;
}
- output->top_claw_voltage = claw_.U(1, 0) + claw_.U(0, 0);
+ output->top_claw_voltage = claw_.U(1, 0);
output->bottom_claw_voltage = claw_.U(0, 0);
if (output->top_claw_voltage > kMaxVoltage) {
diff --git a/frc971/control_loops/claw/claw.gyp b/frc971/control_loops/claw/claw.gyp
index 588ce79..bdce229 100644
--- a/frc971/control_loops/claw/claw.gyp
+++ b/frc971/control_loops/claw/claw.gyp
@@ -29,12 +29,16 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/logging/logging.gyp:queue_logging',
],
'export_dependent_settings': [
'claw_loop',
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
],
},
{
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
old mode 100755
new mode 100644
index ced9b80..dab1dca
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -4,8 +4,10 @@
#include <memory>
#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/controls/polytope.h"
#include "frc971/constants.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
#include "frc971/control_loops/hall_effect_tracker.h"
@@ -36,6 +38,8 @@
private:
double uncapped_average_voltage_;
bool is_zeroing_;
+
+ const ::aos::controls::HPolytope<2> U_Poly_;
};
class ClawMotor;
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index a962124..8704ad0 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,
@@ -212,8 +217,7 @@
EXPECT_TRUE(claw_queue_group.output.FetchLatest());
claw_plant_->U << claw_queue_group.output->bottom_claw_voltage,
- claw_queue_group.output->top_claw_voltage -
- claw_queue_group.output->bottom_claw_voltage;
+ claw_queue_group.output->top_claw_voltage;
claw_plant_->Update();
// Check that the claw is within the limits.
@@ -470,7 +474,6 @@
::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),
@@ -478,7 +481,8 @@
::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(2.015, 2.01)
+));
/*
// Tests that loosing the encoder for a second triggers a re-zero.
@@ -559,18 +563,15 @@
protected:
void TestWindup(ClawMotor::CalibrationMode mode, int start_time, double offset) {
int capped_count = 0;
- double saved_zeroing_position[2] = {0, 0};
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());
// Move the zeroing position far away and verify that it gets moved
// back.
- saved_zeroing_position[TOP_CLAW] = claw_motor_.top_claw_goal_;
- saved_zeroing_position[BOTTOM_CLAW] = claw_motor_.bottom_claw_goal_;
claw_motor_.top_claw_goal_ += offset;
claw_motor_.bottom_claw_goal_ += offset;
kicked = true;
@@ -579,10 +580,19 @@
measured = true;
EXPECT_EQ(mode, claw_motor_.mode());
- EXPECT_NEAR(saved_zeroing_position[TOP_CLAW],
- claw_motor_.top_claw_goal_, 0.1);
- EXPECT_NEAR(saved_zeroing_position[BOTTOM_CLAW],
- claw_motor_.bottom_claw_goal_, 0.1);
+ Eigen::Matrix<double, 4, 1> R;
+ R << claw_motor_.bottom_claw_goal_,
+ claw_motor_.top_claw_goal_ - claw_motor_.bottom_claw_goal_, 0.0,
+ 0.0;
+ Eigen::Matrix<double, 2, 1> uncapped_voltage =
+ claw_motor_.claw_.K() * (R - claw_motor_.claw_.X_hat);
+ // Use a factor of 1.8 because so long as it isn't actually running
+ // away, the CapU function will deal with getting the actual output
+ // down.
+ EXPECT_LT(::std::abs(uncapped_voltage(0, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
+ EXPECT_LT(::std::abs(uncapped_voltage(1, 0)),
+ values.claw.max_zeroing_voltage * 1.8);
}
}
if (claw_motor_.mode() == mode) {
diff --git a/frc971/control_loops/claw/claw_motor_plant.cc b/frc971/control_loops/claw/claw_motor_plant.cc
index d78de55..106491d 100644
--- a/frc971/control_loops/claw/claw_motor_plant.cc
+++ b/frc971/control_loops/claw/claw_motor_plant.cc
@@ -9,25 +9,25 @@
StateFeedbackPlantCoefficients<4, 2, 2> MakeClawPlantCoefficients() {
Eigen::Matrix<double, 4, 4> A;
- A << 1.0, 0.0, 0.00807639596609, 0.0, 0.0, 1.0, 0.0, 0.00807639596609, 0.0, 0.0, 0.641687189181, 0.0, 0.0, 0.0, 0.0, 0.641687189181;
+ A << 1.0, 0.0, 0.00737284608086, 0.0, 0.0, 1.0, -0.00294667339472, 0.00442617268614, 0.0, 0.0, 0.525184383468, 0.0, 0.0, 0.0, -0.380328742836, 0.144855640632;
Eigen::Matrix<double, 4, 2> B;
- B << 0.000752046077845, 0.0, 0.0, 0.000752046077845, 0.140084829969, 0.0, 0.0, 0.140084829969;
+ B << 0.00102145540588, 0.0, -0.00102145540588, 0.00216714216844, 0.184611558069, 0.0, -0.184611558069, 0.332485973629;
Eigen::Matrix<double, 2, 4> C;
C << 1, 0, 0, 0, 1, 1, 0, 0;
Eigen::Matrix<double, 2, 2> D;
D << 0, 0, 0, 0;
Eigen::Matrix<double, 2, 1> U_max;
- U_max << 12.0, 24.0;
+ U_max << 12.0, 12.0;
Eigen::Matrix<double, 2, 1> U_min;
- U_min << -12.0, -24.0;
+ U_min << -12.0, -12.0;
return StateFeedbackPlantCoefficients<4, 2, 2>(A, B, C, D, U_max, U_min);
}
StateFeedbackController<4, 2, 2> MakeClawController() {
Eigen::Matrix<double, 4, 2> L;
- L << 1.60168718918, 2.51306790994e-16, -1.60168718918, 1.60168718918, 47.8568612552, 7.50700456808e-15, -47.8568612552, 47.8568612552;
+ L << 1.48518438347, 2.30607329869e-16, -1.48518438347, 1.10485564063, 34.6171964667, 5.33831435952e-15, -34.6171964667, 3.52560374486;
Eigen::Matrix<double, 2, 4> K;
- K << 81.0129676169, 0.0, 1.94955302675, 0.0, 0.0, 113.660854272, 0.0, 2.47702820281;
+ K << 104.272994613, 0.0, 1.72618753001, 0.0, 49.1477742369, 129.930293084, -0.546087759204, 0.551235956004;
return StateFeedbackController<4, 2, 2>(L, K, MakeClawPlantCoefficients());
}
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/coerce_goal.cc b/frc971/control_loops/coerce_goal.cc
new file mode 100644
index 0000000..b32b590
--- /dev/null
+++ b/frc971/control_loops/coerce_goal.cc
@@ -0,0 +1,59 @@
+#include "frc971/control_loops/coerce_goal.h"
+
+#include "Eigen/Dense"
+
+#include "aos/controls/polytope.h"
+
+namespace frc971 {
+namespace control_loops {
+
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R) {
+ if (region.IsInside(R)) {
+ return R;
+ }
+ Eigen::Matrix<double, 2, 1> parallel_vector;
+ Eigen::Matrix<double, 2, 1> perpendicular_vector;
+ perpendicular_vector = K.transpose().normalized();
+ parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
+
+ aos::controls::HPolytope<1> t_poly(
+ region.H() * parallel_vector,
+ region.k() - region.H() * perpendicular_vector * w);
+
+ Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
+ if (vertices.innerSize() > 0) {
+ double min_distance_sqr = 0;
+ Eigen::Matrix<double, 2, 1> closest_point;
+ for (int i = 0; i < vertices.innerSize(); i++) {
+ Eigen::Matrix<double, 2, 1> point;
+ point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
+ const double length = (R - point).squaredNorm();
+ if (i == 0 || length < min_distance_sqr) {
+ closest_point = point;
+ min_distance_sqr = length;
+ }
+ }
+ return closest_point;
+ } else {
+ Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
+ region.Vertices();
+ double min_distance = INFINITY;
+ int closest_i = 0;
+ for (int i = 0; i < region_vertices.outerSize(); i++) {
+ const double length = ::std::abs(
+ (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
+ if (i == 0 || length < min_distance) {
+ closest_i = i;
+ min_distance = length;
+ }
+ }
+ return (Eigen::Matrix<double, 2, 1>() << region_vertices(0, closest_i),
+ region_vertices(1, closest_i)).finished();
+ }
+}
+
+} // namespace control_loops
+} // namespace frc971
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
new file mode 100644
index 0000000..43707b4
--- /dev/null
+++ b/frc971/control_loops/coerce_goal.h
@@ -0,0 +1,23 @@
+#ifndef FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
+#define FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
+
+#include "Eigen/Dense"
+
+#include "aos/controls/polytope.h"
+
+namespace frc971 {
+namespace control_loops {
+
+// Intersects a line with a region, and finds the closest point to R.
+// Finds a point that is closest to R inside the region, and on the line
+// defined by K X = w. If it is not possible to find a point on the line,
+// finds a point that is inside the region and closest to the line.
+Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
+ const Eigen::Matrix<double, 1, 2> &K,
+ double w,
+ const Eigen::Matrix<double, 2, 1> &R);
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_COERCE_GOAL_H_
diff --git a/frc971/control_loops/control_loops.gyp b/frc971/control_loops/control_loops.gyp
index 273063c..29cd29e 100644
--- a/frc971/control_loops/control_loops.gyp
+++ b/frc971/control_loops/control_loops.gyp
@@ -25,6 +25,21 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
+ 'target_name': 'coerce_goal',
+ 'type': 'static_library',
+ 'sources': [
+ 'coerce_goal.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ ],
+ 'export_dependent_settings': [
+ '<(EXTERNALS):eigen',
+ '<(DEPTH)/aos/build/externals.gyp:libcdd',
+ ],
+ },
+ {
'target_name': 'state_feedback_loop',
'type': 'static_library',
'sources': [
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index f864c55..c8b943c 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -14,6 +14,7 @@
#include "aos/common/logging/matrix_logging.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/polydrivetrain_cim_plant.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/queues/other_sensors.q.h"
@@ -27,53 +28,6 @@
// Width of the robot.
const double width = 22.0 / 100.0 * 2.54;
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R) {
- if (region.IsInside(R)) {
- return R;
- }
- Eigen::Matrix<double, 2, 1> parallel_vector;
- Eigen::Matrix<double, 2, 1> perpendicular_vector;
- perpendicular_vector = K.transpose().normalized();
- parallel_vector << perpendicular_vector(1, 0), -perpendicular_vector(0, 0);
-
- aos::controls::HPolytope<1> t_poly(
- region.H() * parallel_vector,
- region.k() - region.H() * perpendicular_vector * w);
-
- Eigen::Matrix<double, 1, Eigen::Dynamic> vertices = t_poly.Vertices();
- if (vertices.innerSize() > 0) {
- double min_distance_sqr = 0;
- Eigen::Matrix<double, 2, 1> closest_point;
- for (int i = 0; i < vertices.innerSize(); i++) {
- Eigen::Matrix<double, 2, 1> point;
- point = parallel_vector * vertices(0, i) + perpendicular_vector * w;
- const double length = (R - point).squaredNorm();
- if (i == 0 || length < min_distance_sqr) {
- closest_point = point;
- min_distance_sqr = length;
- }
- }
- return closest_point;
- } else {
- Eigen::Matrix<double, 2, Eigen::Dynamic> region_vertices =
- region.Vertices();
- double min_distance = INFINITY;
- int closest_i = 0;
- for (int i = 0; i < region_vertices.outerSize(); i++) {
- const double length = ::std::abs(
- (perpendicular_vector.transpose() * (region_vertices.col(i)))(0, 0));
- if (i == 0 || length < min_distance) {
- closest_i = i;
- min_distance = length;
- }
- }
- return region_vertices.col(closest_i);
- }
-}
-
class DrivetrainMotorsSS {
public:
DrivetrainMotorsSS()
diff --git a/frc971/control_loops/drivetrain/drivetrain.gyp b/frc971/control_loops/drivetrain/drivetrain.gyp
index d5d5640..bf73d9f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.gyp
+++ b/frc971/control_loops/drivetrain/drivetrain.gyp
@@ -44,6 +44,7 @@
'<(DEPTH)/frc971/frc971.gyp:constants',
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(DEPTH)/frc971/queues/queues.gyp:queues',
'<(AOS)/common/util/util.gyp:log_interval',
'<(AOS)/common/logging/logging.gyp:queue_logging',
@@ -52,6 +53,7 @@
'export_dependent_settings': [
'<(DEPTH)/aos/build/externals.gyp:libcdd',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/control_loops.gyp:coerce_goal',
'<(AOS)/common/common.gyp:controls',
'drivetrain_loop',
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9ee7a27..2d5e9c9 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -12,11 +12,6 @@
namespace frc971 {
namespace control_loops {
-Eigen::Matrix<double, 2, 1> CoerceGoal(aos::controls::HPolytope<2> ®ion,
- const Eigen::Matrix<double, 1, 2> &K,
- double w,
- const Eigen::Matrix<double, 2, 1> &R);
-
class DrivetrainLoop
: public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
public:
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index e03ef59..0827fec 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -11,6 +11,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/state_feedback_loop.h"
+#include "frc971/control_loops/coerce_goal.h"
#include "frc971/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "frc971/queues/other_sensors.q.h"
diff --git a/frc971/control_loops/python/claw.py b/frc971/control_loops/python/claw.py
index f6b3349..ca69a2b 100755
--- a/frc971/control_loops/python/claw.py
+++ b/frc971/control_loops/python/claw.py
@@ -2,6 +2,8 @@
import control_loop
import controls
+import polytope
+import polydrivetrain
import numpy
import sys
from matplotlib import pylab
@@ -13,15 +15,17 @@
self.stall_torque = 2.42
# Stall Current in Amps
self.stall_current = 133
- # Free Speed in RPM, pulled from drivetrain
+ # Free Speed in RPM
self.free_speed = 5500.0
# Free Current in Amps
self.free_current = 2.7
# Moment of inertia of the claw in kg m^2
- # approzimately 0.76 (without ball) in CAD
- self.J = 1.00
+ # measured from CAD
+ self.J_top = 0.3
+ self.J_bottom = 0.9
+
# Resistance of the motor
- self.R = 12.0 / self.stall_current + 0.024 + .003
+ self.R = 12.0 / self.stall_current
# Motor velocity constant
self.Kv = ((self.free_speed / 60.0 * 2.0 * numpy.pi) /
(13.5 - self.R * self.free_current))
@@ -32,25 +36,53 @@
# Control loop time step
self.dt = 0.01
- # State is [bottom position, top - bottom position,
- # bottom velocity, top - bottom velocity]
- # Input is [bottom power, top power]
- # Motor time constant.
- self.motor_timeconstant = self.Kt / self.Kv / (self.J * self.G * self.G * self.R)
+ # State is [bottom position, bottom velocity, top - bottom position,
+ # top - bottom velocity]
+ # Input is [bottom power, top power - bottom power * J_top / J_bottom]
+ # Motor time constants. difference_bottom refers to the constant for how the
+ # bottom velocity affects the difference of the top and bottom velocities.
+ self.common_motor_constant = -self.Kt / self.Kv / (self.G * self.G * self.R)
+ self.bottom_bottom = self.common_motor_constant / self.J_bottom
+ self.difference_bottom = -self.common_motor_constant * (1 / self.J_bottom
+ - 1 / self.J_top)
+ self.difference_difference = self.common_motor_constant / self.J_top
# State feedback matrices
+
self.A_continuous = numpy.matrix(
[[0, 0, 1, 0],
[0, 0, 0, 1],
- [0, 0, -self.motor_timeconstant, 0],
- [0, 0, 0, -self.motor_timeconstant]])
+ [0, 0, self.bottom_bottom, 0],
+ [0, 0, self.difference_bottom, self.difference_difference]])
- self.motor_feedforward = self.Kt / (self.J * self.G * self.R)
+ self.A_bottom_cont = numpy.matrix(
+ [[0, 1],
+ [0, self.bottom_bottom]])
+ self.A_diff_cont = numpy.matrix(
+ [[0, 1],
+ [0, self.difference_difference]])
+
+ self.motor_feedforward = self.Kt / (self.G * self.R)
+ self.motor_feedforward_bottom = self.motor_feedforward / self.J_bottom
+ self.motor_feedforward_difference = self.motor_feedforward / self.J_top
+ self.motor_feedforward_difference_bottom = (
+ self.motor_feedforward * (1 / self.J_bottom - 1 / self.J_top))
self.B_continuous = numpy.matrix(
[[0, 0],
[0, 0],
- [self.motor_feedforward, 0],
- [0.0, self.motor_feedforward]])
+ [self.motor_feedforward_bottom, 0],
+ [-self.motor_feedforward_bottom, self.motor_feedforward_difference]])
+
+ print "Cont X_ss", self.motor_feedforward / -self.common_motor_constant
+
+ self.B_bottom_cont = numpy.matrix(
+ [[0],
+ [self.motor_feedforward_bottom]])
+
+ self.B_diff_cont = numpy.matrix(
+ [[0],
+ [self.motor_feedforward_difference]])
+
self.C = numpy.matrix([[1, 0, 0, 0],
[1, 1, 0, 0]])
self.D = numpy.matrix([[0, 0],
@@ -59,32 +91,100 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- #controlability = controls.ctrb(self.A, self.B);
- #print "Rank of controlability matrix.", numpy.linalg.matrix_rank(controlability)
+ self.A_bottom, self.B_bottom = controls.c2d(
+ self.A_bottom_cont, self.B_bottom_cont, self.dt)
+ self.A_diff, self.B_diff = controls.c2d(
+ self.A_diff_cont, self.B_diff_cont, self.dt)
- self.Q = numpy.matrix([[(1.0 / (0.12 ** 2.0)), 0.0, 0.0, 0.0],
- [0.0, (1.0 / (0.08 ** 2.0)), 0.0, 0.0],
- [0.0, 0.0, 0.050, 0.0],
- [0.0, 0.0, 0.0, 0.07]])
+ self.K_bottom = controls.dplace(self.A_bottom, self.B_bottom, [.65, .45])
+ self.K_diff = controls.dplace(self.A_diff, self.B_diff, [.40, .28])
- self.R = numpy.matrix([[(1.0 / (12.0 ** 2.0)), 0.0],
- [0.0, (1.0 / (12.0 ** 2.0))]])
- self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+ print "K_diff", self.K_diff
+ print "K_bottom", self.K_bottom
+
+ print "A"
+ print self.A
+ print "B"
+ print self.B
+
+
+ self.Q = numpy.matrix([[(1.0 / (0.10 ** 2.0)), 0.0, 0.0, 0.0],
+ [0.0, (1.0 / (0.06 ** 2.0)), 0.0, 0.0],
+ [0.0, 0.0, 0.10, 0.0],
+ [0.0, 0.0, 0.0, 0.1]])
+
+ self.R = numpy.matrix([[(1.0 / (40.0 ** 2.0)), 0.0],
+ [0.0, (1.0 / (5.0 ** 2.0))]])
+ #self.K = controls.dlqr(self.A, self.B, self.Q, self.R)
+
+ self.K = numpy.matrix([[self.K_bottom[0, 0], 0.0, self.K_bottom[0, 1], 0.0],
+ [0.0, self.K_diff[0, 0], 0.0, self.K_diff[0, 1]]])
+
+ # Compute the feed forwards aceleration term.
+ self.K[1, 0] = -self.B[1, 0] * self.K[0, 0] / self.B[1, 1]
+
+ lstsq_A = numpy.identity(2)
+ lstsq_A[0, :] = self.B[1, :]
+ lstsq_A[1, :] = self.B[3, :]
+ print "System of Equations coefficients:"
+ print lstsq_A
+ print "det", numpy.linalg.det(lstsq_A)
+
+ out_x = numpy.linalg.lstsq(
+ lstsq_A,
+ numpy.matrix([[self.A[1, 2]], [self.A[3, 2]]]))[0]
+ self.K[1, 2] = -lstsq_A[0, 0] * (self.K[0, 2] - out_x[0]) / lstsq_A[0, 1] + out_x[1]
print "K unaugmented"
print self.K
- print "Placed controller poles"
- print numpy.linalg.eig(self.A - self.B * self.K)[0]
+ print "B * K unaugmented"
+ print self.B * self.K
+ F = self.A - self.B * self.K
+ print "A - B * K unaugmented"
+ print F
+ print "eigenvalues"
+ print numpy.linalg.eig(F)[0]
self.rpl = .02
self.ipl = 0.004
self.PlaceObserverPoles([self.rpl + 1j * self.ipl,
- self.rpl - 1j * self.ipl,
self.rpl + 1j * self.ipl,
+ self.rpl - 1j * self.ipl,
self.rpl - 1j * self.ipl])
- self.U_max = numpy.matrix([[12.0], [24.0]])
- self.U_min = numpy.matrix([[-12.0], [-24.0]])
+ # The box formed by U_min and U_max must encompass all possible values,
+ # or else Austin's code gets angry.
+ self.U_max = numpy.matrix([[12.0], [12.0]])
+ self.U_min = numpy.matrix([[-12.0], [-12.0]])
+
+ # Compute the steady state velocities for a given applied voltage.
+ # The top and bottom of the claw should spin at the same rate if the
+ # physics is right.
+ X_ss = numpy.matrix([[0], [0], [0.0], [0]])
+
+ U = numpy.matrix([[1.0],[1.0]])
+ A = self.A
+ B = self.B
+ #X_ss[2, 0] = X_ss[2, 0] * A[2, 2] + B[2, 0] * U[0, 0]
+ X_ss[2, 0] = 1 / (1 - A[2, 2]) * B[2, 0] * U[0, 0]
+ #X_ss[3, 0] = X_ss[3, 0] * A[3, 3] + X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ #X_ss[3, 0] * (1 - A[3, 3]) = X_ss[2, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[3, 0] = 1 / (1 - A[3, 3]) * (X_ss[2, 0] * A[3, 2] + B[3, 1] * U[1, 0] + B[3, 0] * U[0, 0])
+ #X_ss[3, 0] = 1 / (1 - A[3, 3]) / (1 - A[2, 2]) * B[2, 0] * U[0, 0] * A[3, 2] + B[3, 0] * U[0, 0] + B[3, 1] * U[1, 0]
+ X_ss[0, 0] = A[0, 2] * X_ss[2, 0] + B[0, 0] * U[0, 0]
+ X_ss[1, 0] = A[1, 2] * X_ss[2, 0] + A[1, 3] * X_ss[3, 0] + B[1, 0] * U[0, 0] + B[1, 1] * U[1, 0]
+
+ print "X_ss", X_ss
+
+ self.U_poly = polytope.HPolytope(
+ numpy.matrix([[1, 0],
+ [-1, 0],
+ [0, 1],
+ [0, -1]]),
+ numpy.matrix([[12],
+ [12],
+ [12],
+ [12]]))
self.InitializeState()
@@ -165,11 +265,9 @@
def FullSeparationPriority(claw, U):
bottom_u = U[0, 0]
- top_u = U[1, 0] + bottom_u
+ top_u = U[1, 0]
- #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
if bottom_u > claw.U_max[0, 0]:
- #print "Bottom is too big. Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
top_u -= bottom_u - claw.U_max[0, 0]
if top_u < claw.U_min[1, 0]:
top_u = claw.U_min[1, 0]
@@ -194,20 +292,104 @@
bottom_u = claw.U_min[0, 0]
- return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+ return numpy.matrix([[bottom_u], [top_u]])
-def AverageUFix(claw, U):
+def ScaleU(claw, U, K, error):
+ """Clips U as necessary.
+
+ Args:
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ """
bottom_u = U[0, 0]
- top_u = U[1, 0] + bottom_u
+ top_u = U[1, 0]
- #print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
- if (bottom_u > claw.U_max[0, 0] or top_u > claw.U_max[1, 0] or
- top_u < claw.U_min[1, 0] or bottom_u < claw.U_min[0, 0]):
- scalar = 12.0 / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ if (bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0] or
+ top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]):
+
+ position_K = K[:, 0:2]
+ velocity_K = K[:, 2:]
+ position_error = error[0:2, 0]
+ velocity_error = error[2:, 0]
+
+ # H * U <= k
+ # U = UPos + UVel
+ # H * (UPos + UVel) <= k
+ # H * UPos <= k - H * UVel
+ #
+ # Now, we can do a coordinate transformation and say the following.
+ #
+ # UPos = position_K * position_error
+ # (H * position_K) * position_error <= k - H * UVel
+ #
+ # Add in the constraint that 0 <= t <= 1
+ # Now, there are 2 ways this can go. Either we have a region, or we don't
+ # have a region. If we have a region, then pick the largest t and go for it.
+ # If we don't have a region, we need to pick a good comprimise.
+
+ # First prototype! -> Only cap the position power, leave the velocity power active.
+
+ #u_velocity = velocity_K * velocity_error
+ #u_position = position_K * position_error
+ #scalar = min(1.0, claw.U_max[0, 0] / max(numpy.abs(u_position[0, 0]), numpy.abs(u_position[1, 0])))
+ #return u_velocity + scalar * u_position
+
+ pos_poly = polytope.HPolytope(
+ claw.U_poly.H * position_K,
+ claw.U_poly.k - claw.U_poly.H * velocity_K * velocity_error)
+
+ adjusted_pos_error = polydrivetrain.CoerceGoal(pos_poly, numpy.matrix([[position_error[1, 0], -position_error[0, 0]]]), 0.0, position_error)
+
+ return velocity_K * velocity_error + position_K * adjusted_pos_error
+
+ #U = Kpos * poserror + Kvel * velerror
+
+ #scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+
+ #top_u *= scalar
+ #bottom_u *= scalar
+
+ return numpy.matrix([[bottom_u], [top_u]])
+
+def AverageUFix(claw, U, preserve_v3=False):
+ """Clips U as necessary.
+
+ Args:
+ claw: claw object containing moments of inertia and U limits.
+ U: Input matrix to clip as necessary.
+ preserve_v3: There are two ways to attempt to clip the voltages:
+ -If you preserve the imaginary v3, then it will attempt to
+ preserve the effect on the separation of the two claws.
+ If it is not able to do this, then it calls itself with preserve_v3
+ set to False.
+ -If you preserve the ratio of the voltage of the bottom and the top,
+ then it will attempt to preserve the ratio of those two. This is
+ equivalent to preserving the ratio of v3 and the bottom voltage.
+ """
+ bottom_u = U[0, 0]
+ top_u = U[1, 0]
+ seperation_u = top_u - bottom_u * claw.J_top / claw.J_bottom
+
+ bottom_bad = bottom_u > claw.U_max[0, 0] or bottom_u < claw.U_min[0, 0]
+ top_bad = top_u > claw.U_max[0, 0] or top_u < claw.U_min[0, 0]
+
+ scalar = claw.U_max[0, 0] / max(numpy.abs(top_u), numpy.abs(bottom_u))
+ if bottom_bad and preserve_v3:
+ bottom_u *= scalar
+ top_u = seperation_u + bottom_u * claw.J_top / claw.J_bottom
+ if abs(top_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif top_bad and preserve_v3:
+ top_u *= scalar
+ bottom_u = (top_u - seperation_u) * claw.J_bottom / claw.J_top
+ if abs(bottom_u) > claw.U_max[0, 0]:
+ return AverageUFix(claw, U, preserve_v3=False)
+ elif (bottom_bad or top_bad) and not preserve_v3:
top_u *= scalar
bottom_u *= scalar
+ print "Scaling"
- return numpy.matrix([[bottom_u], [top_u - bottom_u]])
+ return numpy.matrix([[bottom_u], [top_u]])
def ClipDeltaU(claw, U):
delta_u = U[0, 0]
@@ -219,7 +401,6 @@
#print "Bottom is", new_unclipped_bottom_u, "Top is", top_u
if new_unclipped_bottom_u > claw.U_max[0, 0]:
- #print "Bottom is too big. Was", new_unclipped_bottom_u, "changing top by", new_unclipped_bottom_u - claw.U_max[0, 0]
top_u -= new_unclipped_bottom_u - claw.U_max[0, 0]
new_unclipped_bottom_u = claw.U_max[0, 0]
if top_u > claw.U_max[1, 0]:
@@ -237,48 +418,90 @@
return numpy.matrix([[new_bottom_u - old_bottom_u], [new_top_u]])
-def main(argv):
- # Simulate the response of the system to a step input.
- #claw = ClawDeltaU()
- #simulated_x = []
- #for _ in xrange(100):
- # claw.Update(numpy.matrix([[12.0]]))
- # simulated_x.append(claw.X[0, 0])
+def run_test(claw, initial_X, goal, max_separation_error=0.01, show_graph=True, iterations=200):
+ """Runs the claw plant on a given claw (claw) with an initial condition (initial_X) and goal (goal).
- #pylab.plot(range(100), simulated_x)
- #pylab.show()
+ The tests themselves are not terribly sophisticated; I just test for
+ whether the goal has been reached and whether the separation goes
+ outside of the initial and goal values by more then max_separation_error.
+ Prints out something for a failure of either condition and returns
+ False if tests fail.
+ Args:
+ claw: claw object to use.
+ initial_X: starting state.
+ goal: goal state.
+ show_graph: Whether or not to display a graph showing the changing
+ states and voltages.
+ iterations: Number of timesteps to run the model for."""
- # Simulate the closed loop response of the system to a step input.
- claw = Claw("TopClaw")
+ claw.X = initial_X
+
+ # Various lists for graphing things.
t = []
- close_loop_x_bottom = []
- close_loop_x_sep = []
- close_loop_u_bottom = []
- close_loop_u_top = []
- R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
- claw.X[0, 0] = 0
- for i in xrange(100):
- #print "Error is", (R - claw.X_hat)
- U = claw.K * (R - claw.X_hat)
- #U = numpy.clip(claw.K * (R - claw.X_hat), claw.U_min, claw.U_max)
- #U = FullSeparationPriority(claw, U)
- U = AverageUFix(claw, U)
- #U = claw.K * (R - claw.X_hat)
- #U = ClipDeltaU(claw, U)
- claw.UpdateObserver(U)
- claw.Update(U)
- close_loop_x_bottom.append(claw.X[0, 0] * 10)
- close_loop_u_bottom.append(U[0, 0])
- close_loop_x_sep.append(claw.X[1, 0] * 10)
- close_loop_u_top.append(U[1, 0] + U[0, 0])
- t.append(0.01 * i)
+ x_bottom = []
+ x_top = []
+ u_bottom = []
+ u_top = []
+ x_separation = []
- pylab.plot(t, close_loop_x_bottom, label='x bottom')
- pylab.plot(t, close_loop_x_sep, label='separation')
- pylab.plot(t, close_loop_u_bottom, label='u bottom')
- pylab.plot(t, close_loop_u_top, label='u top')
- pylab.legend()
- pylab.show()
+ tests_passed = True
+
+ # Bounds which separation should not exceed.
+ lower_bound = (initial_X[1, 0] if initial_X[1, 0] < goal[1, 0]
+ else goal[1, 0]) - max_separation_error
+ upper_bound = (initial_X[1, 0] if initial_X[1, 0] > goal[1, 0]
+ else goal[1, 0]) + max_separation_error
+
+ for i in xrange(iterations):
+ U = claw.K * (goal - claw.X)
+ U = ScaleU(claw, U, claw.K, goal - claw.X)
+ claw.Update(U)
+
+ if claw.X[1, 0] > upper_bound or claw.X[1, 0] < lower_bound:
+ tests_passed = False
+ print "Claw separation was", claw.X[1, 0]
+ print "Should have been between", lower_bound, "and", upper_bound
+
+ t.append(i * claw.dt)
+ x_bottom.append(claw.X[0, 0] * 10.0)
+ x_top.append((claw.X[1, 0] + claw.X[0, 0]) * 10.0)
+ u_bottom.append(U[0, 0])
+ u_top.append(U[1, 0])
+ x_separation.append(claw.X[1, 0] * 10.0)
+
+ if show_graph:
+ pylab.plot(t, x_bottom, label='x bottom * 10')
+ pylab.plot(t, x_top, label='x top * 10')
+ pylab.plot(t, u_bottom, label='u bottom')
+ pylab.plot(t, u_top, label='u top')
+ pylab.plot(t, x_separation, label='separation * 10')
+ pylab.legend()
+ pylab.show()
+
+ # Test to make sure that we are near the goal.
+ if numpy.max(abs(claw.X - goal)) > 1e-4:
+ tests_passed = False
+ print "X was", claw.X, "Expected", goal
+
+ return tests_passed
+
+def main(argv):
+ claw = Claw()
+
+ # Test moving the claw with constant separation.
+ initial_X = numpy.matrix([[-1.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [0.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
+ # Test just changing separation.
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[0.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
+
+ # Test changing both separation and position at once..
+ initial_X = numpy.matrix([[0.0], [0.0], [0.0], [0.0]])
+ R = numpy.matrix([[1.0], [1.0], [0.0], [0.0]])
+ run_test(claw, initial_X, R)
# Write the generated constants out to a file.
if len(argv) != 3:
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index bda178e..a103c79 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -85,8 +85,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'
@@ -112,6 +114,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)
@@ -210,7 +216,7 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
- U = numpy.clip(U, self.U_min, self.U_max)
+ #U = numpy.clip(U, self.U_min, self.U_max)
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U