Add status data for line follow drivetrain
Change-Id: I6eb64c2896b73e443ffb7f8a11370494ef54debf
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 66e9c32..74be414 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -57,6 +57,18 @@
float right_velocity;
};
+// For logging state of the line follower.
+struct LineFollowLogging {
+ // Whether we are currently freezing target choice.
+ bool frozen;
+ // Whether we currently have a target.
+ bool have_target;
+ // Absolute position of the current goal.
+ float x;
+ float y;
+ float theta;
+};
+
queue_group DrivetrainQueue {
implements aos.control_loops.ControlLoop;
@@ -184,6 +196,7 @@
GearLogging gear_logging;
CIMLogging cim_logging;
TrajectoryLogging trajectory_logging;
+ LineFollowLogging line_follow_logging;
};
queue Goal goal;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 5216916..b00ab3f 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -732,6 +732,15 @@
goal.Send();
RunForTime(chrono::seconds(5));
+
+ my_drivetrain_queue_.status.FetchLatest();
+ EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.frozen);
+ EXPECT_TRUE(my_drivetrain_queue_.status->line_follow_logging.have_target);
+ EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.x);
+ EXPECT_EQ(1.0, my_drivetrain_queue_.status->line_follow_logging.y);
+ EXPECT_FLOAT_EQ(M_PI_4,
+ my_drivetrain_queue_.status->line_follow_logging.theta);
+
// Should have run off the end of the target, running along the y=x line.
EXPECT_LT(1.0, drivetrain_motor_plant_.GetPosition().x());
EXPECT_NEAR(drivetrain_motor_plant_.GetPosition().x(),
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index c4ca5ef..d085d17 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -233,6 +233,15 @@
U_ *= (maxU > kMaxVoltage) ? kMaxVoltage / maxU : 1.0;
}
+void LineFollowDrivetrain::PopulateStatus(
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+ status->line_follow_logging.frozen = freeze_target_;
+ status->line_follow_logging.have_target = have_target_;
+ status->line_follow_logging.x = target_pose_.abs_pos().x();
+ status->line_follow_logging.y = target_pose_.abs_pos().y();
+ status->line_follow_logging.theta = target_pose_.abs_theta();
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 83fba20..de9ff5a 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -41,9 +41,8 @@
// over.
bool SetOutput(
::frc971::control_loops::DrivetrainQueue::Output *output);
- // TODO(james): Populate.
void PopulateStatus(
- ::frc971::control_loops::DrivetrainQueue::Status * /*status*/) const {}
+ ::frc971::control_loops::DrivetrainQueue::Status *status) const;
private:
// Nominal max voltage.