Added position control and profiling to drivetrain.
Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95
diff --git a/y2015/actors/drivetrain_actor.cc b/y2015/actors/drivetrain_actor.cc
index 9ef81c5..5ace60d 100644
--- a/y2015/actors/drivetrain_actor.cc
+++ b/y2015/actors/drivetrain_actor.cc
@@ -56,7 +56,7 @@
// They're more than 24V apart, so stop moving forwards and let it deal
// with spinning first.
profile.SetGoal(
- (status.filtered_left_position + status.filtered_right_position -
+ (status.estimated_left_position + status.estimated_right_position -
params.left_initial_position - params.right_initial_position) /
2.0);
} else {
@@ -138,10 +138,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain_queue.status->filtered_left_position -
+ control_loops::drivetrain_queue.status->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error = ::std::abs(
- control_loops::drivetrain_queue.status->filtered_right_position -
+ control_loops::drivetrain_queue.status->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain_queue.status->robot_speed);
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() {
diff --git a/y2015/control_loops/drivetrain/drivetrain.cc b/y2015/control_loops/drivetrain/drivetrain.cc
index bc3fa21..b5eb83a 100644
--- a/y2015/control_loops/drivetrain/drivetrain.cc
+++ b/y2015/control_loops/drivetrain/drivetrain.cc
@@ -752,11 +752,11 @@
// set the output status of the control loop state
if (status) {
status->robot_speed = dt_closedloop.GetEstimatedRobotSpeed();
- status->filtered_left_position = dt_closedloop.GetEstimatedLeftEncoder();
- status->filtered_right_position = dt_closedloop.GetEstimatedRightEncoder();
+ status->estimated_left_position = dt_closedloop.GetEstimatedLeftEncoder();
+ status->estimated_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->estimated_left_velocity = dt_closedloop.loop().X_hat(1, 0);
+ status->estimated_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/y2015/control_loops/drivetrain/drivetrain.q b/y2015/control_loops/drivetrain/drivetrain.q
index 824688c..3edd33f 100644
--- a/y2015/control_loops/drivetrain/drivetrain.q
+++ b/y2015/control_loops/drivetrain/drivetrain.q
@@ -97,13 +97,13 @@
// Estimated speed of the center of the robot in m/s (positive forwards).
double robot_speed;
// Estimated relative position of the left side in meters.
- double filtered_left_position;
+ double estimated_left_position;
// Estimated relative position of the right side in meters.
- double filtered_right_position;
+ double estimated_right_position;
// Estimated velocity of the left side in m/s.
- double filtered_left_velocity;
+ double estimated_left_velocity;
// Estimated velocity of the right side in m/s.
- double filtered_right_velocity;
+ double estimated_right_velocity;
// The voltage we wanted to send to the left side last cycle.
double uncapped_left_voltage;