From 3b5ff6d51c205d810ddc1fec2c62fd5d0a3b6770 Mon Sep 17 00:00:00 2001
From: zrg <674148718@qq.com>
Date: Sat, 18 Dec 2021 19:52:13 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=AA=8C=E8=AF=81bug?=
 =?UTF-8?q?=EF=BC=8Carduino=E7=A8=8B=E5=BA=8F=E6=9E=81=E5=AF=B9=E6=95=B0?=
 =?UTF-8?q?=E8=BE=93=E5=85=A5=E9=94=99=E8=AF=AF=E7=9A=84=E9=97=AE=E9=A2=98?=
 =?UTF-8?q?=EF=BC=8C=E4=BC=9A=E9=80=A0=E6=88=90=E7=94=B5=E6=9C=BA=E6=97=A0?=
 =?UTF-8?q?=E6=B3=95=E8=BF=90=E8=A1=8C?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 arduino/main/main.ino | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

diff --git a/arduino/main/main.ino b/arduino/main/main.ino
index 1df3ad1..8d9684c 100644
--- a/arduino/main/main.ino
+++ b/arduino/main/main.ino
@@ -67,7 +67,7 @@ float LQR_K4_2 = 1.5;   //
 float LQR_K4_3 = 1.42; //
 
 //电机参数
-BLDCMotor motor = BLDCMotor(7);
+BLDCMotor motor = BLDCMotor(5);
 BLDCDriver3PWM driver = BLDCDriver3PWM(32, 33, 25, 22);
 float target_velocity = 0;
 float target_angle = 89.3;
@@ -279,10 +279,11 @@ if(isnan(nan))
   Serial.println(F("Motor ready."));
   Serial.println(F("Set the target velocity using serial terminal:"));
 
-
+//digitalWrite(22,LOW);
 }
 char buf[255];
 long loop_count = 0;
+double last_pitch;
 void loop() {
     motor.loopFOC();
   if (1)
@@ -303,8 +304,11 @@ void loop() {
     
     double pitch = acc2rotation(accX, accY);
     double gyroZrate = gyroZ / 131.0; // Convert to deg/s
-
+    if(abs(pitch-last_pitch)>100)
+      kalmanZ.setAngle(pitch);
+    
     kalAngleZ = kalmanZ.getAngle(pitch, gyroZrate + gyroZ_OFF, dt);
+    last_pitch = pitch;
     gyroZangle += (gyroZrate + gyroZ_OFF) * dt;
     compAngleZ = 0.93 * (compAngleZ + (gyroZrate + gyroZ_OFF) * dt) + 0.07 * pitch;
 
@@ -357,14 +361,15 @@ if (abs(pendulum_angle) < swing_up_angle) // if angle small enough stabilize 0.5
 #if 1
 
 //Serial.print(gyroZangle);Serial.print("\t");
+Serial.print(pitch);Serial.print("\t");
 Serial.print(kalAngleZ);Serial.print("\t");
 
-  Serial.print(target_voltage);Serial.print("\t");
-//  Serial.print(target_velocity);Serial.print("\t");
-  Serial.print(motor.shaft_velocity);Serial.print("\t");
-  Serial.print(target_angle);Serial.print("\t");
-  Serial.print(pendulum_angle);Serial.print("\t");
-  Serial.print(gyroZrate);Serial.print("\t");
+//  Serial.print(target_voltage);Serial.print("\t");
+////  Serial.print(target_velocity);Serial.print("\t");
+//  Serial.print(motor.shaft_velocity);Serial.print("\t");
+//  Serial.print(target_angle);Serial.print("\t");
+//  Serial.print(pendulum_angle);Serial.print("\t");
+//  Serial.print(gyroZrate);Serial.print("\t");
   Serial.print("\r\n");
 #endif
 //  motor.move(target_velocity);