372 lines
10 KiB
C
372 lines
10 KiB
C
/* 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??<3F><><EFBFBD><EFBFBD>D<EFBFBD><44>?<3F><><EFBFBD>ꡧ0x0F<30><46>?<3F><>?<3F><>???<3F><><EFBFBD><EFBFBD>?<3F><>?a?<3F><>
|
||
|
||
// ?a??<3F><><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD>1-16<31>ꡧSBUS<55><53><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD>?<3F>̡<EFBFBD>??<3F><><EFBFBD>o172~1811<31><31>?
|
||
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]<5D><><EFBFBD><EFBFBD>X 88-1688
|
||
//sbus_channel[1]<5D><><EFBFBD><EFBFBD>Y 136-1736
|
||
//sbus_channel[2]<5D><><EFBFBD><EFBFBD>Y 360-1960
|
||
//sbus_channel[3]<5D><><EFBFBD><EFBFBD>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_13,GPIO_PIN_SET);
|
||
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_12,GPIO_PIN_RESET);
|
||
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_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 */
|