add Encoder and PID control and Abstacle control

Core/Src/app_threadx.c

@@ -59,6 +59,8 @@
 
 TX_EVENT_FLAGS_GROUP system_events;
 TX_EVENT_FLAGS_GROUP sensor_events;
+TX_EVENT_FLAGS_GROUP response_events; // 用于避障事件实现
+
 MotorCommand current_motor_cmd = {0,0};
 _GPSData gps_data;
 
@@ -95,6 +97,13 @@ TX_QUEUE im948_uart_rx_queue;
 TX_THREAD gps_task;
 ULONG gps_task_stack[GPS_TASK_STACK_SIZE / sizeof(ULONG)];
 
+static TX_THREAD obstacle_thread;
+static UCHAR obstacle_stack[512];
+//超声波
+static UCHAR ultrasonic_stack[512];
+static void obstacle_thread_entry(ULONG);
+TX_THREAD ultrasonic_task_thread;
+
 /* USER CODE END 1 */
 /* USER CODE END 1 */
 
@@ -182,7 +191,40 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
 	{
 		return status;
 	}
-    HCBle_SendData("✅ BLE RX/TX 线程和队列初始化完成\r\n");
+	
+	// obstacle_thread create
+	status = tx_thread_create(&obstacle_thread,
+							"obstacle",
+							obstacle_thread_entry,
+							0,obstacle_stack,sizeof(obstacle_stack),8,8,TX_NO_TIME_SLICE,TX_AUTO_START);
+	
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	 status = tx_thread_create(&ultrasonic_task_thread,
+                     "Ultrasonic",
+                     ultrasonic_task_entry,
+                     0,
+                     ultrasonic_stack,
+                     sizeof(ultrasonic_stack),
+                     7, 7,    // 这里的优先级
+                     TX_NO_TIME_SLICE,
+                     TX_AUTO_START);
+	//
+//    HCBle_SendData("✅ BLE RX/TX 线程和队列初始化完成\r\n");
+					 
+	status = ControlThreadCreate();
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}			 
+	status = Encoder_ThreadCreate();	
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}	
 	
     return TX_SUCCESS;
 }
@@ -206,6 +248,48 @@ void MX_ThreadX_Init(void)
 }
 
 /* USER CODE BEGIN 1 */
-
+//新加入的 obstacle 
+static void obstacle_thread_entry(ULONG arg)
+{
+	while(1)
+	{
+		ULONG evt; // 这个应该是用来接收数据的
+		tx_event_flags_get(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR_CLEAR,&evt,TX_WAIT_FOREVER);
+		
+		switch(obstacle_level)
+		{
+			case 1: // 远
+				Buzzer_Open();
+				Shake_Motor_Open();
+				tx_thread_sleep(30);
+				Buzzer_Close();
+				Shake_Motor_Close();
+				break;
+			
+			case 2:
+				for(int i = 0; i < 2;i++)
+				{
+					Buzzer_Open();
+					Shake_Motor_Open();
+					tx_thread_sleep(50);
+					Buzzer_Close();
+                    Shake_Motor_Close();
+                    tx_thread_sleep(30);
+				}
+				break;
+				
+			case 3: // 近
+				Buzzer_Open();
+				Shake_Motor_Open();
+				tx_thread_sleep(150);
+				Buzzer_Close();
+				Shake_Motor_Close();
+				break;
+			default:
+				break;
+		}
+		tx_thread_sleep(20);
+	}
+}
 
 /* USER CODE END 1 */