From d42f9d402f02ad4417a526f9e5b7d03ab9e1558f Mon Sep 17 00:00:00 2001
From: 45coll <674148718@qq.com>
Date: Tue, 12 Oct 2021 16:28:45 +0800
Subject: [PATCH] =?UTF-8?q?=E6=96=B0=E6=9D=BF=E6=B5=8B=E8=AF=95=E7=A8=8B?=
 =?UTF-8?q?=E5=BA=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 arduino/main/main.ino | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/arduino/main/main.ino b/arduino/main/main.ino
index d047be6..9cf8c77 100644
--- a/arduino/main/main.ino
+++ b/arduino/main/main.ino
@@ -12,7 +12,7 @@ Deng's FOC 闭环速度控制例程 测试库：SimpleFOC 2.1.1 测试硬件：
 #include <AsyncUDP.h> //引用以使用异步UDP
 #include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
 Kalman kalmanZ;
-#define gyroZ_OFF -0.52
+#define gyroZ_OFF -0.19
 #define swing_up_voltage 5 //V
 #define balance_voltage 10   //V
 /* ----IMU Data---- */
@@ -68,7 +68,7 @@ BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 //命令设置
 Command comm;
 double target_velocity = 0;
-double target_angle = 91;
+double target_angle = 89.86;
 double target_voltage = 0;
 void do_K1(char* cmd) { comm.scalar(&LQR_K1, cmd); }
 
@@ -183,7 +183,8 @@ void setup() {
   Serial.println(F("Motor ready."));
   Serial.println(F("Set the target velocity using serial terminal:"));
 
-
+//  pinMode(22,OUTPUT);
+//  digitalWrite(22,LOW);
 }
 char buf[255];
 int t_v;
@@ -229,7 +230,7 @@ void loop() {
    }
     else // else do swing-up
     {    // sets 1.5V to the motor in order to swing up
-        target_voltage = -_sign(gyroZrate) * 3;
+        target_voltage = _sign(gyroZrate) * 3;
     }
 
     // set the target voltage to the motor
@@ -266,8 +267,9 @@ void loop() {
     }
       
 #endif
-#if 0
+#if 1
 
+//Serial.print(gyroZangle);Serial.print("\t");
 Serial.print(kalAngleZ);Serial.print("\t");
 
   Serial.print(target_voltage);Serial.print("\t");