添加灰度传感器索引宏和右转90度位置环子任务

- GrayscaleSensor: 添加 GS_L4~GS_R4 枚举索引、GS_IS_ON_BLACK 阈值宏
- 提取 yaw_correct() 工具函数，消除角度修正重复代码
- 位置环新增 T型路口检测和自动右转90度子任务

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; // 存储小车相对于黑线的位置

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);

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)
 {
-    //角度速率（单位度/秒）
+    //检测到需要右转90度
-    angle_velocity = PIDCalculate((PIDInstance *)&pid_position,grayscale_sensor_position,0);
+    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)
+    yaw_correct(&set_value);
-    {
-        while(set_value > 180)
-            set_value -= 360;
-    }
-    else if(set_value<-180)
-    {
-        while(set_value < -180)
-            set_value += 360;
-    }
     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;
+    }
 }