其他分享
首页 > 其他分享> > 树莓派ROS stm32 slam Freertos VFH+A*避障路径规划-智能平衡计划(七)

树莓派ROS stm32 slam Freertos VFH+A*避障路径规划-智能平衡计划(七)

作者:互联网

基于树莓派ROSstm32搭载Freertos智能平衡车Day7


前言

交互进程的实现用ESP32,实现交互进程


提示:以下是本篇文章正文内容,下面案例可供参考

通信功能分析及ESP32模块介绍

在这里插入图片描述
在这里插入图片描述

上位机APP

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

ESP32介绍

在这里插入图片描述

官网:https://www.espressif.com/zh-hans/products/hardware/modules

在这里插入图片描述

ESP32 AT指令

在这里插入图片描述

WIFI 相关的 AT指令

在这里插入图片描述

ESP32 多连接 TCP 服务器使用

在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

ESP32 BLE相关AT指令

在这里插入图片描述

ESP32 基于 BLE 连接的应用

在这里插入图片描述

在这里插入图片描述
在这里插入图片描述

BLE服务器发送数据

在这里插入图片描述

BLE客户端发送数据

在这里插入图片描述

ESP32模块驱动讲解

ESP32 电气连接原理图

在这里插入图片描述
在这里插入图片描述

上位机通信协议实现

通信协议的制定——上位机发送数据

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

通信协议的制定——上位机接收数据

在这里插入图片描述
在这里插入图片描述

二、start!代码

由于是不定长的数据所以要配置为DMA模式
在这里插入图片描述

中断也要打开
在这里插入图片描述

esp32.c


#include "esp32.h"
#include "usart.h"
#include <stdarg.h>
#include <string.h>
#include "connect.h"
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"


uint8_t NET_MODE = 0;             //0、蓝牙模式     1、wifi模式   默认蓝牙
uint8_t WIFI_CONNECT_FLAG = 0;    //WIFI连接标志位
uint8_t BLE_CONNECT_FLAG = 0;     //BLE连接标志位

tEsp32_RcvBuf Esp32Rcv;           //ESP数据接收缓冲区


tTimeDelay    ESP32_TimeDelay;


extern 	QueueHandle_t Message_Queue;

#define AT_CWSAP_STRING   "AT+CWSAP=\"FarsightESP32\",\"123456789\",5,3\r\n"
#define AT_BLEADVDATA_STRING    "AT+BLEADVDATA=\"0201060B09466172736967687420030302A0\"\r\n"

volatile tATCmd  ATCmds[20]=
{
  //*CmdSend,              *CmdRcv,    TimeOut,   CmdStatus,  
  {NULL,NULL,0,NO_RCV},
  {"AT\r\n",             "OK",         5000,      NO_RCV, },   //检测AT指令       
  {"AT+CIPAPMAC?\r\n",   "CIPAPMAC",   2000,      NO_RCV, },	 //获取MAC地址    
  {AT_CWSAP_STRING,       "CWSAP" ,    2000,      NO_RCV, },   //建立MAC相关的AP名称  
  {"AT+CWMODE=3\r\n",    "OK" ,        2000,      NO_RCV, },   //设置WIFI模式AP+Station
  {"AT+CIPMUX=1\r\n",    "OK" ,        2000,      NO_RCV, },   //设置多连接
  {"AT+CIPSERVER=1\r\n", "OK" ,        2000,      NO_RCV, },   //初始化TCP服务器 默认IP(192.168.4.1)默认端口号(333)
  {"AT+CIPSTO=0\r\n",    "OK" ,        2000,      NO_RCV, },   //设置TCP连接时间
  {"AT+CIPSEND=0\r\n",   "OK" ,        500,       NO_RCV, },   //TCP发送数据

	{"AT+RST\r\n",          "ready" ,    1000,      NO_RCV, },   //重启AT指令:
  {"AT+BLEINIT=2\r\n",   "OK" ,        1000,      NO_RCV, },   //初始化为 BLE server:
  {"AT+BLEADDR?\r\n",    "BLEADDR" ,   2000,      NO_RCV, },   //查询自身的 BLE 地址
  {AT_BLEADVDATA_STRING,  "OK" ,       2000,      NO_RCV, },   //配置广播数据包
  {"AT+BLEGATTSSRVCRE\r\n",  "OK",     1000,      NO_RCV, },   //创建服务:
  {"AT+BLEGATTSSRVSTART\r\n", "OK" ,   3000,      NO_RCV, },   //开启服务 
  {"AT+BLEADVSTART\r\n",   "OK" ,      1000,      NO_RCV, },   //开始广播
  {"AT+BLEGATTSNTFY\r\n" , ">" ,       500,       NO_RCV, },   //服务器发送数据
	
  {"CMDSTR_NOUSE",       "OK" ,        2000,      NO_RCV, }, 
};


