Final

AutoGuideStick.ioc

@@ -34,10 +34,11 @@ Mcu.IP10=TIM1
 Mcu.IP11=TIM2
 Mcu.IP12=TIM3
 Mcu.IP13=TIM4
-Mcu.IP14=TIM8
+Mcu.IP14=TIM5
-Mcu.IP15=UART4
+Mcu.IP15=TIM8
-Mcu.IP16=USART2
+Mcu.IP16=UART4
-Mcu.IP17=USART3
+Mcu.IP17=USART2
+Mcu.IP18=USART3
 Mcu.IP2=DEBUG
 Mcu.IP3=GPDMA1
 Mcu.IP4=MEMORYMAP
@@ -46,15 +47,15 @@ Mcu.IP6=PWR
 Mcu.IP7=RCC
 Mcu.IP8=SYS
 Mcu.IP9=THREADX
-Mcu.IPNb=18
+Mcu.IPNb=19
 Mcu.Name=STM32H563ZITx
 Mcu.Package=LQFP144
 Mcu.Pin0=PH0-OSC_IN(PH0)
 Mcu.Pin1=PH1-OSC_OUT(PH1)
-Mcu.Pin10=PG3
+Mcu.Pin10=PG2
-Mcu.Pin11=PC6
+Mcu.Pin11=PG3
-Mcu.Pin12=PC7
+Mcu.Pin12=PC6
-Mcu.Pin13=PC8
+Mcu.Pin13=PC7
 Mcu.Pin14=PC9
 Mcu.Pin15=PA13(JTMS/SWDIO)
 Mcu.Pin16=PA14(JTCK/SWCLK)
@@ -72,21 +73,22 @@ Mcu.Pin26=VP_GPDMA1_VS_GPDMACH4
 Mcu.Pin27=VP_GPDMA1_VS_GPDMACH5
 Mcu.Pin28=VP_PWR_VS_SECSignals
 Mcu.Pin29=VP_PWR_VS_LPOM
-Mcu.Pin3=PB1
+Mcu.Pin3=PA0
 Mcu.Pin30=VP_SYS_VS_tim6
 Mcu.Pin31=VP_THREADX_VS_RTOSJjThreadXJjCoreJjDefault
 Mcu.Pin32=VP_TIM2_VS_ClockSourceINT
 Mcu.Pin33=VP_TIM3_VS_ClockSourceINT
 Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
-Mcu.Pin35=VP_BOOTPATH_VS_BOOTPATH
+Mcu.Pin35=VP_TIM5_VS_ClockSourceINT
-Mcu.Pin36=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin36=VP_BOOTPATH_VS_BOOTPATH
-Mcu.Pin4=PG0
+Mcu.Pin37=VP_MEMORYMAP_VS_MEMORYMAP
-Mcu.Pin5=PG1
+Mcu.Pin4=PB1
-Mcu.Pin6=PE9
+Mcu.Pin5=PG0
-Mcu.Pin7=PE11
+Mcu.Pin6=PG1
-Mcu.Pin8=PB10
+Mcu.Pin7=PE9
-Mcu.Pin9=PG2
+Mcu.Pin8=PE11
-Mcu.PinsNb=37
+Mcu.Pin9=PB10
+Mcu.PinsNb=38
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32H563ZITx
@@ -110,6 +112,7 @@ NVIC.SavedSystickIrqHandlerGenerated=true
 NVIC.SysTick_IRQn=true\:14\:0\:false\:false\:false\:false\:false\:true\:false
 NVIC.TIM2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.TIM5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.TIM6_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
 NVIC.TimeBase=TIM6_IRQn
 NVIC.TimeBaseIP=TIM6
@@ -117,6 +120,9 @@ NVIC.UART4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.USART3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
+PA0.GPIOParameters=GPIO_Label
+PA0.GPIO_Label=HC_Echo
+PA0.Signal=S_TIM5_CH1
 PA13(JTMS/SWDIO).Mode=Serial_Wire
 PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO
 PA14(JTCK/SWCLK).Mode=Serial_Wire
@@ -146,18 +152,16 @@ PC2.GPIOParameters=GPIO_Label
 PC2.GPIO_Label=PWMA
 PC2.Locked=true
 PC2.Signal=S_TIM4_CH4
-PC6.GPIOParameters=GPIO_Label
+PC6.GPIOParameters=GPIO_PuPd,GPIO_Label
 PC6.GPIO_Label=E2A
+PC6.GPIO_PuPd=GPIO_PULLUP
 PC6.Locked=true
 PC6.Signal=S_TIM8_CH1
-PC7.GPIOParameters=GPIO_Label
+PC7.GPIOParameters=GPIO_PuPd,GPIO_Label
 PC7.GPIO_Label=E2B
+PC7.GPIO_PuPd=GPIO_PULLUP
 PC7.Locked=true
 PC7.Signal=S_TIM8_CH2
-PC8.GPIOParameters=GPIO_Label
-PC8.GPIO_Label=HC_Echo
-PC8.Locked=true
-PC8.Signal=S_TIM3_CH3
 PC9.GPIOParameters=GPIO_PuPd,GPIO_Label
 PC9.GPIO_Label=HC_Trig
 PC9.GPIO_PuPd=GPIO_NOPULL
@@ -174,12 +178,14 @@ PD5.Signal=USART2_TX
 PD6.Locked=true
 PD6.Mode=Asynchronous
 PD6.Signal=USART2_RX
-PE11.GPIOParameters=GPIO_Label
+PE11.GPIOParameters=GPIO_PuPd,GPIO_Label
 PE11.GPIO_Label=E1B
+PE11.GPIO_PuPd=GPIO_PULLUP
 PE11.Locked=true
 PE11.Signal=S_TIM1_CH2
-PE9.GPIOParameters=GPIO_Label
+PE9.GPIOParameters=GPIO_PuPd,GPIO_Label
 PE9.GPIO_Label=E1A
+PE9.GPIO_PuPd=GPIO_PULLUP
 PE9.Locked=true
 PE9.Signal=S_TIM1_CH1
 PG0.GPIOParameters=GPIO_Label
@@ -324,12 +330,12 @@ SH.S_TIM1_CH1.0=TIM1_CH1,Encoder_Interface
 SH.S_TIM1_CH1.ConfNb=1
 SH.S_TIM1_CH2.0=TIM1_CH2,Encoder_Interface
 SH.S_TIM1_CH2.ConfNb=1
-SH.S_TIM3_CH3.0=TIM3_CH3,Input_Capture3_from_TI3
-SH.S_TIM3_CH3.ConfNb=1
 SH.S_TIM3_CH4.0=TIM3_CH4,PWM Generation4 CH4
 SH.S_TIM3_CH4.ConfNb=1
 SH.S_TIM4_CH4.0=TIM4_CH4,PWM Generation4 CH4
 SH.S_TIM4_CH4.ConfNb=1
