Added position control and profiling to drivetrain.
Change-Id: I09ad01c46b23f1c7b6e8530bc10b03a733245b95
diff --git a/y2015_bot3/actors/drivetrain_actor.cc b/y2015_bot3/actors/drivetrain_actor.cc
index 0f9eedd..32ce93f 100644
--- a/y2015_bot3/actors/drivetrain_actor.cc
+++ b/y2015_bot3/actors/drivetrain_actor.cc
@@ -60,7 +60,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 {
@@ -142,10 +142,10 @@
const double kPositionThreshold = 0.05;
const double left_error = ::std::abs(
- drivetrain_queue.status->filtered_left_position -
+ drivetrain_queue.status->estimated_left_position -
(left_goal_state(0, 0) + params.left_initial_position));
const double right_error = ::std::abs(
- drivetrain_queue.status->filtered_right_position -
+ drivetrain_queue.status->estimated_right_position -
(right_goal_state(0, 0) + params.right_initial_position));
const double velocity_error =
::std::abs(drivetrain_queue.status->robot_speed);
diff --git a/y2015_bot3/autonomous/auto.cc b/y2015_bot3/autonomous/auto.cc
index 9dd2892..71a0efc 100644
--- a/y2015_bot3/autonomous/auto.cc
+++ b/y2015_bot3/autonomous/auto.cc
@@ -67,9 +67,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;
}
::std::unique_ptr< ::frc971::actors::DrivetrainAction> SetDriveGoal(
@@ -114,10 +114,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");
diff --git a/y2015_bot3/control_loops/drivetrain/drivetrain.cc b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
index 17cc97b..a3e7c24 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.cc
+++ b/y2015_bot3/control_loops/drivetrain/drivetrain.cc
@@ -739,11 +739,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_bot3/control_loops/drivetrain/drivetrain.q b/y2015_bot3/control_loops/drivetrain/drivetrain.q
index 7ff0498..6d432c4 100644
--- a/y2015_bot3/control_loops/drivetrain/drivetrain.q
+++ b/y2015_bot3/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;