Add more status information to spline following
Change-Id: I2cafd5b35107a64789473c4c5a6b307d95f51dea
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 5c21744..9d02bb2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -52,6 +52,14 @@
PLANNED,
}
+table SplineState {
+ x:double (id: 0);
+ y:double (id: 1);
+ theta:double (id: 2);
+ left_velocity:double (id: 3);
+ right_velocity:double (id: 4);
+}
+
// For logging information about the state of the trajectory planning.
table TrajectoryLogging {
// state of planning the trajectory.
@@ -82,6 +90,12 @@
// Splines that we have full plans for.
available_splines:[int] (id: 12);
+
+ state_error:SplineState (id: 14);
+ left_voltage_components:SplineState (id: 15);
+ right_voltage_components:SplineState (id: 16);
+ left_ff_voltage:double (id: 17);
+ right_ff_voltage:double (id: 18);
}
enum RobotSide : ubyte {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index f90e7e0..dbabfb1 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -175,6 +175,7 @@
Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+ last_U_components_ = K * Eigen::DiagonalMatrix<double, 5>(state_error);
if (backwards) {
Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
@@ -186,6 +187,8 @@
next_U_ = U_ff + U_fb - voltage_error;
uncapped_U_ = next_U_;
ScaleCapU(&next_U_);
+ last_state_error_ = state_error;
+ last_U_ff_ = U_ff;
}
}
@@ -215,6 +218,20 @@
}
}
+namespace {
+template <typename Matrix>
+flatbuffers::Offset<SplineState> MakeSplineState(
+ const Matrix &state, flatbuffers::FlatBufferBuilder *fbb) {
+ SplineState::Builder builder(*fbb);
+ builder.add_x(state(0));
+ builder.add_y(state(1));
+ builder.add_theta(::aos::math::NormalizeAngle(state(2)));
+ builder.add_left_velocity(state(3));
+ builder.add_right_velocity(state(4));
+ return builder.Finish();
+}
+} // namespace
+
flatbuffers::Offset<TrajectoryLogging> SplineDrivetrain::MakeTrajectoryLogging(
flatbuffers::FlatBufferBuilder *builder) const {
int *spline_handles;
@@ -225,6 +242,13 @@
spline_handles[ii] = trajectories_[ii].spline_handle();
}
+ const flatbuffers::Offset<SplineState> state_error_offset =
+ MakeSplineState(last_state_error_, builder);
+ const flatbuffers::Offset<SplineState> left_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(0), builder);
+ const flatbuffers::Offset<SplineState> right_voltage_components_offset =
+ MakeSplineState(last_U_components_.row(1), builder);
+
drivetrain::TrajectoryLogging::Builder trajectory_logging_builder(*builder);
if (executing_spline_) {
::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
@@ -241,6 +265,13 @@
trajectory_logging_builder.add_left_velocity(goal_state(3));
trajectory_logging_builder.add_right_velocity(goal_state(4));
}
+ trajectory_logging_builder.add_state_error(state_error_offset);
+ trajectory_logging_builder.add_left_voltage_components(
+ left_voltage_components_offset);
+ trajectory_logging_builder.add_right_voltage_components(
+ right_voltage_components_offset);
+ trajectory_logging_builder.add_left_ff_voltage(last_U_ff_(0));
+ trajectory_logging_builder.add_right_ff_voltage(last_U_ff_(1));
}
trajectory_logging_builder.add_is_executing(!IsAtEnd() && executing_spline_);
trajectory_logging_builder.add_is_executed(executing_spline_ && IsAtEnd());
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index f02a5b6..24fb66c 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -105,6 +105,9 @@
// Information used for status message.
::Eigen::Matrix<double, 2, 1> uncapped_U_;
+ ::Eigen::Matrix<double, 5, 1> last_state_error_;
+ ::Eigen::Matrix<double, 2, 5> last_U_components_;
+ ::Eigen::Matrix<double, 2, 1> last_U_ff_;
bool enable_ = false;
bool output_was_capped_ = false;
};