void uart_data_send(uint8_t *fmt, uint16_t len)
{
	taskENTER_CRITICAL();  
	HAL_UART_Transmit(&huart6, (uint8_t *)fmt, len,100);
	taskEXIT_CRITICAL(); 
}


tCmdStatus ESPSend_Cmd(tATCmdNum ATCmdNum)
{		
		uint8_t len;
	
		//清空接收缓存以及接收状态

		ATCmds[ATCmdNum].CmdStatus = NO_RCV;
		
		//发送命令
		len = strlen(ATCmds[ATCmdNum].CmdSend);
		uart_data_send((uint8_t *)ATCmds[ATCmdNum].CmdSend, len);
		HAL_UART_Transmit(&huart1,(uint8_t *)ATCmds[ATCmdNum].CmdSend, len,100);

		
	 //打开超时定时器
	 SetTime(&ESP32_TimeDelay, ATCmds[ATCmdNum].TimeOut);
		 
	 while(ATCmds[ATCmdNum].CmdStatus != RCV_SUCCESS)
	 {
		 
			ESP32_Cmd_Rcv(ATCmdNum);
		  if(ATCmds[ATCmdNum].CmdStatus == RCV_TIMEOUT)
				return RCV_TIMEOUT;
	 }
	 
	 return RCV_SUCCESS;
}

/*发送数据函数*/
tCmdStatus ESP32_Send_Data(uint8_t *SendBuf,uint8_t len)
{
	  uint8_t buf[30] =  {0};
		tATCmdNum ATCmdNum;
	
		if(! (BLE_CONNECT_FLAG || WIFI_CONNECT_FLAG))  //未连接状态不能发送数据
		{
			DBG("未连接设备\n");
			return NO_CONNECT;
		}		
		
		if(NET_MODE == BLE_MODE)    //蓝牙模式
		{
			sprintf((char *)buf,"AT+BLEGATTSNTFY=%d,%d,%d,%d\r\n",0,1,2,len);
			ATCmdNum = AT_BLEGATTSNTFY;		
		}
		else 			//WIFI模式
		{
			sprintf((char *)buf,"AT+CIPSEND=%d,%d\r\n",0,len);
			ATCmdNum = AT_CIPSEND;
		}
			
		uart_data_send(buf,strlen((char *)buf));     //发送命令
		
		//打开超时定时器
	 ATCmds[ATCmdNum].CmdStatus = NO_RCV;        //清接收状态
	 SetTime(&ESP32_TimeDelay, ATCmds[ATCmdNum].TimeOut);
		 
	 while(ATCmds[ATCmdNum].CmdStatus != RCV_SUCCESS)
	 {		 
			ESP32_Cmd_Rcv(ATCmdNum);
		  if(ATCmds[ATCmdNum].CmdStatus == RCV_TIMEOUT)
				return RCV_TIMEOUT;
	 }
		
		uart_data_send( SendBuf,len);                //发送数据

		DBG("send data ok\n");
	 
	 return RCV_SUCCESS;

}



