/* USER CODE BEGIN Header */ /** ****************************************************************************** * @file : main.c * @brief : Main program body ****************************************************************************** * @attention * * Copyright (c) 2026 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "dma.h" #include "tim.h" #include "usart.h" #include "gpio.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "RTX.h" #include "math.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ int map(int x, int in_min, int in_max, int out_min, int out_max) { // ??????? if (in_max == in_min) { return out_min; } // ???????? return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void cosplay(); /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ uint8_t buf[25] = {0}; // DMA????????? uint16_t channels[16] = {0}; // 16????????????172~1811?? uint8_t sbus_data_ready = 0; // ??????????λ void sbus_parse(uint8_t *buf, uint16_t *channels); float aerfa =0.0f; int k=0; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ int16_t lx,ly,fx; int vx,vy,yaw; float v24; void yawxuan(); void cosplay(); void stay(); int ceshi=0; double w=0.0; /* USER CODE END 0 */ /** * @brief The application entry point. * @retval int */ int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 12222 /* MCU Configuration--------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* USER CODE BEGIN Init */ /* USER CODE END Init */ /* Configure the system clock */ SystemClock_Config(); /* USER CODE BEGIN SysInit */ /* USER CODE END SysInit */ /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_USART2_UART_Init(); MX_TIM2_Init(); MX_TIM1_Init(); /* USER CODE BEGIN 2 */ HAL_UART_Receive_DMA(&huart2, buf,25); HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_4); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { if (sbus_data_ready) { sbus_parse(buf, channels); // 璋冪敤瑙ｆ瀽鍑芥暟 sbus_data_ready = 0; } if(channels[4]==200&&channels[5]==200) { k=80;cosplay(); } else if(channels[4]==200&&channels[5]==1000) { k=10;cosplay(); } else if(channels[4]==200&&channels[5]==1800) { stay(); } if(channels[4]==1000&&channels[5]==1000) { yawxuan(); } else if (channels[4]==1000&&channels[5]==200) { //jixiebi } else if(channels[4]==1000&&channels[5]==1800) { //jixiebi } HAL_Delay(75); /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ } /** * @brief System Clock Configuration * @retval None */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Initializes the RCC Oscillators according to the specified parameters * in the RCC_OscInitTypeDef structure. */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1; RCC_OscInitStruct.HSIState = RCC_HSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { Error_Handler(); } /** Initializes the CPU, AHB and APB buses clocks */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) { Error_Handler(); } } /* USER CODE BEGIN 4 */ void sbus_parse(uint8_t *buf, uint16_t *channels) { if(buf[0] != 0x0F) return; // SBUS??����D��?���ꡧ0x0F��?��?��???����?��?a?�� // ?a??�����̨�1-16�ꡧSBUS�����̨�?�̡�??���o172~1811��? channels[0] = ((buf[1] | buf[2] << 8) & 0x07FF); channels[1] = ((buf[2] >> 3 | buf[3] << 5) & 0x07FF); channels[2] = ((buf[3] >> 6 | buf[4] << 2 | buf[5] << 10) & 0x07FF); channels[3] = ((buf[5] >> 1 | buf[6] << 7) & 0x07FF); channels[4] = ((buf[6] >> 4 | buf[7] << 4) & 0x07FF); channels[5] = ((buf[7] >> 7 | buf[8] << 1 | buf[9] << 9) & 0x07FF); channels[6] = ((buf[9] >> 2 | buf[10] << 6) & 0x07FF); channels[7] = ((buf[10] >>5 | buf[11] << 3) & 0x07FF); channels[8] = ((buf[11] >> 8 | buf[12] << 0 | buf[13] << 8) & 0x07FF); channels[9] = ((buf[13] >>3 | buf[14] << 5) & 0x07FF); channels[10] = ((buf[14] >>6 | buf[15] << 2 | buf[16] <<10) & 0x07FF); channels[11] = ((buf[16] >>1 | buf[17] <<7) & 0x07FF); channels[12] = ((buf[17] >>4 | buf[18] <<4) & 0x07FF); channels[13] = ((buf[18] >>7 | buf[19] <<1 | buf[20] <<9) & 0x07FF); channels[14] = ((buf[20] >>2 | buf[21] <<6) & 0x07FF); channels[15] = ((buf[21] >>5 | buf[22] <<3) & 0x07FF); } //sbus_channel[0 0]����X 88-1688 //sbus_channel[1]����Y 136-1736 //sbus_channel[2]����Y 360-1960 //sbus_channel[3]����X 184-1784 void cosplay() { lx=channels[3]; ly=channels[2]; vx=map(lx,216,1784,-100,100); vy=map(ly,360,1960,-100,100); aerfa=atan2(vy,vx); v24=k*sin(aerfa-0.785398); if(v24>0) { HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET); } else { HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET); } __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3,fabs(v24)); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4,fabs(v24)); } void stay() { HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET); } void yawxuan() { w=map(channels[0],544,1332,-80,80); if(yaw>-15&&yaw<15) yaw=0; if(yaw>0) { HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_RESET); } else { HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_15,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOB,GPIO_PIN_14,GPIO_PIN_RESET); } __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3,fabs(w)); __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4,fabs(w)); } /* USER CODE END 4 */ /** * @brief This function is executed in case of error occurrence. * @retval None */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ /* User can add his own implementation to report the HAL error return state */ __disable_irq(); while (1) { } /* USER CODE END Error_Handler_Debug */ } #ifdef USE_FULL_ASSERT /** * @brief Reports the name of the source file and the source line number * where the assert_param error has occurred. * @param file: pointer to the source file name * @param line: assert_param error line source number * @retval None */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ /* User can add his own implementation to report the file name and line number, ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ /* USER CODE END 6 */ } #endif /* USE_FULL_ASSERT */