Publish the x, y, theta state of the drivetrain
This gives us a start of an on-field position to follow.
Change-Id: Ibdd3f06e1856efd451ceb504801ed32605b5e560
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 7fb859d..2e78e51 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -145,8 +145,11 @@
double estimated_angular_velocity_error;
// The KF estimated heading.
double estimated_heading;
- // The KF wheel estimated heading.
- //double estimated_wheel_heading;
+
+ // xytheta of the robot.
+ double x;
+ double y;
+ double theta;
// True if the output voltage was capped last cycle.
bool output_was_capped;