Fixed profile error while turning in place.
Change-Id: Ib53fd1c7c9be0c422a4f2a656a832fd7d1193771
diff --git a/frc971/actors/drivetrain_actor.cc b/frc971/actors/drivetrain_actor.cc
index e7755f1..9b72cb1 100644
--- a/frc971/actors/drivetrain_actor.cc
+++ b/frc971/actors/drivetrain_actor.cc
@@ -56,7 +56,8 @@
// 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.filtered_left_position + status.filtered_right_position -
+ params.left_initial_position - params.right_initial_position) /
2.0);
} else {
static const double divisor = K(0, 0) + K(0, 2);
@@ -81,6 +82,7 @@
} else if (dx_left != 0 && dx_right != 0 &&
::aos::sign(dx_left) != ::aos::sign(dx_right)) {
// Both saturating in opposite directions. Don't do anything.
+ LOG(DEBUG, "Saturating opposite ways, not adjusting\n");
dx = 0;
} else if (::std::abs(dx_left) > ::std::abs(dx_right)) {
dx = dx_left;