void ESP32_Cmd_Rcv(tATCmdNum ATCmdNum)
{
	memset(&Esp32Rcv,0,sizeof(Esp32Rcv));
	
	if(xQueueReceive(Message_Queue, &Esp32Rcv,0 ))
	{
				DBG("%s", Esp32Rcv.RcvBuf);
		
				//接收处理命令
				if(strstr((const char*)Esp32Rcv.RcvBuf,ATCmds[ATCmdNum].CmdRcv) != NULL)
				{
					ATCmds[ATCmdNum].CmdStatus = RCV_SUCCESS;						
				}			
	 
				//打开接收指示灯
				//SetLedRun(LED_RX);
				
				
	}
	else
	{
			if(CompareTime(&ESP32_TimeDelay))
			{
				ATCmds[ATCmdNum].CmdStatus = RCV_TIMEOUT;
			}
	}	

}

void ESP32_Data_Rcv(void)
{
	memset(&Esp32Rcv,0,sizeof(Esp32Rcv));
	if(xQueueReceive(Message_Queue, &Esp32Rcv,0 ))
	{
			 //接收处理数据(保护客户端发来的数据,还有其他调试数据)			

				DBG("%s", Esp32Rcv.RcvBuf);
		
			 if(NET_MODE == BLE_MODE)    //蓝牙模式
			 {
						
						if(strstr((char *)(Esp32Rcv.RcvBuf),"WRITE") != NULL ) //收到客户端数据
						{
								DBG("收到上位机数据\n");
				
								BLE_CONNECT_FLAG = 1;   //对方打开读写特征值时,置连接标志
							
							
								//提取处理数据;
								EP32_RcvData_Extract(Esp32Rcv.RcvBuf,Esp32Rcv.DataLen);	
												
								return ;			          				
						}
						
						if(strstr((char *)(Esp32Rcv.RcvBuf),"BLEDISCONN") != NULL) //客户端断开连接
					 {
							 DBG("蓝牙断开连接,重新广播\n");	
						 
							 BLE_CONNECT_FLAG = 0;    //清除连接标志位
								//重新广播
							 ESPSend_Cmd(AT_BLEADVDATA);
							 ESPSend_Cmd(AT_BLEADVSTART);
					 }

		  }
			else     //WIFI模式
			{
				
				   if((!WIFI_CONNECT_FLAG) && (strstr((char *)(Esp32Rcv.RcvBuf),"CONNECT")!=NULL )) //收到客户端数据
					 {
							DBG("WIFI已连接\n");	
						 
							WIFI_CONNECT_FLAG = 1;   //置连接标志位
					 }

			
				
					if(strstr((char *)(Esp32Rcv.RcvBuf),"+IPD") != NULL  ) //收到客户端数据
				  {
				 		DBG("WIFI收到上位机数据\n");	
						
						//提取并处理数据;
						EP32_RcvData_Extract(Esp32Rcv.RcvBuf,Esp32Rcv.DataLen);	
											
						return ;						
					
				  }	

					if(strstr((char *)(Esp32Rcv.RcvBuf),"CLOSED") != NULL) //客户端断开连接
					 {
							 DBG("WIFI断开连接\n");	
						 
							 WIFI_CONNECT_FLAG = 0;    //清除连接标志位

					 }	
				
			}
			
			
	}
}
	



void ESP32_Init(void)
{
		tATCmdNum i = AT_IDIE;
	
		if(NET_MODE == BLE_MODE)                //蓝牙模式初始化
		{
			for(i = AT_BLEINIT; i<=AT_BLEADVSTART ; i++)
			{
				if( ESPSend_Cmd(i) != RCV_SUCCESS)
				{				
					DBG("PES32 Init failed\n");
					return ;
				}
				
			}
		}
		else                                  //WIFI模式初始化
		{
					for(i = AT; i<=AT_CIPSTO ; i++)
					{						
						if( ESPSend_Cmd(i) != RCV_SUCCESS)
						{
							DBG("PES32 Init failed\n");
							return ;
						}
					}
					
						DBG("PES32 Init Success\n");
		}
}

esp32.h

#ifndef __ESP32_H
#define __ESP32_H