+SH.S_TIM5_CH1.0=TIM5_CH1,Input_Capture1_from_TI1
+SH.S_TIM5_CH1.ConfNb=1
 SH.S_TIM8_CH1.0=TIM8_CH1,Encoder_Interface
 SH.S_TIM8_CH1.ConfNb=1
 SH.S_TIM8_CH2.0=TIM8_CH2,Encoder_Interface
@@ -337,17 +343,23 @@ SH.S_TIM8_CH2.ConfNb=1
 THREADX.IPParameters=TX_APP_GENERATE_INIT_CODE,TX_MINIMUM_STACK
 THREADX.TX_APP_GENERATE_INIT_CODE=false
 THREADX.TX_MINIMUM_STACK=400
+TIM1.EncoderMode=TIM_ENCODERMODE_TI12
+TIM1.IPParameters=EncoderMode
 TIM2.IPParameters=Prescaler
 TIM2.Prescaler=250 - 1
-TIM3.Channel-Input_Capture3_from_TI3=TIM_CHANNEL_3
 TIM3.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
-TIM3.IPParameters=Prescaler,Channel-PWM Generation4 CH4,Channel-Input_Capture3_from_TI3,PeriodNoDither
+TIM3.IPParameters=Prescaler,Channel-PWM Generation4 CH4,PeriodNoDither
 TIM3.PeriodNoDither=255
 TIM3.Prescaler=48
 TIM4.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
 TIM4.IPParameters=Channel-PWM Generation4 CH4,Prescaler,PeriodNoDither
 TIM4.PeriodNoDither=255
 TIM4.Prescaler=48
+TIM5.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
+TIM5.IPParameters=Channel-Input_Capture1_from_TI1,Prescaler
+TIM5.Prescaler=250-1
+TIM8.EncoderMode=TIM_ENCODERMODE_TI12
+TIM8.IPParameters=EncoderMode
 UART4.BaudRate=9600
 UART4.IPParameters=BaudRate
 USART2.BaudRate=9600
@@ -382,4 +394,6 @@ VP_TIM3_VS_ClockSourceINT.Mode=Internal
 VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
 VP_TIM4_VS_ClockSourceINT.Mode=Internal
 VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
+VP_TIM5_VS_ClockSourceINT.Mode=Internal
+VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
 board=custom

Core/Inc/app_threadx.h

@@ -58,6 +58,7 @@ extern "C" {
 extern TX_QUEUE ble_tx_queue;
 extern TX_EVENT_FLAGS_GROUP system_events;
 extern TX_EVENT_FLAGS_GROUP sensor_events; //传感器事件组
+extern TX_EVENT_FLAGS_GROUP response_events; //避障
 //typedef struct
 //{
 ////	uint32_t msg_type;  // 应该使用 int --- 4字节

Core/Inc/main.h

@@ -59,6 +59,8 @@ void Error_Handler(void);
 /* Private defines -----------------------------------------------------------*/
 #define PWMA_Pin GPIO_PIN_2
 #define PWMA_GPIO_Port GPIOC
+#define HC_Echo_Pin GPIO_PIN_0
+#define HC_Echo_GPIO_Port GPIOA
 #define PWMB_Pin GPIO_PIN_1
 #define PWMB_GPIO_Port GPIOB
 #define AIN1_Pin GPIO_PIN_0
@@ -77,8 +79,6 @@ void Error_Handler(void);
 #define E2A_GPIO_Port GPIOC
 #define E2B_Pin GPIO_PIN_7
 #define E2B_GPIO_Port GPIOC
-#define HC_Echo_Pin GPIO_PIN_8
-#define HC_Echo_GPIO_Port GPIOC
 #define HC_Trig_Pin GPIO_PIN_9
 #define HC_Trig_GPIO_Port GPIOC
 #define Shake_Motor_Pin GPIO_PIN_12

Core/Inc/stm32h5xx_it.h

@@ -57,6 +57,7 @@ void GPDMA1_Channel4_IRQHandler(void);
 void GPDMA1_Channel5_IRQHandler(void);
 void TIM2_IRQHandler(void);
 void TIM3_IRQHandler(void);
+void TIM5_IRQHandler(void);
 void TIM6_IRQHandler(void);
 void USART2_IRQHandler(void);
 void USART3_IRQHandler(void);

Core/Inc/tim.h

@@ -40,6 +40,8 @@ extern TIM_HandleTypeDef htim3;
 
 extern TIM_HandleTypeDef htim4;
 
+extern TIM_HandleTypeDef htim5;
+
 extern TIM_HandleTypeDef htim8;
 
 /* USER CODE BEGIN Private defines */
@@ -50,6 +52,7 @@ void MX_TIM1_Init(void);
 void MX_TIM2_Init(void);
 void MX_TIM3_Init(void);
 void MX_TIM4_Init(void);
+void MX_TIM5_Init(void);
 void MX_TIM8_Init(void);
 
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);

Core/Src/app_threadx.c

@@ -36,7 +36,7 @@
 
 // BLE define 
 #define BLE_RX_THREAD_STACK_SIZE 2048
-#define BLE_RX_THREAD_PRIORITY    10
+#define BLE_RX_THREAD_PRIORITY    9
 #define BLE_TX_THREAD_STACK_SIZE 2048
 #define BLE_TX_THREAD_PRIORITY    10
 // IMU thread config
@@ -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,46 @@ 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);
+		
+		if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	//
+//    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 +254,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 */

Core/Src/gpio.c

@@ -46,10 +46,10 @@ void MX_GPIO_Init(void)
   /* GPIO Ports Clock Enable */
   __HAL_RCC_GPIOH_CLK_ENABLE();
   __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOB_CLK_ENABLE();
   __HAL_RCC_GPIOG_CLK_ENABLE();
   __HAL_RCC_GPIOE_CLK_ENABLE();
-  __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOD_CLK_ENABLE();
 
   /*Configure GPIO pin Output Level */

Core/Src/main.c

@@ -100,10 +100,15 @@ int main(void)
   MX_UART4_Init();
   MX_TIM1_Init();
   MX_TIM8_Init();
+  MX_TIM5_Init();
   /* USER CODE BEGIN 2 */
 
    imu600_init();
    GPS_Init();
+   DWT_Init();
+   PWM_GPIO_TIM_Init();
+   HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
+//   				Buzzer_Open();
 //	HCBle_InitDMAReception();
 //	HAL_Delay(200);
 //	GPS_Init();

Core/Src/stm32h5xx_it.c

@@ -58,6 +58,7 @@
 /* External variables --------------------------------------------------------*/
 extern TIM_HandleTypeDef htim2;
 extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim5;
 extern DMA_NodeTypeDef Node_GPDMA1_Channel5;
 extern DMA_QListTypeDef List_GPDMA1_Channel5;
 extern DMA_HandleTypeDef handle_GPDMA1_Channel5;
