Log more debugging information in the drivetrain
This was helpful for understanding what the drivetrain was doing when
performing the Falcon 500 testing, and when run under replay allows us
to extract more detailed debugging information than we logged.
Change-Id: I457bc262dd39e006eb19d5e153fd2375b9656918
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 63caaab..51c6f03 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -322,6 +322,9 @@
dt_config_.AngularLinearToLeftRight(linear, angular);
const flatbuffers::Offset<CIMLogging> cim_logging_offset =
+ dt_openloop_.PopulateShiftingStatus(status->fbb());
+
+ const flatbuffers::Offset<PolyDriveLogging> poly_drive_logging_offset =
dt_openloop_.PopulateStatus(status->fbb());
flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
@@ -359,6 +362,7 @@
builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
builder.add_cim_logging(cim_logging_offset);
+ builder.add_poly_drive_logging(poly_drive_logging_offset);
builder.add_gear_logging(gear_logging_offset);
builder.add_line_follow_logging(line_follow_logging_offset);
builder.add_trajectory_logging(trajectory_logging_offset);
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 67134f5..1956abd 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -33,6 +33,17 @@
right_velocity:double;
}
+// Logging information for the polydrivetrain implementation.
+table PolyDriveLogging {
+ // Calculated velocity goals for the left/right sides of the drivetrain, in
+ // m/s.
+ goal_left_velocity:float;
+ goal_right_velocity:float;
+ // Feedforward components of the left/right voltages.
+ ff_left_voltage:float;
+ ff_right_voltage:float;
+}
+
enum PlanningState : byte {
NO_PLAN,
BUILDING_TRAJECTORY,
@@ -133,8 +144,12 @@
// Information about shifting logic and curent gear, for logging purposes
gear_logging:GearLogging;
cim_logging:CIMLogging;
+
trajectory_logging:TrajectoryLogging;
+
line_follow_logging:LineFollowLogging;
+
+ poly_drive_logging:PolyDriveLogging;
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/polydrivetrain.h b/frc971/control_loops/drivetrain/polydrivetrain.h
index 74fb2eb..7bfa630 100644
--- a/frc971/control_loops/drivetrain/polydrivetrain.h
+++ b/frc971/control_loops/drivetrain/polydrivetrain.h
@@ -55,7 +55,10 @@
void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
- flatbuffers::Offset<CIMLogging> PopulateStatus(
+ flatbuffers::Offset<PolyDriveLogging> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ flatbuffers::Offset<CIMLogging> PopulateShiftingStatus(
flatbuffers::FlatBufferBuilder *fbb);
// Computes the next state of a shifter given the current state and the
@@ -101,6 +104,9 @@
Scalar right_motor_speed_ = 0.0;
Scalar current_left_velocity_ = 0.0;
Scalar current_right_velocity_ = 0.0;
+
+ // Feedforward voltage, for logging.
+ Eigen::Matrix<Scalar, 2, 1> ff_volts_{0.0, 0.0};
};
template <typename Scalar>
@@ -353,15 +359,16 @@
CoerceGoal<Scalar>(R_poly_hv, equality_k, equality_w, loop_->R());
}
- const Eigen::Matrix<Scalar, 2, 1> FF_volts = FF * loop_->R();
+ ff_volts_ = FF * loop_->R();
const Eigen::Matrix<Scalar, 2, 1> U_ideal =
- loop_->controller().K() * (loop_->R() - loop_->X_hat()) + FF_volts;
+ loop_->controller().K() * (loop_->R() - loop_->X_hat()) + ff_volts_;
for (int i = 0; i < 2; i++) {
loop_->mutable_U()[i] = ::aos::Clip(U_ideal[i], -12, 12);
}
if (dt_config_.loop_type == LoopType::OPEN_LOOP) {
+ ff_volts_.setZero();
loop_->mutable_X_hat() =
loop_->plant().A() * loop_->X_hat() + loop_->plant().B() * loop_->U();
}
@@ -405,6 +412,7 @@
loop_->mutable_U(1, 0) = ::aos::Clip(
(R_right / dt_config_.v)(0, 0) + (IsInGear(right_gear_) ? 0 : wiggle),
-kTwelve, kTwelve);
+ ff_volts_ = loop_->U();
#ifdef __linux__
loop_->mutable_U() *= kTwelve / voltage_battery;
#else
@@ -425,7 +433,21 @@
}
template <typename Scalar>
-flatbuffers::Offset<CIMLogging> PolyDrivetrain<Scalar>::PopulateStatus(
+flatbuffers::Offset<PolyDriveLogging>
+PolyDrivetrain<Scalar>::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ PolyDriveLogging::Builder builder(*fbb);
+
+ builder.add_goal_left_velocity(goal_left_velocity_);
+ builder.add_goal_right_velocity(goal_right_velocity_);
+ builder.add_ff_left_voltage(ff_volts_(0, 0));
+ builder.add_ff_right_voltage(ff_volts_(1, 0));
+
+ return builder.Finish();
+}
+
+template <typename Scalar>
+flatbuffers::Offset<CIMLogging> PolyDrivetrain<Scalar>::PopulateShiftingStatus(
flatbuffers::FlatBufferBuilder *fbb) {
CIMLogging::Builder builder(*fbb);