#include <stdint.h>
#include <stdio.h>
#include "delay.h"

extern uint8_t Esp32_RcvBuf [255];
extern uint8_t Esp32_RBuffLen ; 

//#define DEBUG 

#ifdef DEBUG 
#define DBG(x...)   printf(x) 
#else 
#define DBG(x...) 
#endif  



#define  BLE_MODE   0
#define  WIFI_MODE  1



//AT命令序列号
typedef enum
{
	AT_IDIE  = 0,
	AT,
	AT_CIPAPMAC,
	AT_CWSAP,
	AT_CWMODE,
	AT_CIPMUX,
	AT_CIPSERVER,
	AT_CIPSTO,
	AT_CIPSEND,
	
	AT_RST,
	AT_BLEINIT,
	AT_BLEADDR,
	AT_BLEADVDATA,
	AT_BLEGATTSSRVCRE,
	AT_BLEGATTSSRVSTART,
	AT_BLEADVSTART,
	AT_BLEGATTSNTFY,
	CMDSTR_NOUSE,

}tATCmdNum;


//命令返回结果的状态
typedef enum{
	NO_RCV  = 0,
	RCV_SUCCESS,
	RCV_TIMEOUT,
	NO_CONNECT,
}tCmdStatus;


typedef struct{
	char *CmdSend;         //发送的命令
	char *CmdRcv; 	       //正确返回包含的字符串
	uint16_t TimeOut;      //超时的时间
	tCmdStatus CmdStatus;  //命令返回的状态
}tATCmd; 

typedef struct {
	uint8_t DataLen;
	uint8_t RcvBuf  [255];
}tEsp32_RcvBuf;

extern tTimeDelay    ESP32_TimeDelay; 

extern uint8_t Esp32_RcvBuf  [255];
extern uint8_t ESP32_RCV_FLAG;
extern uint8_t NET_MODE ; 
extern uint8_t WIFI_CONNECT_FLAG ;    //WIFI连接标志位
extern uint8_t BLE_CONNECT_FLAG ;     //BLE连接标志位


void ESP32_Cmd_Rcv(tATCmdNum ATCmdNum);
tCmdStatus ESP32_Send_Data(uint8_t *SendBuf,uint8_t len);
void ESP32_Init(void);
void ESP32_Data_Rcv(void);

#endif /*__ESP32_H*/


freertos.c

/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2018 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"

/* USER CODE BEGIN Includes */     

#include "main.h"
#include "stm32f4xx_hal.h"
#include "mpu6050.h"
#include "car_task.h"
#include "inv_mpu_user.h"
#include "esp32.h"
#include "connect.h"

/* USER CODE END Includes */

/* Variables -----------------------------------------------------------------*/
osThreadId Task_200HZHandle;
osThreadId Task_100HZHandle;
osThreadId Task_5HZHandle;
osThreadId Task_InteractioHandle;

/* USER CODE BEGIN Variables */

#define   Message_Q_NUM      5
#define   Message_Q_Length   sizeof(tEsp32_RcvBuf)
xQueueHandle  Message_Queue;
tEsp32_RcvBuf Uart6_Rcv;


/* USER CODE END Variables */