@@ -242,6 +243,20 @@ void TIM3_IRQHandler(void)
   /* USER CODE END TIM3_IRQn 1 */
 }
 
+/**
+  * @brief This function handles TIM5 global interrupt.
+  */
+void TIM5_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM5_IRQn 0 */
+
+  /* USER CODE END TIM5_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim5);
+  /* USER CODE BEGIN TIM5_IRQn 1 */
+
+  /* USER CODE END TIM5_IRQn 1 */
+}
+
 /**
   * @brief This function handles TIM6 global interrupt.
   */

Core/Src/tim.c

@@ -28,6 +28,7 @@ TIM_HandleTypeDef htim1;
 TIM_HandleTypeDef htim2;
 TIM_HandleTypeDef htim3;
 TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim5;
 TIM_HandleTypeDef htim8;
 
 /* TIM1 init function */
@@ -51,7 +52,7 @@ void MX_TIM1_Init(void)
   htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim1.Init.RepetitionCounter = 0;
   htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
@@ -126,7 +127,6 @@ void MX_TIM3_Init(void)
 
   TIM_ClockConfigTypeDef sClockSourceConfig = {0};
   TIM_MasterConfigTypeDef sMasterConfig = {0};
-  TIM_IC_InitTypeDef sConfigIC = {0};
   TIM_OC_InitTypeDef sConfigOC = {0};
 
   /* USER CODE BEGIN TIM3_Init 1 */
@@ -147,10 +147,6 @@ void MX_TIM3_Init(void)
   {
     Error_Handler();
   }
-  if (HAL_TIM_IC_Init(&htim3) != HAL_OK)
-  {
-    Error_Handler();
-  }
   if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
   {
     Error_Handler();
@@ -161,14 +157,6 @@ void MX_TIM3_Init(void)
   {
     Error_Handler();
   }
-  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
-  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
-  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
-  sConfigIC.ICFilter = 0;
-  if (HAL_TIM_IC_ConfigChannel(&htim3, &sConfigIC, TIM_CHANNEL_3) != HAL_OK)
-  {
-    Error_Handler();
-  }
   sConfigOC.OCMode = TIM_OCMODE_PWM1;
   sConfigOC.Pulse = 0;
   sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
@@ -236,6 +224,59 @@ void MX_TIM4_Init(void)
   /* USER CODE END TIM4_Init 2 */
   HAL_TIM_MspPostInit(&htim4);
 
+}
+/* TIM5 init function */
+void MX_TIM5_Init(void)
+{
+
+  /* USER CODE BEGIN TIM5_Init 0 */
+
+  /* USER CODE END TIM5_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_IC_InitTypeDef sConfigIC = {0};
+
+  /* USER CODE BEGIN TIM5_Init 1 */
+
+  /* USER CODE END TIM5_Init 1 */
+  htim5.Instance = TIM5;
+  htim5.Init.Prescaler = 250-1;
+  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim5.Init.Period = 4294967295;
+  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_IC_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
+  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
+  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
+  sConfigIC.ICFilter = 0;
+  if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM5_Init 2 */
+
+  /* USER CODE END TIM5_Init 2 */
+
 }
 /* TIM8 init function */
 void MX_TIM8_Init(void)
@@ -258,7 +299,7 @@ void MX_TIM8_Init(void)
   htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
   htim8.Init.RepetitionCounter = 0;
   htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  sConfig.EncoderMode = TIM_ENCODERMODE_TI1;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
   sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
   sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
   sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
@@ -303,7 +344,7 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
     */
     GPIO_InitStruct.Pin = E1A_Pin|E1B_Pin;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
     GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
     HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
@@ -327,7 +368,7 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
     */
     GPIO_InitStruct.Pin = E2A_Pin|E2B_Pin;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
     GPIO_InitStruct.Alternate = GPIO_AF3_TIM8;
     HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
@@ -365,17 +406,6 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
     /* TIM3 clock enable */
     __HAL_RCC_TIM3_CLK_ENABLE();
 
-    __HAL_RCC_GPIOC_CLK_ENABLE();
-    /**TIM3 GPIO Configuration
-    PC8     ------> TIM3_CH3
-    */
-    GPIO_InitStruct.Pin = HC_Echo_Pin;
-    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
-    HAL_GPIO_Init(HC_Echo_GPIO_Port, &GPIO_InitStruct);
-
     /* TIM3 interrupt Init */
     HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(TIM3_IRQn);
@@ -394,6 +424,32 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM4_MspInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspInit 0 */
+
+  /* USER CODE END TIM5_MspInit 0 */
+    /* TIM5 clock enable */
+    __HAL_RCC_TIM5_CLK_ENABLE();
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM5 GPIO Configuration
+    PA0     ------> TIM5_CH1
+    */
+    GPIO_InitStruct.Pin = HC_Echo_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
+    HAL_GPIO_Init(HC_Echo_GPIO_Port, &GPIO_InitStruct);
+
+    /* TIM5 interrupt Init */
+    HAL_NVIC_SetPriority(TIM5_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM5_IRQn);
+  /* USER CODE BEGIN TIM5_MspInit 1 */
+
+  /* USER CODE END TIM5_MspInit 1 */
+  }
 }
 void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
 {
@@ -509,14 +565,6 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
     /* Peripheral clock disable */
     __HAL_RCC_TIM3_CLK_DISABLE();
 
-    /**TIM3 GPIO Configuration
-    PB1     ------> TIM3_CH4
-    PC8     ------> TIM3_CH3
-    */
-    HAL_GPIO_DeInit(PWMB_GPIO_Port, PWMB_Pin);
-
-    HAL_GPIO_DeInit(HC_Echo_GPIO_Port, HC_Echo_Pin);
-
     /* TIM3 interrupt Deinit */
     HAL_NVIC_DisableIRQ(TIM3_IRQn);
   /* USER CODE BEGIN TIM3_MspDeInit 1 */
@@ -534,6 +582,25 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
 
   /* USER CODE END TIM4_MspDeInit 1 */
   }
+  else if(tim_baseHandle->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspDeInit 0 */
+
+  /* USER CODE END TIM5_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM5_CLK_DISABLE();
+
+    /**TIM5 GPIO Configuration
+    PA0     ------> TIM5_CH1
+    */
+    HAL_GPIO_DeInit(HC_Echo_GPIO_Port, HC_Echo_Pin);
+
+    /* TIM5 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM5_IRQn);
+  /* USER CODE BEGIN TIM5_MspDeInit 1 */
+
+  /* USER CODE END TIM5_MspDeInit 1 */
+  }
 }
 
 /* USER CODE BEGIN 1 */

MDK-ARM/AutoGuideStick.uvoptx

