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

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

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;
+    }
 }