/* Function prototypes -------------------------------------------------------*/
void StartTask_200HZ(void const * argument);
void StartTask_100HZ(void const * argument);
void StartTask_5HZ(void const * argument);
void StartTask_Interaction(void const * argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

/* Hook prototypes */

/* Init FreeRTOS */

void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */
       
  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the thread(s) */
  /* definition and creation of Task_200HZ */
  osThreadDef(Task_200HZ, StartTask_200HZ, osPriorityNormal, 0, 128);
  Task_200HZHandle = osThreadCreate(osThread(Task_200HZ), NULL);

  /* definition and creation of Task_100HZ */
  osThreadDef(Task_100HZ, StartTask_100HZ, osPriorityIdle, 0, 128);
  Task_100HZHandle = osThreadCreate(osThread(Task_100HZ), NULL);

  /* definition and creation of Task_5HZ */
  osThreadDef(Task_5HZ, StartTask_5HZ, osPriorityIdle, 0, 128);
  Task_5HZHandle = osThreadCreate(osThread(Task_5HZ), NULL);

  /* definition and creation of Task_Interactio */
  osThreadDef(Task_Interactio, StartTask_Interaction, osPriorityIdle, 0, 128*5);
  Task_InteractioHandle = osThreadCreate(osThread(Task_Interactio), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */
}

/* StartTask_200HZ function */
void StartTask_200HZ(void const * argument)
{

  /* USER CODE BEGIN StartTask_200HZ */
	
	//printf("环境采集进程运行\n");
	
	
	MPU_Init();
	
	while(mpu_dmp_init());
	
  /* Infinite loop */
  for(;;)
  {
		
		Car_Task_200HZ();
		
    osDelay(5);
  }
  /* USER CODE END StartTask_200HZ */
}

/* StartTask_100HZ function */
void StartTask_100HZ(void const * argument)
{
  /* USER CODE BEGIN StartTask_100HZ */
	
	//printf("PID控制进程运行\n");
	
	
	
	
  /* Infinite loop */
  for(;;)
  {
		
		Car_Task_100HZ();
    osDelay(10);
  }
  /* USER CODE END StartTask_100HZ */
}

/* StartTask_5HZ function */
void StartTask_5HZ(void const * argument)
{
  /* USER CODE BEGIN StartTask_5HZ */
	
	//printf("菜单显示进程运行\n");
	
  /* Infinite loop */
  for(;;)
  {
		Car_Task_5HZ();
    osDelay(500);
  }
  /* USER CODE END StartTask_5HZ */
}

/* StartTask_Interaction function */
void StartTask_Interaction(void const * argument)
{
  /* USER CODE BEGIN StartTask_Interaction */
	
	uint8_t time = 0;
	
	 printf("交互进程运行\n");
	
	 Message_Queue =  xQueueCreate ( Message_Q_NUM, Message_Q_Length );
	
	 HAL_UART_Receive_DMA(&huart6, Uart6_Rcv.RcvBuf, 255);
	 __HAL_UART_ENABLE_IT(&huart6,UART_IT_IDLE );
	
	ESP32_Init();
	
	
  /* Infinite loop */
  for(;;)
  {
		
		ESP32_Data_Rcv();
		
		time++;
		if(time >= 50)
		{
			time = 0;
			Connect_Send_data(READ_ALL_ARG);
		}
    osDelay(10);
  }
  /* USER CODE END StartTask_Interaction */
}

/* USER CODE BEGIN Application */
     
/* USER CODE END Application */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

中断函数

/**
  ******************************************************************************
  * @file    stm32f4xx_it.c
  * @brief   Interrupt Service Routines.
  ******************************************************************************
  *
  * COPYRIGHT(c) 2018 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "stm32f4xx.h"
#include "stm32f4xx_it.h"
#include "cmsis_os.h"

/* USER CODE BEGIN 0 */
#include "car_task.h"
#include "esp32.h"
#include "string.h"

extern xQueueHandle  Message_Queue;
extern tEsp32_RcvBuf Uart6_Rcv;
/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern TIM_HandleTypeDef htim7;
extern DMA_HandleTypeDef hdma_usart3_rx;
extern DMA_HandleTypeDef hdma_usart6_rx;
extern UART_HandleTypeDef huart3;
extern UART_HandleTypeDef huart6;

extern TIM_HandleTypeDef htim6;

/******************************************************************************/
/*            Cortex-M4 Processor Interruption and Exception Handlers         */ 
/******************************************************************************/

/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

  /* USER CODE END NonMaskableInt_IRQn 0 */
  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */

  /* USER CODE END NonMaskableInt_IRQn 1 */
}

/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
  /* USER CODE BEGIN HardFault_IRQn 0 */

  /* USER CODE END HardFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
    /* USER CODE END W1_HardFault_IRQn 0 */
  }
  /* USER CODE BEGIN HardFault_IRQn 1 */

  /* USER CODE END HardFault_IRQn 1 */
}