@@ -148,7 +148,24 @@
           <Name>-U004200403132511238363431 -O2254 -SF10000 -C0 -A1 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(6BA02477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC1000 -FN1 -FF0STM32H5xx_2M_0800.FLM -FS08000000 -FL0200000 -FP0($$Device:STM32H563ZITx$CMSIS\Flash\STM32H5xx_2M_0800.FLM) -WA0 -WE0 -WVCE4 -WS2710 -WM0 -WP2 -WK0</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint/>
+      <Breakpoint>
+        <Bp>
+          <Number>0</Number>
+          <Type>0</Type>
+          <LineNumber>110</LineNumber>
+          <EnabledFlag>1</EnabledFlag>
+          <Address>134265834</Address>
+          <ByteObject>0</ByteObject>
+          <HtxType>0</HtxType>
+          <ManyObjects>0</ManyObjects>
+          <SizeOfObject>0</SizeOfObject>
+          <BreakByAccess>0</BreakByAccess>
+          <BreakIfRCount>1</BreakIfRCount>
+          <Filename>..\fun\Ultrasound.c</Filename>
+          <ExecCommand></ExecCommand>
+          <Expression>\\AutoGuideStick\../fun/Ultrasound.c\110</Expression>
+        </Bp>
+      </Breakpoint>
       <WatchWindow1>
         <Ww>
           <count>0</count>
@@ -267,6 +284,16 @@
           <WinNumber>2</WinNumber>
           <ItemText>msg</ItemText>
         </Ww>
+        <Ww>
+          <count>7</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>status</ItemText>
+        </Ww>
+        <Ww>
+          <count>8</count>
+          <WinNumber>2</WinNumber>
+          <ItemText>distance_cm</ItemText>
+        </Ww>
       </WatchWindow2>
       <Tracepoint>
         <THDelay>0</THDelay>
@@ -2930,6 +2957,30 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>214</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\fun\encoder.c</PathWithFileName>
+      <FilenameWithoutPath>encoder.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>7</GroupNumber>
+      <FileNumber>215</FileNumber>
+      <FileType>5</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\fun\encoder.h</PathWithFileName>
+      <FilenameWithoutPath>encoder.h</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>

MDK-ARM/AutoGuideStick.uvprojx

@@ -1734,6 +1734,16 @@
               <FileType>5</FileType>
               <FilePath>..\fun\value.h</FilePath>
             </File>
+            <File>
+              <FileName>encoder.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\encoder.c</FilePath>
+            </File>
+            <File>
+              <FileName>encoder.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\encoder.h</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

MDK-ARM/AutoGuideStick/AutoGuideStick.build_log.htm

@@ -22,21 +22,12 @@ Dialog DLL:      TCM.DLL V1.56.4.0
  
 <h2>Project:</h2>
 D:\advance_stick\AutoGuideStick\MDK-ARM\AutoGuideStick.uvprojx
-Project File Date:  07/01/2025
+Project File Date:  07/02/2025
 
 <h2>Output:</h2>
 *** Using Compiler 'V6.21', folder: 'D:\keil5\ARM\ARMCLANG\Bin'
 Build target 'AutoGuideStick'
-compiling imu948.c...
+"AutoGuideStick\AutoGuideStick.axf" - 0 Error(s), 0 Warning(s).
-../fun/HCBle.c(138): warning: illegal character encoding in string literal [-Winvalid-source-encoding]
-  138 |     HCBle_SendData("? <BD><E2><CE><F6>ʧ<B0><DC>: %s\r\n", frame);
-      |                       ^~~~~~~~~~~~~~~~ ~~~~~~~~
-1 warning generated.
-compiling HCBle.c...
-linking...
-Program Size: Code=83396 RO-data=1364 RW-data=20 ZI-data=15268  
-FromELF: creating hex file...
-"AutoGuideStick\AutoGuideStick.axf" - 0 Error(s), 1 Warning(s).
 
 <h2>Software Packages used:</h2>
 
@@ -60,7 +51,7 @@ Package Vendor: Keil
 
    * Component: ARM::CMSIS:CORE@5.6.0
       Include file:  CMSIS/Core/Include/tz_context.h
-Build Time Elapsed:  00:00:01
+Build Time Elapsed:  00:00:02
 </pre>
 </body>
 </html>

MDK-ARM/AutoGuideStick/AutoGuideStick.lnp

@@ -202,6 +202,7 @@
 "autoguidestick\imu.o"
 "autoguidestick\motor.o"
 "autoguidestick\imu948.o"
+"autoguidestick\encoder.o"
 --library_type=microlib --strict --scatter "AutoGuideStick\AutoGuideStick.sct"
 --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
 --info sizes --info totals --info unused --info veneers

MDK-ARM/AutoGuideStick/AutoGuideStick_AutoGuideStick.dep

@@ -1,7 +1,7 @@
 Dependencies for Project 'AutoGuideStick', Target 'AutoGuideStick': (DO NOT MODIFY !)
 CompilerVersion: 6210000::V6.21::ARMCLANG
-F (startup_stm32h563xx.s)(0x68634847)(--target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -masm=auto  -Wa,armasm,--diag_suppress=A1950W -c
+F (startup_stm32h563xx.s)(0x68654635)(--target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -masm=auto  -Wa,armasm,--diag_suppress=A1950W -c
-
+
 -gdwarf-4 -Wa,armasm,--pd,"__MICROLIB SETA 1" -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include
 
 -I./RTE/_AutoGuideStick
@@ -42,9 +42,10 @@ I (..\Core\Inc\gpdma.ha.\Core\Inc\tim.h)(0x00000000)
 I (..\Drivers\CMSIS\Include\core_cm33.h)(0x683FA4DE)
 I (D:\keil5\ARM\ARMCLANG\include\stdint.h)(0x6569B012)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h)(0x683FA4DE)
-I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h)(0x683FA4DE)
+I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h)(0x683FA4DE)
+I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h)(0x683FA4DE)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h)(0x683FA4DE)
-I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h)(0x683FA4DE)
+I (..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h)(0x683FA4DE)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h)(0x683FA4DD)
 I (D:\keil5\ARM\ARMCLANG\include\stddef.h)(0x6569B012)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h)(0x683FA4DD)
@@ -141,8 +142,8 @@ I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h)(0x683FA4DD)
 -ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
 
 -D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
-
+
--o autoguidestick/gpdma.o -MD)
+-o autoguidestick/gpdma.o -MD)
 I (..\Core\Inc\main.hc.\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h)(0x00000000)
 I (..\Core\Inc\stm32h5xx_hal_conf.h)(0x685C2A98)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h)(0x683FA4DD)
@@ -183,8 +184,9 @@ I (..\Core\Inc\gpdma.ha.\Core\Inc\tim.h)(0x00000000)
 -I./RTE/_AutoGuideStick
 
 -ID:/keil5/ARM/PACK/ARM/CMSIS/5.9.0/CMSIS/Core/Include
