From 0a0180779d773ed70c1475255d8ffa678282a1fe Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E6=85=95=E7=82=8E?= <29385962@qq.com>
Date: Mon, 21 Feb 2022 14:52:44 +0000
Subject: [PATCH] update arduino/Betas/RGB_V1.1.1/main/main.ino.

---
 arduino/Betas/RGB_V1.1.1/main/main.ino | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/arduino/Betas/RGB_V1.1.1/main/main.ino b/arduino/Betas/RGB_V1.1.1/main/main.ino
index c0d0888..120d851 100644
--- a/arduino/Betas/RGB_V1.1.1/main/main.ino
+++ b/arduino/Betas/RGB_V1.1.1/main/main.ino
@@ -701,13 +701,15 @@ float controllerLQR(float p_angle, float p_vel, float m_vel)
     last_unstable_time = millis();
     if (stable)	//如果是稳态进入非稳态则调整为目标角度
     {
-      target_angle = EEPROM.readFloat(0) - p_angle;
+      //target_angle = EEPROM.readFloat(0) - p_angle;
+      target_angle = EEPROM.readFloat(0);
       stable = 0;
     }
   }
   if ((millis() - last_unstable_time) > 500 && !stable)	//非稳态进入稳态超过500ms检测，更新目标角为目标角+摆角，假设进入稳态
   {
-    target_angle  -= _sign(target_velocity) * 0.4;
+    //target_angle  -= _sign(target_velocity) * 0.4;
+    target_angle = target_angle+p_angle;
     stable = 1;
   }