/**
* @brief This function handles Memory management fault.
*/
void MemManage_Handler(void)
{
  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

  /* USER CODE END MemoryManagement_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
    /* USER CODE END W1_MemoryManagement_IRQn 0 */
  }
  /* USER CODE BEGIN MemoryManagement_IRQn 1 */

  /* USER CODE END MemoryManagement_IRQn 1 */
}

/**
* @brief This function handles Pre-fetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
  /* USER CODE BEGIN BusFault_IRQn 0 */

  /* USER CODE END BusFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_BusFault_IRQn 0 */
    /* USER CODE END W1_BusFault_IRQn 0 */
  }
  /* USER CODE BEGIN BusFault_IRQn 1 */

  /* USER CODE END BusFault_IRQn 1 */
}

/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
  /* USER CODE BEGIN UsageFault_IRQn 0 */

  /* USER CODE END UsageFault_IRQn 0 */
  while (1)
  {
    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */
    /* USER CODE END W1_UsageFault_IRQn 0 */
  }
  /* USER CODE BEGIN UsageFault_IRQn 1 */

  /* USER CODE END UsageFault_IRQn 1 */
}

/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

  /* USER CODE END DebugMonitor_IRQn 0 */
  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

  /* USER CODE END DebugMonitor_IRQn 1 */
}

/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  osSystickHandler();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f4xx.s).                    */
/******************************************************************************/

/**
* @brief This function handles EXTI line1 interrupt.
*/
void EXTI1_IRQHandler(void)
{
  /* USER CODE BEGIN EXTI1_IRQn 0 */

  /* USER CODE END EXTI1_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_1);
  /* USER CODE BEGIN EXTI1_IRQn 1 */

  /* USER CODE END EXTI1_IRQn 1 */
}

/**
* @brief This function handles DMA1 stream1 global interrupt.
*/
void DMA1_Stream1_IRQHandler(void)
{
  /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */

  /* USER CODE END DMA1_Stream1_IRQn 0 */
  HAL_DMA_IRQHandler(&hdma_usart3_rx);
  /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */

  /* USER CODE END DMA1_Stream1_IRQn 1 */
}

/**
* @brief This function handles USART3 global interrupt.
*/
void USART3_IRQHandler(void)
{
  /* USER CODE BEGIN USART3_IRQn 0 */

  /* USER CODE END USART3_IRQn 0 */
  HAL_UART_IRQHandler(&huart3);
  /* USER CODE BEGIN USART3_IRQn 1 */

  /* USER CODE END USART3_IRQn 1 */
}

/**
* @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts.
*/
void TIM6_DAC_IRQHandler(void)
{
  /* USER CODE BEGIN TIM6_DAC_IRQn 0 */

  /* USER CODE END TIM6_DAC_IRQn 0 */
  HAL_TIM_IRQHandler(&htim6);
  /* USER CODE BEGIN TIM6_DAC_IRQn 1 */

  /* USER CODE END TIM6_DAC_IRQn 1 */
}

/**
* @brief This function handles TIM7 global interrupt.
*/
void TIM7_IRQHandler(void)
{
  /* USER CODE BEGIN TIM7_IRQn 0 */

  /* USER CODE END TIM7_IRQn 0 */
  HAL_TIM_IRQHandler(&htim7);
  /* USER CODE BEGIN TIM7_IRQn 1 */

  /* USER CODE END TIM7_IRQn 1 */
}

/**
* @brief This function handles DMA2 stream1 global interrupt.
*/
void DMA2_Stream1_IRQHandler(void)
{
  /* USER CODE BEGIN DMA2_Stream1_IRQn 0 */

  /* USER CODE END DMA2_Stream1_IRQn 0 */
  HAL_DMA_IRQHandler(&hdma_usart6_rx);
  /* USER CODE BEGIN DMA2_Stream1_IRQn 1 */

  /* USER CODE END DMA2_Stream1_IRQn 1 */
}