-
+
--ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
+-ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
+
 -D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
 
 -o autoguidestick/memorymap.o -MD)
@@ -216,7 +218,7 @@ I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h)(0x683FA4DD)
-I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h)(0x683FA4DD)
+I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h)(0x683FA4DD)
@@ -248,7 +250,7 @@ I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h)(0x683FA4DD)
 I (D:\keil5\ARM\ARMCLANG\include\math.h)(0x6569B012)
 I (..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h)(0x683FA4DE)
 I (..\Drivers\CMSIS\Include\core_cm33.h)(0x683FA4DE)
-I (..\Drivers\CMSIS\Include\core_cm33.h)(0x683FA4DE)
+I (D:\keil5\ARM\ARMCLANG\include\stdint.h)(0x6569B012)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h)(0x683FA4DE)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h)(0x683FA4DE)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h)(0x683FA4DE)
@@ -290,7 +292,8 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 
 -ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
 
--D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
+-D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
+
 -o autoguidestick/tim.o -MD)
 I (..\Core\Inc\main.h..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h)(0x00000000)
 I (..\Core\Inc\stm32h5xx_hal_conf.h)(0x685C2A98)
@@ -397,8 +400,8 @@ I (..\Core\Inc\gpdma.ha.\Core\Inc\tim.h)(0x00000000)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h)(0x683FA4DE)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h)(0x683FA4DE)
 I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h)(0x683FA4DE)
-I (D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h)(0x683FA4DE)
+I (..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h)(0x683FA4DE)
-I (..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h)(0x683FA4DE)
+I (..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h)(0x683FA4DD)
 I (D:\keil5\ARM\ARMCLANG\include\stddef.h)(0x6569B012)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h)(0x683FA4DD)
 I (..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h)(0x683FA4DD)
@@ -2074,7 +2077,7 @@ I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
 
 -o autoguidestick/tx_event_flags_info_get.o -MD)
 I (..\Middlewares\ST\threadx\common\inc\tx_api.h)(0x683FA4DA)
-I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
+I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
 I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
 I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
@@ -2115,10 +2118,11 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 
 -o autoguidestick/tx_event_flags_set.o -MD)
 I (..\Middlewares\ST\threadx\common\inc\tx_api.h)(0x683FA4DA)
-I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
+I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
-I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
+I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
-I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
-I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
 I (..\Middlewares\ST\threadx\common\inc\tx_event_flags.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_event_flags_set_notify.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
 
@@ -2159,8 +2163,9 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_api.h)(0x683FA4DA)
 I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
 I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
-I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
+I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
-I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_mutex_create.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
 
 -fno-rtti -funsigned-char -fshort-enums -fshort-wchar
@@ -2203,7 +2208,7 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
 I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
-I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
 I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_mutex_get.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
 
@@ -2247,9 +2252,9 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
 I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
-I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_mutex_initialize.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
-I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
+
 -fno-rtti -funsigned-char -fshort-enums -fshort-wchar
 
 -D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
@@ -2291,9 +2296,9 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
 I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_mutex_priority_change.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
