From 31e601ebedc76b27f8b0313a65a1b4030754d89c Mon Sep 17 00:00:00 2001
From: Aleksandr <san.crot@yandex.ru>
Date: Tue, 2 Nov 2021 15:23:42 +0300
Subject: [PATCH] Fix race condition in trap traj control

---
 Firmware/MotorControl/controller.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp
index 4149b30d..546847ed 100644
--- a/Firmware/MotorControl/controller.cpp
+++ b/Firmware/MotorControl/controller.cpp
@@ -209,7 +209,7 @@ bool Controller::update() {
             if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {
                 // Drop into position control mode when done to avoid problems on loop counter delta overflow
                 config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
-                pos_setpoint_ = input_pos_;
+                pos_setpoint_ = axis_->trap_traj_.Xf_;
                 vel_setpoint_ = 0.0f;
                 torque_setpoint_ = 0.0f;
                 trajectory_done_ = true;