/**
* @brief This function handles USART6 global interrupt.
*/
void USART6_IRQHandler(void)
{
  /* USER CODE BEGIN USART6_IRQn 0 */
	portBASE_TYPE pxHigherPriorityTaskWoken;
	
	
	
	if(__HAL_UART_GET_FLAG(&huart6, UART_FLAG_IDLE) == SET)
	{
			__HAL_UART_CLEAR_FLAG(&huart6, UART_FLAG_IDLE);
			HAL_UART_DMAStop(&huart6);
		
			Uart6_Rcv.DataLen = 255 - hdma_usart6_rx.Instance->NDTR;
		
			xQueueSendFromISR (Message_Queue, &Uart6_Rcv, &pxHigherPriorityTaskWoken);
		
			memset(&Uart6_Rcv, 0, sizeof(Uart6_Rcv));
	}
	
	
  /* USER CODE END USART6_IRQn 0 */
  HAL_UART_IRQHandler(&huart6);
  /* USER CODE BEGIN USART6_IRQn 1 */

	HAL_UART_Receive_DMA(&huart6, Uart6_Rcv.RcvBuf, 255);
	
  /* USER CODE END USART6_IRQn 1 */
}

/* USER CODE BEGIN 1 */

uint8_t ECHO_IRQ_FLAG = 0;
uint8_t time_cnt = 0;
static int last_distence =0 ;

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	int time = 0;
	
	if(GPIO_Pin == GPIO_PIN_1 )
	{
		if(ECHO_IRQ_FLAG == 0)              //上升沿中断
		{ 
			ECHO_IRQ_FLAG = 1;
			
			time_cnt  = 0;
			TIM7->CNT =0;
			HAL_TIM_Base_Start_IT(&htim7);    //启动定时器,开始计时
			
		}
		else                                //下降沿中断
		{
			ECHO_IRQ_FLAG = 0;
			
			HAL_TIM_Base_Stop_IT(&htim7);     //关闭定时器,计时完毕
			
		  time  = (time_cnt*1000 + TIM7->CNT) /2 ;   //统计声音单次传播的时间
			
			Distence = 34000 * time /1000000;		//计算距离
			
			//过滤掉可能出现0这种错误情况
			if(Distence == 0)   Distence = last_distence;
			else                last_distence = Distence;      
			 			
 		}
	}
}


void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart->Instance == USART3)
	{
		HAL_UART_Receive_DMA(&huart3, (uint8_t *)&CCD, sizeof(CCD));
	}

}

/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

cartask.c

#include "car_task.h"
#include "mpu6050.h"
#include "inv_mpu_user.h"
#include "contrl.h"

int  FS_MODE = 0 ;                      //0、遥控模式   1、蔽障模式  2、巡线模式 
int  Balance_Pwm,Velocity_Pwm,Turn_Pwm;        //PID计算的PWM值
int  Motor1, Motor2;                  //左右电机PWM值
int  Encoder_left, Encoder_right;     //检测速度
float Movement = 0;                  //速度调节  
int  Contrl_Turn = 64;                //转向调节变量
int  Distence ;                       //小车和前方障碍物之间的距离
struct tCCD  CCD;                      //摄像头的数据


//环境数据采集任务
void Car_Task_200HZ(void)
{
		static struct mpu6050_data Last_Data;
	
		if(mpu_dmp_get_data() !=0 )
			OutMpu = Last_Data;
		else
			 Last_Data = OutMpu;
			
}