-I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
+
-F (../Middlewares/ST/threadx/common/src/tx_mutex_priority_change.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
-
+
 -D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
 
 -I./RTE/_AutoGuideStick
@@ -2334,9 +2339,9 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
 I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_queue_cleanup.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
-I (..\Middlewares\ST\threadx\common\inc\tx_mutex.h)(0x683FA4DA)
+
-F (../Middlewares/ST/threadx/common/src/tx_queue_cleanup.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
-
+
 -D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
 
 -I./RTE/_AutoGuideStick
@@ -2377,9 +2382,9 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_queue.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_queue_delete.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
 
-F (../Middlewares/ST/threadx/common/src/tx_queue_delete.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
-
+
--fno-rtti -funsigned-char -fshort-enums -fshort-wchar
+-D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
 
 -I./RTE/_AutoGuideStick
 
@@ -2421,6 +2426,50 @@ I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
 I (..\Middlewares\ST\threadx\common\inc\tx_queue.h)(0x683FA4DA)
 F (../Middlewares/ST/threadx/common/src/tx_queue_front_send.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
 
-F (../Middlewares/ST/threadx/common/src/tx_queue_front_send.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
-
+
--fno-rtti -funsigned-char -fshort-enums -fshort-wchar
+-D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
+
+-I./RTE/_AutoGuideStick
+
+-ID:/keil5/ARM/PACK/ARM/CMSIS/5.9.0/CMSIS/Core/Include
+
+-ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
+
+-D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
+
+-o autoguidestick/tx_queue_front_send.o -MD)
+I (..\Middlewares\ST\threadx\common\inc\tx_api.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
+I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
+I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_thread.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_queue.h)(0x683FA4DA)
+F (../Middlewares/ST/threadx/common/src/tx_queue_info_get.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
+
+-D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
+
+-I./RTE/_AutoGuideStick
+
+-ID:/keil5/ARM/PACK/ARM/CMSIS/5.9.0/CMSIS/Core/Include
+
+-ID:/keil5/ARM/PACK/Keil/STM32H5xx_DFP/1.3.0/Drivers/CMSIS/Device/ST/STM32H5xx/Include
+
+-D__UVISION_VERSION="539" -D_RTE_ -DSTM32H563xx -D_RTE_ -DTX_INCLUDE_USER_DEFINE_FILE -DTX_SINGLE_MODE_NON_SECURE="1" -DUSE_HAL_DRIVER -DSTM32H563xx
+
+-o autoguidestick/tx_queue_info_get.o -MD)
+I (..\Middlewares\ST\threadx\common\inc\tx_api.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h)(0x683FA4D9)
+I (..\Core\Inc\tx_user.he:\keil5\ARM\ARMCLANG\include\stdlib.h)(0x00000000)
+I (D:\keil5\ARM\ARMCLANG\include\string.h)(0x6569B012)
+I (..\Middlewares\ST\threadx\common\inc\tx_trace.h)(0x683FA4DA)
+I (..\Middlewares\ST\threadx\common\inc\tx_queue.h)(0x683FA4DA)
+F (../Middlewares/ST/threadx/common/src/tx_queue_initialize.c)(0x683FA4DA)(-xc -std=c11 --target=arm-arm-none-eabi -mcpu=cortex-m33 -mfpu=fpv5-sp-d16 -mfloat-abi=hard -c
+
+-fno-rtti -funsigned-char -fshort-enums -fshort-wchar
+
+-D__MICROLIB -gdwarf-4 -O0 -ffunction-sections -Wno-packed -Wno-missing-variable-declarations -Wno-missing-prototypes -Wno-missing-noreturn -Wno-sign-conversion -Wno-nonportable-include-path -Wno-reserved-id-macro -Wno-unused-macros -Wno-documentation-unknown-command -Wno-documentation -Wno-license-management -Wno-parentheses-equality -Wno-reserved-identifier -I ../Core/Inc -I ../AZURE_RTOS/App -I ../Drivers/STM32H5xx_HAL_Driver/Inc -I ../Drivers/STM32H5xx_HAL_Driver/Inc/Legacy -I ../Middlewares/ST/threadx/common/inc -I ../Drivers/CMSIS/Device/ST/STM32H5xx/Include -I ../Middlewares/ST/threadx/ports/cortex_m33/ac6/inc -I ../Drivers/CMSIS/Include -I ../fun
+

MDK-ARM/AutoGuideStick/app_azure_rtos.d

@@ -40,5 +40,5 @@ autoguidestick/app_azure_rtos.o: ..\AZURE_RTOS\App\app_azure_rtos.c \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
-  ..\AZURE_RTOS\App\app_azure_rtos_config.h
+  ..\fun\value.h ..\AZURE_RTOS\App\app_azure_rtos_config.h

MDK-ARM/AutoGuideStick/app_threadx.d

@@ -40,4 +40,5 @@ autoguidestick/app_threadx.o: ..\Core\Src\app_threadx.c \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h

MDK-ARM/AutoGuideStick/buzzer.d

@@ -40,4 +40,4 @@ autoguidestick/buzzer.o: ..\fun\Buzzer.c ..\fun\Buzzer.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Motor.h ..\fun\IMU.h \
-  ..\fun\imu948.h ..\fun\value.h
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h

MDK-ARM/AutoGuideStick/encoder.d

@@ -0,0 +1,43 @@
+autoguidestick/encoder.o: ..\fun\encoder.c ..\fun\encoder.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h

MDK-ARM/AutoGuideStick/gps.d

@@ -39,4 +39,5 @@ autoguidestick/gps.o: ..\fun\gps.c ..\fun\gps.h ..\fun\headfile.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h

MDK-ARM/AutoGuideStick/hcble.d

@@ -39,4 +39,5 @@ autoguidestick/hcble.o: ..\fun\HCBle.c ..\fun\HCBle.h ..\fun\headfile.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h

MDK-ARM/AutoGuideStick/imu.d

@@ -39,4 +39,4 @@ autoguidestick/imu.o: ..\fun\IMU.c ..\fun\IMU.h ..\fun\headfile.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h

MDK-ARM/AutoGuideStick/imu948.d

@@ -40,4 +40,4 @@ autoguidestick/imu948.o: ..\fun\imu948.c ..\fun\imu948.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\encoder.h ..\fun\value.h

MDK-ARM/AutoGuideStick/main.d

@@ -39,4 +39,5 @@ autoguidestick/main.o: ..\Core\Src\main.c ..\Core\Inc\app_threadx.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h

MDK-ARM/AutoGuideStick/motor.d

@@ -39,4 +39,4 @@ autoguidestick/motor.o: ..\fun\Motor.c ..\fun\Motor.h ..\fun\headfile.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h ..\fun\IMU.h \
-  ..\fun\imu948.h ..\fun\value.h
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h

MDK-ARM/AutoGuideStick/shake_motor.d

@@ -40,4 +40,4 @@ autoguidestick/shake_motor.o: ..\fun\Shake_Motor.c ..\fun\Shake_Motor.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Ultrasound.h ..\fun\Buzzer.h ..\fun\Motor.h ..\fun\IMU.h \
-  ..\fun\imu948.h ..\fun\value.h
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h

MDK-ARM/AutoGuideStick/stm32h5xx_it.d

@@ -40,4 +40,5 @@ autoguidestick/stm32h5xx_it.o: ..\Core\Src\stm32h5xx_it.c \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
-  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h

MDK-ARM/AutoGuideStick/ultrasound.d

@@ -40,4 +40,4 @@ autoguidestick/ultrasound.o: ..\fun\Ultrasound.c ..\fun\Ultrasound.h \
   D:\keil5\ARM\ARMCLANG\include\stdio.h \
   D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
   ..\fun\Shake_Motor.h ..\fun\Buzzer.h ..\fun\Motor.h ..\fun\IMU.h \
-  ..\fun\imu948.h ..\fun\value.h
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h

fun/HCBle.c

@@ -33,7 +33,7 @@ uint8_t  rx_data;    //
 uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
 RingBuffer ble_rx_ring = {0}; //<2F><>ʼ<EFBFBD><CABC>
 MotorCommand cmd;
-LocationData current_location = {0};
+BleMessage current_location = {0};
 float imu_angle = 0.0f;
 
 TX_EVENT_FLAGS_GROUP ble_event_flags;
@@ -128,8 +128,15 @@ void HCBle_ParseAndHandleFrame(const char *frame)
     if (sscanf(frame, "#{\"leftSpeed\":%d,\"rightSpeed\":%d}$", &left, &right) == 2) {
         cmd.LeftSpeed = left;
         cmd.RightSpeed = right;
+		
+		// <20><><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>
+		target_rpm_L = map_speed_to_rpm(cmd.LeftSpeed);
+		target_rpm_R = map_speed_to_rpm(cmd.RightSpeed);
+
         HCBle_SendData("left=%d, right=%d\r\n", cmd.LeftSpeed, cmd.RightSpeed);
 //		HCBle_SendData("left=%d, right=%d\r\n", left, right);
+		DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+//		DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed);
         return;
     }
 
@@ -200,8 +207,10 @@ void ble_tx_task_entry(ULONG thread_input) {
 		if(tx_queue_receive(&ble_tx_queue,&msg,TX_WAIT_FOREVER) == TX_SUCCESS)
 		{
 			HCBle_SendData("#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.1f}\n", 
-                              msg.lat, msg.lon,0);
+                              msg.lat, msg.lon,msg.angle);
+			
 		}
+		tx_thread_sleep(100);
     }
 }
 

fun/HCBle.h

@@ -30,12 +30,17 @@ typedef struct
 }RingBuffer;
 
 
-
+typedef struct
+{
+	float lat;
+	float lon;
+	float angle;	
+}BleMessage;
 
 extern uint8_t  rx_data;  
 extern RingBuffer ble_rx_ring; //<2F><>ʼ<EFBFBD><CABC>
 extern uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
-extern LocationData current_location;
+extern BleMessage current_location;
 
 extern uint8_t flag;
 

fun/Motor.c

@@ -64,23 +64,68 @@ void MotorB_Speed(uint8_t speed)
 // ʾ<><CABE> ͬʱ<CDAC><CAB1><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>A/B
 // <20><>ʾ <20><><EFBFBD><EFBFBD> Dir 0  ---- ǰ<><C7B0> go forward
 //      <20><><EFBFBD><EFBFBD> Dir 1  ---- <20><><EFBFBD><EFBFBD> go forward 
-void DriveBOTH(int16_t speedA,int16_t speedB)
+//void DriveBOTH(int16_t speedA,int16_t speedB)
-{
+//{
-	// <20><><EFBFBD><EFBFBD> A
+//	// <20><><EFBFBD><EFBFBD> A
-	if (speedA >= 0) MotorA_Dir(0);
+//	if (speedA >= 0) MotorA_Dir(0);
-	else
+//	else
-	{
+//	{
-		MotorA_Dir(1);
+//		MotorA_Dir(1);
-		speedA = -speedA;
+//		speedA = -speedA;
-	}
+//	}
-	MotorA_Speed(speedA);
+//	MotorA_Speed(speedA);
+//	
+//	// <20><><EFBFBD><EFBFBD> B
+//	if (speedB >= 0) MotorB_Dir(0);
+//	else
+//	{
+//		MotorB_Dir(1);
+//		speedB = -speedB;
+//	}
+//	MotorB_Speed(speedB);
+//}
 
-	// <20><><EFBFBD><EFBFBD> B
+
-	if (speedB >= 0) MotorA_Dir(0);
+// Motor.c   <20><><EFBFBD>Ͻ<EFBFBD><CFBD>д<EFBFBD><D0B4><EFBFBD>
-	else
+void DriveBOTH(int16_t speedA, int16_t speedB)
-	{
+{
-		MotorB_Dir(1);
+    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A
-		speedB = -speedB;
+    if(speedA == 0) {
-	}
+        // <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
-	MotorA_Speed(speedB);
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
+    } 
+    else if(speedA > 0) {
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
+        speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    else {
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
+        speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    
+	
+	if(speedB == 0) {
+        // <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
+    } 
+    else if(speedB > 0) {
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
+        speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    else {
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
+        speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    // ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B...
+    
+	
+    // <20><><EFBFBD><EFBFBD>PWM
+    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, speedA);
+    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, speedB);
 }

fun/Ultrasound.c

@@ -9,7 +9,7 @@ volatile uint32_t ic_val1 = 0; //
 volatile uint32_t ic_val2 = 0; // <20><><EFBFBD><EFBFBD>ֵ2
 volatile uint8_t  is_first_capture = 0; // <20>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>
 volatile uint32_t distance_cm = 0;  // <20><><EFBFBD>밴<EFBFBD><EBB0B4> cm<63><6D><EFBFBD><EFBFBD>
-
+volatile uint8_t obstacle_level = 0; // 0 - <20><><EFBFBD>ϰ<EFBFBD> , 1 = Զ ,2 = <20><> , 3 = <20><>
 /*******
 <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 Psc: 250 - 1  <20>Դﵽ   1tick = 1us<75><73>Ч<EFBFBD><D0A7>
@@ -64,7 +64,7 @@ void HCSR04_Trigger(void)
 
 #ifdef TEST
 void ultrasonic_task_entry(ULONG thread_input) {
-    HAL_TIM_IC_Start_IT(&htim2, TIM_CHANNEL_2);
+    HAL_TIM_IC_Start_IT(&htim5, TIM_CHANNEL_1);
 	DWT_Init();
 	
     while (1) {
@@ -74,9 +74,22 @@ void ultrasonic_task_entry(ULONG thread_input) {
         if (tx_event_flags_get(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR_CLEAR,
                                &events, TX_WAIT_FOREVER) == TX_SUCCESS) {
             if (distance_cm < 30) {
-                tx_event_flags_set(&system_events,EVENT_OBSTACLE_DETECTED,TX_OR);// ִ<EFBFBD>б<EFBFBD><EFBFBD>ϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>񶯡<EFBFBD>ͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD>
+                obstacle_level = 3; // <20>ܽ<EFBFBD>
+            }else if(distance_cm > 30 && distance_cm < 50)
+			{
+				obstacle_level = 2; // <20><>
+			}else if(distance_cm > 50 && distance_cm < 80)
+			{
+				obstacle_level = 1;
+			}else 
+			{
+				obstacle_level = 0; // <20><><EFBFBD>ϰ<EFBFBD>
+			}
 			
-            }
+			if(obstacle_level > 0)
+			{
+				tx_event_flags_set(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR); // <20><><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
+			}
         }
 
         tx_thread_sleep(50);  // ÿ<>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 50 ticks  20Hz <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
@@ -94,14 +107,14 @@ Echo
 
 ******/
 void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
-    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
+    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
         if (is_first_capture == 0) {
-            ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
+            ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
-            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_FALLING);
+            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
             is_first_capture = 1;
         } else {
-            ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_2);
+            ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
-            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_2, TIM_INPUTCHANNELPOLARITY_RISING);
+            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
             is_first_capture = 0;
 
             uint32_t delta = (ic_val2 > ic_val1) ? (ic_val2 - ic_val1) : (0xFFFF - ic_val1 + ic_val2);
@@ -110,6 +123,7 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
             // ֪ͨ<CDA8><D6AA><EFBFBD><EFBFBD> ȡ<><C8A1>ע<EFBFBD><D7A2><EFBFBD>Ի<EFBFBD><D4BB>Ѳ<EFBFBD><D1B2><EFBFBD><EFBFBD>߳<EFBFBD>
             tx_event_flags_set(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR);
         }
+		   HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
     }
 }
 

