Added position control and profiling to drivetrain.
Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95
diff --git a/y2015/autonomous/auto.cc b/y2015/autonomous/auto.cc
index 4e8aa59..4ab5a41 100644
--- a/y2015/autonomous/auto.cc
+++ b/y2015/autonomous/auto.cc
@@ -117,10 +117,10 @@
control_loops::drivetrain_queue.status.FetchAnother();
double left_error = ::std::abs(
left_initial_position -
- control_loops::drivetrain_queue.status->filtered_left_position);
+ control_loops::drivetrain_queue.status->estimated_left_position);
double right_error = ::std::abs(
right_initial_position -
- control_loops::drivetrain_queue.status->filtered_right_position);
+ control_loops::drivetrain_queue.status->estimated_right_position);
const double kPositionThreshold = 0.05 + distance;
if (right_error < kPositionThreshold && left_error < kPositionThreshold) {
LOG(INFO, "At the goal\n");
@@ -234,9 +234,9 @@
void InitializeEncoders() {
control_loops::drivetrain_queue.status.FetchAnother();
left_initial_position =
- control_loops::drivetrain_queue.status->filtered_left_position;
+ control_loops::drivetrain_queue.status->estimated_left_position;
right_initial_position =
- control_loops::drivetrain_queue.status->filtered_right_position;
+ control_loops::drivetrain_queue.status->estimated_right_position;
}
void WaitForClawZero() {