void Car_Task_100HZ(void)
{
	//启动超声波模块检测
	HC_SRC04_Start();
	
	Encoder_left  = Read_Encoder(1);
	Encoder_right = -Read_Encoder(2);
	
	//1、确定直立环PWM
	
		Balance_Pwm = Vertical_Ring_PD(OutMpu.pitch, OutMpu.gyro_x);
	
	//2、确定速度环PWM
	
	  Velocity_Pwm = Vertical_speed_PI(Encoder_left,Encoder_right,OutMpu.pitch, Movement );
	
	
	//3、确定转向环PWM
	
	  if(FS_MODE == 0)       //遥控模式
			Turn_Pwm = Vertical_turn_PD(Contrl_Turn, OutMpu.gyro_z);
		else if(FS_MODE == 1)  //蔽障模式
		{
			if(Distence < 20)
					Turn_Pwm = Vertical_turn_PD(20, OutMpu.gyro_z);
			else
				 Turn_Pwm = 0;
		}
		else if(FS_MODE == 2)  //巡线模式
		{
			   Turn_Pwm = Vertical_turn_PD(CCD.middle, OutMpu.gyro_z);
		}
	
	//4、确定最终左右电机的PWM
		Motor1 = Balance_Pwm + Velocity_Pwm + Turn_Pwm;
	  Motor2 = Balance_Pwm + Velocity_Pwm - Turn_Pwm;
	
		PWM_Limiting(&Motor1,&Motor2);
	
	
	//5、设置电机
		Set_PWM(Motor1,Motor2);
}


void Car_Task_5HZ(void)
{
//		printf("acc_x = %d\n",OutMpu.acc_x);
//		printf("acc_y = %d\n",OutMpu.acc_y);
//		printf("acc_z = %d\n",OutMpu.acc_z);
//		printf("gyro_x = %d\n",OutMpu.gyro_x);
//		printf("gyro_y = %d\n",OutMpu.gyro_y);
//		printf("gyro_z = %d\n",OutMpu.gyro_z);
//	  printf("pitch = %f\n",OutMpu.pitch);
//	  printf("roll = %f\n",OutMpu.roll);
//	  printf("yaw = %f\n",OutMpu.yaw);
	
//	  printf("Encoder_left = %d\n",Encoder_left);
//  	printf("Encoder_left = %d\n",Encoder_right);
	
//		printf("Distence = %d\n",Distence);
	
		printf("CCD.left = %d\n",CCD.left);
	  printf("CCD.right = %d\n",CCD.right);
	  printf("CCD.middle = %d\n",CCD.middle);
		printf("CCD.threshold = %d\n",CCD.threshold);
	  printf("\r\n");
}

//启动超声波检测
void  HC_SRC04_Start(void)
{
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_SET);
	delay_us(20);
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_0, GPIO_PIN_RESET);
	
}

cartask.h

#ifndef __CAR_TASK_H
#define __CAR_TASK_H
#include "stm32f4xx_hal.h"


extern int  FS_MODE  ;                      //0、遥控模式   1、蔽障模式  2、巡线模式 
extern int  Balance_Pwm,Velocity_Pwm,Turn_Pwm;        //PID计算的PWM值
extern int  Motor1, Motor2;                  //左右电机PWM值
extern int  Encoder_left, Encoder_right;     //检测速度
extern float Movement ;                  //速度调节  
extern int  Contrl_Turn ;                //转向调节变量
extern int  Distence ;                       //小车和前方障碍物之间的距离




struct mpu6050_data{
	
		short acc_x;
		short acc_y;
		short acc_z;
		
		short gyro_x;
		short gyro_y;
		short gyro_z;
	
		float pitch;    //俯仰角
	  float roll;     //翻滚角
	  float yaw;      //偏航角
};

extern struct mpu6050_data OutMpu;



struct tCCD
{
	uint16_t middle;      //中间位置值
	uint16_t threshold;   //像素ad阈值
	uint16_t left;        //左跳变的位置
	uint16_t right;       //右跳变的位置
};

extern struct tCCD  CCD;




extern  int  Distence ;                       //小车和前方障碍物之间的距离


void Car_Task_200HZ(void);
void Car_Task_100HZ(void);
void Car_Task_5HZ(void);

void  HC_SRC04_Start(void);



#endif

总结

以上为APP和平衡车之间的交互,等所有工程实现完毕,会在最后一篇博客上放入源码链接

标签:树莓,避障,END,Freertos,void,BEGIN,CODE,IRQn,USER
来源: https://blog.csdn.net/qq_44691051/article/details/113781495