From 5a0c3ef803e0d3253cf0bc3b421ac6e2d4f3a98a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E4=BA=95=E6=98=8E=E6=B1=9F?= <246462502@qq.com>
Date: Tue, 9 Jul 2024 19:32:36 +0800
Subject: [PATCH] =?UTF-8?q?fear:=20=E5=A2=9E=E5=8A=A0=E5=BC=82=E5=B8=B8?=
 =?UTF-8?q?=E5=81=9C=E8=BD=A6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/jj_motion.c | 35 ++++++++++++++++++++++++-----------
 app/jj_motion.h |  1 +
 app/main.c      |  1 +
 3 files changed, 26 insertions(+), 11 deletions(-)

diff --git a/app/jj_motion.c b/app/jj_motion.c
index c1dfba8..f85e94e 100644
--- a/app/jj_motion.c
+++ b/app/jj_motion.c
@@ -83,7 +83,7 @@ float set_pos = 0.0f;
 
 float in_speed;
 float out_speed;
-
+float last_angle;
 float set_speed;
 float set_speed0   = 657.0f;
 float set_speed1   = 1111.0f;
@@ -98,6 +98,7 @@ int cnt5           = 0;
 int cnt6           = 0;
 int cnt7           = 0;
 int cnt8           = 0;
+int cnt9           = 0;
 int if_start       = 0;
 float shock_stop   = 0;
 uint8_t in_state   = 0;
@@ -141,34 +142,46 @@ float sport_get_speed(void)
 
 void sport_motion()
 {
-
     imu660ra_get_gyro();
     imu660ra_get_acc();
     get_vol();
     in_gyro = imu660ra_gyro_z;
-    // 抖动停车
-    if (fabs(out_gyro) < 700) {
-        PID_SetOutputLimits(&speed_pid, -0.0f, 2800.0f);
+
+    if (fabs(out_gyro) < 900) {
+        PID_SetOutputLimits(&speed_pid, -0.0f, 3000.0f);
     } else {
         PID_SetOutputLimits(&speed_pid, -0.0f, 2100.0f);
     }
+    ////////////////////////////////////////停车任务///////////////////////////////////////////////
+    // 抖动停车
     if (imu660ra_acc_z <= 800) {
         cnt7++;
     } else {
         cnt7 = 0;
     }
-    if (cnt7 >= 100 && shock_stop == 1) {
+    if (cnt7 >= 50 && shock_stop == 1) {
         bt_fly_flag = bt_run_flag = 0;
-        bt_printf("ting");
+        bt_printf("up");
+    }
+    // 異常值停车
+    if (fabs(in_angle - last_angle) > 45.f) {
+        bt_fly_flag = bt_run_flag = 0;
+        bt_printf("bug");
     }
     // 斑马线停车,摄像头识别
     if (1 == in_stop && if_start == 1) {
         stop_flag = 1;
     }
     if (stop_flag == 1) {
-
+        bt_printf("finish");
         bt_fly_flag = bt_run_flag = 0;
     }
+    // 出界停车
+    if (2 == in_stop) {
+        bt_printf("over");
+        bt_fly_flag = bt_run_flag = 0;
+    }
+    ///////////////////////////////////////////////////////////////////////////////////////
     /* 动力风扇设置 */
     if (1 == bt_run_flag) {
         cnt1++;
@@ -234,8 +247,8 @@ void sport_motion()
             cnt5 = 0;
         }
         // 并级pid，限幅
-        pwm_duty_ls = (int32_t)myclip_f(250 - out_pos, 50.0f, 500.f);
-        pwm_duty_rs = (int32_t)myclip_f(250 + out_pos, 50.0f, 500.f);
+        pwm_duty_ls = (int32_t)myclip_f(320 - out_pos, 140.0f, 500.f);
+        pwm_duty_rs = (int32_t)myclip_f(320 + out_pos, 140.0f, 500.f);
         pwm_duty_lb = (int32_t)myclip_f(out_speed + out_gyro, 300.0f, 3000.f);
         pwm_duty_rb = (int32_t)myclip_f(out_speed - out_gyro, 300.0f, 3000.f);
 
@@ -276,7 +289,7 @@ void sport_pid_init()
     PID(&pos_pid, &in_angle, &out_pos, &set_pos, po_Kp1, po_Ki1, po_Kd1, _PID_P_ON_E, _PID_CD_REVERSE);
     PID_SetMode(&pos_pid, _PID_MODE_AUTOMATIC);
     PID_SetSampleTime(&pos_pid, 20);
-    PID_SetOutputLimits(&pos_pid, -250.0f, 250.0f);
+    PID_SetOutputLimits(&pos_pid, -180.0f, 180.0f);
 
     /* 角速度控制 */
     PID(&far_gyro_pid, &in_gyro, &out_gyro, &out_angle, gy_Kp1, gy_Ki1, gy_Kd1, _PID_P_ON_E, _PID_CD_REVERSE); // m是增量
diff --git a/app/jj_motion.h b/app/jj_motion.h
index 2d86754..5c0fb95 100644
--- a/app/jj_motion.h
+++ b/app/jj_motion.h
@@ -58,6 +58,7 @@ extern float set_speed3;
 extern float set_speed;
 extern float set_speed4;
 extern float shock_stop;
+extern float last_angle;
 extern int32_t pwm_duty_ls;
 extern int32_t pwm_duty_rs;
 extern int32_t pwm_duty_lb;
diff --git a/app/main.c b/app/main.c
index e33cfce..2280a53 100644
--- a/app/main.c
+++ b/app/main.c
@@ -71,6 +71,7 @@ int main(void)
         Page_Run();
         by_frame_parse(&test_data[0].u32);
         jj_bt_run();
+        last_angle=in_angle;
         in_pos   = 1000.f * test_data[1].f32;
         in_angle = test_data[0].f32;
         in_state = test_data[2].u8[0];