fun/Ultrasound.h

@@ -6,7 +6,8 @@
 #define HCSR_TEST 1
 #define EVENT_ECHO_DONE		0x01
 
-
+extern volatile uint8_t obstacle_level;
+extern volatile uint32_t distance_cm;
 void DWT_Init(void);
 /********
 delay_us() <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5> (ʹ<><CAB9>DWT)  <20><>ȷ<EFBFBD><C8B7>

fun/encoder.c

@@ -0,0 +1,135 @@
+#include "encoder.h"
+
+volatile int32_t tick_L = 0,tick_R = 0;
+volatile int16_t rpm_L = 0,rpm_R = 0;
+
+extern TIM_HandleTypeDef htim1;
+extern TIM_HandleTypeDef htim8;
+
+static TX_THREAD enc_thread;
+static UCHAR enc_stack[512]; 
+
+static void enc_thread_entry(ULONG);
+
+UINT Encoder_ThreadCreate(void)
+{
+	UINT status;
+	
+	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
+	HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
+	
+	__HAL_TIM_SET_COUNTER(&htim1,0);
+	__HAL_TIM_SET_COUNTER(&htim8,0);
+	
+    status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
+                     enc_stack, sizeof(enc_stack),
+                     6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void enc_thread_entry(ULONG arg)
+{
+	const float msPerMin = 6000.0f;
+	const float ticksPerRev = 1024.0f; // <20><><EFBFBD><EFBFBD>  * 4
+	uint16_t lastL = 0, lastR = 0;
+	
+    while (1)
+    {
+        uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
+        uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);
+
+        int16_t dL = (int16_t)(cntL - lastL);
+        int16_t dR = (int16_t)(cntR - lastR);
+
+        lastL = cntL;
+        lastR = cntR;
+
+        tick_L += dL;
+        tick_R += dR;
+
+        rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
+        rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));
+
+        tx_thread_sleep(10);
+    }
+	
+}
+
+
+//
+
+//ControlThread
+//
+
+static TX_THREAD control_thread;
+static UCHAR control_stack[512];
+
+volatile int16_t target_rpm_L = 0;
+volatile int16_t target_rpm_R = 0;
+
+static void control_thread_entry(ULONG arg);
+
+
+UINT ControlThreadCreate(void)
+{
+	UINT status;
+    status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
+                     control_stack, sizeof(control_stack),
+                     7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void control_thread_entry(ULONG arg)
+{
+    static int16_t pwmL = 0, pwmR = 0;
+    static float integralL = 0, integralR = 0;
+    float Kp = 1.0f, Ki = 0.1f;
+
+    while (1)
+    {
+        int16_t errorL = target_rpm_L - rpm_L;
+        int16_t errorR = target_rpm_R - rpm_R;
+
+        integralL += errorL;
+        integralR += errorR;
+		
+		// <20>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD>ۼӺ<DBBC><D3BA><EFBFBD><EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD>
+		if (errorL == 0) integralL = 0;
+		if (errorR == 0) integralR = 0;
+
+		const float max_integral = 1000.0f;
+        if(integralL > max_integral) integralL = max_integral;
+        if(integralL < -max_integral) integralL = -max_integral;
+		
+		if(integralR > max_integral) integralR = max_integral;
+        if(integralR < -max_integral) integralR = -max_integral;
+        // ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>integralR...
+		
+        pwmL += (int16_t)(Kp * errorL + Ki * integralL);
+        pwmR += (int16_t)(Kp * errorR + Ki * integralR);
+
+        if (pwmL < 0) pwmL = 0;
+        if (pwmL > 255) pwmL = 255;
+        if (pwmR < 0) pwmR = 0;
+        if (pwmR > 255) pwmR = 255;
+
+        DriveBOTH(pwmL, pwmR);
+
+        tx_thread_sleep(10);
+    }
+}
+
+
+
+
+
+int16_t map_speed_to_rpm(int speed)
+{
+    if (abs(speed) < 30) return 0;
+    return (speed - 60) * 1.5;  // <20><><EFBFBD><EFBFBD>
+}
+

fun/encoder.h

@@ -0,0 +1,12 @@
+#ifndef __ENCODER_H__
+#define __ENCODER_H__
+
+#include "headfile.h"
+
+extern volatile int16_t target_rpm_L;
+extern volatile int16_t target_rpm_R;
+int16_t map_speed_to_rpm(int speed);
+UINT ControlThreadCreate(void);
+UINT Encoder_ThreadCreate(void);
+
+#endif 

fun/gps.c

@@ -349,13 +349,14 @@ void gps_thread_entry(ULONG thread_input)
             tx_event_flags_set(&system_events, EVENT_LOCATION_UPDATED, TX_OR);
 
 			
-			msg.lat = current_location.lat;
+//			msg.lat = current_location.lat;
-			msg.lon = current_location.lon;
+//			msg.lon = current_location.lon;
 //            snprintf(msg, sizeof(msg), "#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n",
 //                     current_location.lat,
 //                     current_location.lon,
 //                     current_location.angle);
-            tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
+			BleMessage msg = current_location;
+			tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
 
         }
     }

fun/headfile.h

@@ -23,6 +23,8 @@
 #include "Motor.h"
 #include "IMU.h"
 #include "imu948.h"
+#include "encoder.h"
+
 
 #include "value.h" // ȫ<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 

fun/imu948.c

@@ -1,5 +1,6 @@
 #include "imu948.h"
 
+#define DECLINATION_DEG  -3.0
 extern _GPSData gps_data;
 extern TX_QUEUE ble_tx_queue;
 extern uint8_t rx_byte;
@@ -65,7 +66,9 @@ extern TX_QUEUE im948_uart_rx_queue;
 void imu_angle_ble_task_entry(ULONG thread_input)
 {
     ULONG rx_data;
-	BleMessage msg;
+	static uint8_t filtInit = 0;
+	static float   heading_filt = 0;
+	const  float   alpha = 0.20f;               // 20% һ<>׵<EFBFBD>ͨ
     // <20><>ʼ<EFBFBD><CABC>ģ<EFBFBD><C4A3>
 //    IM948_Init();
 //			HCBle_SendData("halo");
@@ -82,12 +85,28 @@ void imu_angle_ble_task_entry(ULONG thread_input)
         {
             isNewData = 0;
 
-            float angle = AngleZ * 0.0054931640625f;
-//			msg.angle = AngleZ * 0.0054931640625f;
-//			tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
-//			HCBle_SendData("Z:%.2f\r\n",AngleZ);
-            // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>angle<6C><65>ä<EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD>ж<EFBFBD>
 
+            // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>angle<6C><65>ä<EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD>ж<EFBFBD>
+			        /* <20><><EFBFBD><EFBFBD> <20><> IMU <20>Ĵ<EFBFBD><C4B4><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>ں<EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> */
+			float angleRaw = AngleZ;                   /* <20><><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD>׼ */
+
+        /* ת<>汱    <20><>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>ڡ<EFBFBD><DAA1><EFBFBD><EFBFBD>ݴ<EFBFBD>ƫ<EFBFBD><C6AB>Ϊ-3.0<EFBFBD><EFBFBD>*/
+			float heading = angleRaw + DECLINATION_DEG;
+			if (heading > 180)  heading -= 360;
+			if (heading < -180) heading += 360;
+
+        /* <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB> IIR */
+			const float alpha = 0.2f;
+			heading_filt += alpha * (heading - heading_filt);
+
+			current_location.angle = heading_filt;
+//			current_location.angle = AngleZ * 0.0054931640625f;
+			if(current_location.lat != 0 && current_location.lon != 0)
+			{
+				BleMessage msg = current_location; // <20>ṹ<EFBFBD><E1B9B9>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>
+				tx_queue_send(&ble_tx_queue,&msg,TX_NO_WAIT);
+			}
+			tx_thread_sleep(10);
         }
     }
 }

fun/value.h

@@ -1,12 +1,12 @@
 #ifndef __VALUE_H__
 #define __VALUE_H__
 
-typedef struct
+//typedef struct
-{
+//{
-	float lat;
+//	float lat;
-	float lon;
+//	float lon;
-	float angle;	
+//	float angle;	
-}BleMessage;
+//}BleMessage;
 
 #define EVENT_GPS_DATA_READY			(1U << 0)
 #define EVENT_IMU_DATA_READY			(1U << 1)