Added filtered velocity to the output and removed unnecesary comment after gyro fix.
Change-Id: I604610b9d0558de86543cfc249d03e54a7319206
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b348bf8..08cf99e 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -166,7 +166,6 @@
// Decay the offset quickly because this gyro is great.
const double offset =
(right - left - gyro * constants::GetValues().turn_width) / 2.0;
- // TODO(brians): filtered_offset_ = offset first time around.
filtered_offset_ = 0.25 * offset + 0.75 * filtered_offset_;
gyro_ = gyro;
SetRawPosition(left, right);
@@ -756,6 +755,9 @@
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+
+ status->filtered_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->filtered_right_velocity = dt_closedloop.loop().X_hat(3, 0);
status->output_was_capped = dt_closedloop.OutputWasCapped();
status->uncapped_left_voltage = dt_closedloop.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop.loop().U_uncapped(1, 0);
diff --git a/frc971/control_loops/drivetrain/drivetrain.q b/frc971/control_loops/drivetrain/drivetrain.q
index 5f34e92..942c5b3 100644
--- a/frc971/control_loops/drivetrain/drivetrain.q
+++ b/frc971/control_loops/drivetrain/drivetrain.q
@@ -52,6 +52,8 @@
double robot_speed;
double filtered_left_position;
double filtered_right_position;
+ double filtered_left_velocity;
+ double filtered_right_velocity;
double uncapped_left_voltage;
double uncapped_right_voltage;