Fixed auto so that it goes straight.
diff --git a/frc971/actions/drivetrain_action.cc b/frc971/actions/drivetrain_action.cc
index 50baa1f..8a4557a 100644
--- a/frc971/actions/drivetrain_action.cc
+++ b/frc971/actions/drivetrain_action.cc
@@ -50,6 +50,34 @@
.right_velocity_goal(profile_goal_state(1, 0))
.Send();
}
+ control_loops::drivetrain.status.FetchLatest();
+ while (!control_loops::drivetrain.status.get()) {
+ LOG(WARNING,
+ "No previous drivetrain status packet, trying to fetch again\n");
+ control_loops::drivetrain.status.FetchNextBlocking();
+ if (ShouldCancel()) return;
+ }
+ while (true) {
+ if (ShouldCancel()) return;
+ const double kPositionThreshold = 0.05;
+
+ const double left_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_left_position -
+ (profile_goal_state(0, 0) + action_q_->goal->left_initial_position));
+ const double right_error = ::std::abs(
+ control_loops::drivetrain.status->filtered_right_position -
+ (profile_goal_state(0, 0) + action_q_->goal->right_initial_position));
+ const double velocity_error =
+ ::std::abs(control_loops::drivetrain.status->robot_speed);
+ if (left_error < kPositionThreshold && right_error < kPositionThreshold &&
+ velocity_error < 0.2) {
+ break;
+ } else {
+ LOG(DEBUG, "Drivetrain error is %f, %f, %f\n", left_error, right_error,
+ velocity_error);
+ }
+ control_loops::drivetrain.status.FetchNextBlocking();
+ }
LOG(INFO, "Done moving\n");
}
diff --git a/frc971/actions/drivetrain_action.q b/frc971/actions/drivetrain_action.q
index 5797378..f61996f 100644
--- a/frc971/actions/drivetrain_action.q
+++ b/frc971/actions/drivetrain_action.q
@@ -6,9 +6,7 @@
implements frc971.actions.ActionQueueGroup;
message Goal {
- // If true, run this action. If false, cancel the action if it is
- // currently running.
- bool run;
+ uint32_t run;
double left_initial_position;
double right_initial_position;
double y_offset;