From 49b0dc637cf3f8908aa23bdf8fa5a34f91c9e407 Mon Sep 17 00:00:00 2001
From: SkyLi <SkyLiA7Mr@gmail.com>
Date: Sun, 7 Jun 2026 17:09:03 +0800
Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=81=B0=E5=BA=A6=E4=BC=A0?=
 =?UTF-8?q?=E6=84=9F=E5=99=A8=E7=B4=A2=E5=BC=95=E5=AE=8F=E5=92=8C=E5=8F=B3?=
 =?UTF-8?q?=E8=BD=AC90=E5=BA=A6=E4=BD=8D=E7=BD=AE=E7=8E=AF=E5=AD=90?=
 =?UTF-8?q?=E4=BB=BB=E5=8A=A1?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- GrayscaleSensor: 添加 GS_L4~GS_R4 枚举索引、GS_IS_ON_BLACK 阈值宏
- 提取 yaw_correct() 工具函数，消除角度修正重复代码
- 位置环新增 T型路口检测和自动右转90度子任务
---
 project/code/GrayscaleSensor.c |  2 +-
 project/code/GrayscaleSensor.h | 10 +++++
 project/code/Task.c            | 68 +++++++++++++++++++++++++++-------
 3 files changed, 66 insertions(+), 14 deletions(-)

diff --git a/project/code/GrayscaleSensor.c b/project/code/GrayscaleSensor.c
index 6dd7189..bc89a89 100644
--- a/project/code/GrayscaleSensor.c
+++ b/project/code/GrayscaleSensor.c
@@ -13,7 +13,7 @@ volatile static uint16 adc_white[8] = {4096,4096,4096,4096,4096,4096,4096,4096};
 volatile static uint16 adc_black[8] = {0,0,0,0,0,0,0,0}; // 存储8路灰度传感器的黑色参考值，初始值为0
 volatile static uint16 adc_range[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的动态范围，初始值为满量程
 volatile static uint16 adc_raw_value[8] = {0}; // 存储8路灰度传感器的原始 ADC 值
-static float adc_normalized[8] = {0}; // 存储8路灰度传感器的归一化值，范围 [0, 1]
+volatile float adc_normalized[8] = {0}; // 存储8路灰度传感器的归一化值，范围 [0, 1]
 static float adc_normalized_sum = 0; // 归一化值的总和，用于计算位置
 volatile float grayscale_sensor_position_prev = 0; // 存储小车相对于黑线的位置
 volatile float grayscale_sensor_position = 0; // 存储小车相对于黑线的位置
diff --git a/project/code/GrayscaleSensor.h b/project/code/GrayscaleSensor.h
index 5f4c743..1f5707e 100644
--- a/project/code/GrayscaleSensor.h
+++ b/project/code/GrayscaleSensor.h
@@ -3,8 +3,18 @@
 
 #include "zf_common_typedef.h"
 
+enum {
+GS_L4=0,GS_L3,GS_L2,GS_L1,GS_R1,GS_R2,GS_R3,GS_R4
+};
+
 extern volatile float grayscale_sensor_position; // 存储小车相对于黑线的位置
 
+extern volatile float adc_normalized[8];
+
+//模拟量转数字量的阈值
+#define GS_THRESHOLD 0.5
+#define GS_IS_ON_BLACK(x) (adc_normalized[x]>GS_THRESHOLD)
+
 void grayscale_sensor_init(void);
 void grayscale_sensor_scan(void);
 void grayscale_sensor_set_white(void);
diff --git a/project/code/Task.c b/project/code/Task.c
index d9819b9..c452ccb 100644
--- a/project/code/Task.c
+++ b/project/code/Task.c
@@ -36,6 +36,9 @@ void position_control_init(float T);
 void angle_control_task(void);
 void position_control_task(void);
 void motor_control_task(void);
+
+/*Yaw角工具*/
+void yaw_correct(float* yaw);
 void Task_10ms(uint32 state, void *ptr)
 {
     // 10ms 执行一次的任务代码(100Hz)
@@ -135,25 +138,64 @@ void position_control_init(float T)
     };
     PIDInit((PIDInstance *)&pid_position, &config_position);
 }
+/*位置环子任务 BEGIN*/
+//右转90度，按需开启，开启后自动检测条件结束
+volatile static uint8_t position_turn_right_90_runing = 0;
+volatile static float position_turn_target_yaw = 0;
+#define check_turn_right_90() (GS_IS_ON_BLACK(GS_R4) && GS_IS_ON_BLACK(GS_R3) && GS_IS_ON_BLACK(GS_R2))
+#define POSITION_TURN_ANGLE_VELOCITY 37
+#define POSITION_TURN_ANGLE_INCREASE 85
+/*位置环子任务 END*/
 void position_control_task(void)
 {
-    //角度速率（单位度/秒）
-    angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
-
+    //检测到需要右转90度
+    if(!position_turn_right_90_runing && check_turn_right_90())
+    {
+        //开启右转90度标志位
+        position_turn_right_90_runing=1;
+        //记录要到达的角度
+        position_turn_target_yaw = imu_angle.yaw + POSITION_TURN_ANGLE_INCREASE;
+        yaw_correct((float *)&position_turn_target_yaw);
+    }
+    //如果右转90度标志位启用，接管角度速率，固定速度右转
+    if(position_turn_right_90_runing)
+    {
+        //检测角度，如果完成90度，则取消右转90度标志位
+        float sub = position_turn_target_yaw - imu_angle.yaw;
+        yaw_correct(&sub);
+        if(sub<=0)
+        {
+            position_turn_right_90_runing=0;
+            //取消后立即进行一次计算
+            angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
+        }else{
+            //接管角度速率
+            angle_velocity=POSITION_TURN_ANGLE_VELOCITY;
+        }
+    }
+    else{
+        //角度速率（单位度/秒）
+        angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
+    }
     //发送给云台，角度速率前馈
     command_send_angle_velocity((float*)&angle_velocity);
+
 }
 void set_yaw_ref(float set_value)
 {
-    if(set_value>180)
-    {
-        while(set_value > 180)
-            set_value -= 360;
-    }
-    else if(set_value<-180)
-    {
-        while(set_value < -180)
-            set_value += 360;
-    }
+    yaw_correct(&set_value);
     yaw_ref = set_value;
+}
+void yaw_correct(float* yaw)
+{
+    if(*yaw>180)
+    {
+        while(*yaw > 180)
+            *yaw -= 360;
+    }
+    else if(*yaw<-180)
+    {
+        while(*yaw < -180)
+            *yaw += 360;
+    }
 }
\ No newline at end of file