Use the current wheel angle when starting closed loop
Otherwise any error in the wheel angle causes the robot to jump when you
switch to closed loop mode.
Change-Id: I39155c8165ddfd580277b85fa5ca0268d51b896c
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index f9cc45d..248f1ce 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -64,8 +64,10 @@
if (drivetrain_queue.status.get()) {
if (is_control_loop_driving && !last_is_control_loop_driving_) {
- left_goal_ = drivetrain_queue.status->estimated_left_position;
- right_goal_ = drivetrain_queue.status->estimated_right_position;
+ left_goal_ = drivetrain_queue.status->estimated_left_position +
+ wheel * wheel_multiplier_;
+ right_goal_ = drivetrain_queue.status->estimated_right_position -
+ wheel * wheel_multiplier_;
}
}