Added position control and profiling to drivetrain.
Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95
diff --git a/y2014_bot3/actions/drivetrain_action.cc b/y2014_bot3/actions/drivetrain_action.cc
index 93b3d46..bef5bd6 100644
--- a/y2014_bot3/actions/drivetrain_action.cc
+++ b/y2014_bot3/actions/drivetrain_action.cc
@@ -55,7 +55,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) /
2.0);
} else {
static const double divisor = K(0, 0) + K(0, 2);
@@ -133,10 +133,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- control_loops::drivetrain.status->filtered_left_position -
+ control_loops::drivetrain.status->estimated_left_position -
(left_goal_state(0, 0) + action_q_->goal->left_initial_position));
const double right_error = ::std::abs(
- control_loops::drivetrain.status->filtered_right_position -
+ control_loops::drivetrain.status->estimated_right_position -
(right_goal_state(0, 0) + action_q_->goal->right_initial_position));
const double velocity_error =
::std::abs(control_loops::drivetrain.status->robot_speed);
diff --git a/y2014_bot3/autonomous/auto.cc b/y2014_bot3/autonomous/auto.cc
index bf5a3cb..3724cbe 100644
--- a/y2014_bot3/autonomous/auto.cc
+++ b/y2014_bot3/autonomous/auto.cc
@@ -49,9 +49,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 HandleAuto() {
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain.cc b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
index e9f425d..429338f 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.cc
@@ -556,11 +556,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/y2014_bot3/control_loops/drivetrain/drivetrain.q b/y2014_bot3/control_loops/drivetrain/drivetrain.q
index f5f53bb..1b97f81 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain.q
@@ -79,13 +79,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;