目录
一、控制系统设计
一个系统产品的制作，要细分为多个模块，分模块进行工作与进行调试。对平衡小车进分析得到如下的架构图。
|
图1-1 平衡小车初步系统设计图 |
分析平衡小车在跷跷板上平衡需要控制的物理量：
|
1.1机械结构设计
在实现小车机械结构的设计过程中，我们考虑了若干种方案。最终在四轮，三轮中选择了以前轮控制转向的三轮车方案。
四轮的车身稳定，符合主流，但是受限于材料，无法轻易搭建悬挂机构，因此必须考虑差速控制转向。但两个舵机的速度不能保证完全一致，且转向时会产生较大摩擦阻力，还会增加程序的设计难度，因此我们没有采用这种方案。
三轮可能存在侧翻问题，但设计时将重心压得很低，因此不存在这种情况。同时仅用两个圆周舵机，拆装方便，易于控制，结构稳定，转向时后轮的摩擦阻力也可以忽略不计。因此我们最终采用了这种方案。
对平衡小车需要的机械结构进行分析得到下图：
|
小车实现的运动：通过左右电机的调整以及前左右轮的转速控制就可以实现方向调整。
传感器和执行器的安装位置：传感器的安装位置要远离电机，因为电机在运动过程中会产生电磁干扰，对传感器正常测量数据会有一定的影响；同时执行器需要安装在小车外形结构的预留位置，方便排线布局。
执行器控制机械结构运动示意图：
图1-4 执行器控制电机转动示意图
机械结构的设计要求如图1-5所示。
| 图1-5 机械结构设计要求
|
1.2传感系统设计
本次实验中我们主要使用了GY953九轴传感器，其工作原理，是通过陀螺仪与加速度,磁场传感器经过数据融合算法最后得到直接的角度数据。该传感器具有体积小、高性价比、支持串口通信格式 和SPI 通信格式的特点。
传感系统主要设计要求包括以下两点。第一，跷跷板的摆动角度很小，因此传感系统所得数据需要非常精确，且噪声不能太大；第二，跷跷板摆动速度很快，姿态数据变化也很快，所以传感系统数据传输的速度不能太慢。通过设置GY9531的相关参数，这些要求很容易实现。GY953相关技术参数如表1-1所示。
下面是九轴传感器的包含的各个模块以及测量的物理量数据类型：
 |
|
图1-6 传感器功能
表 1-1 GY953技术参数
1.3执行器设计
在整体的硬件性能上，我们提出以下要求：方便转动方向的调节；保证足够的爬坡能力，并且不发生打滑；保证小车以当前方向行驶时不发生偏移或偏移量较少。基于硬件要求，我们提出以下三种方案:
方案一：前轮转向，后轮驱动。执行器选择一个180°舵机和一个360°舵机。其中，180°舵机控制前轮转向，360°舵机通过齿轮与轴控制后轮转动。
方案二：前轮转向，后轮驱动，执行器选择一个180°舵机和两个360°舵机。其中，180°舵机控制前轮转向，两个360°舵机分别单独控制对应后轮的转动。
方案三：后轮驱动，差速转向。执行器选择两个360°舵机，分别单独与左右后轮相连。
经比较，方案一的优点在于调节转动方向的灵敏性和行驶时较小的方向偏移，但爬坡能力相对较弱。方案二弥补了方案一驱动能力不足的缺点，但由于左右舵机在制造时的差异，难以保证两个后轮转速相同，从而存在一定的行驶方向偏移。方案三使用差速转向，小车转向性能受物理环境影响较大，但调整方向的复杂性略低于方案一和方案二。最后，我们决定使用方案三，通过优化小车的机械机构来减轻驱动的负载。
在使用执行器（舵机）时，我们需要对两个360°舵机通过PWM的占空比单独控制它的转速和转向。
图1-7 所使用的舵机示意图
1.4控制算法设计
我们的控制器选用单片机STM32F401，控制的物理量主要是PWM信号该控制器的主要功能是：发送指令给九轴传感器，接收九轴传感器发送过来的数据，对数据进行处理，产生PWM信号。
小车寻找跷跷板平衡的控制系统框图如图1-8所示。传感器用来测量当前小车的姿态参数，由控制器进行数据的处理，输出数据控制舵机的转动方向和速度，以此控制小车的前进、后退及其速度，最终由小车在跷跷板上的运动，控制跷跷板摆动，此时小车也随着跷跷板摆动，再将角度数据反馈给传感器。最终实现跷跷板的平衡。
|
|
|
图1-8 控制系统框图
基于控制系统框图，我们对控制系统提出以下性能要求：响应速度较快；超调量较小；震荡次数较少。由于跷跷板触地对整个系统的影响较大，所以控制系统对响应速度和超调量具有较高要求。
在采用1.3中的方案三作为执行器方案后，控制系统将受到以下外界干扰：跷跷板转动时惯性的影响，跷跷板转动轴摩擦力的影响等，同时，传感器在使用过程中会随时间产生较大的漂移量。惯性和摩擦的影响可以通过提高响应速度和减小超调量来削弱，传感器的漂移可以通过滤波算法进行校正，
控制算法分为两部分，传感器信号处理与舵机驱动。由于传感器在测量数据时产生漂移，我们使用滤波算法进行削弱其影响，其中，常见的滤波算法有均值滤波、互补滤波、卡尔曼滤波等。
均值滤波的优点在于计算简单，通过计算多次数据的平均值，将平均值作为真实值。
互补滤波是通过基于传感器对角度、角速度、加速度的测量灵敏性不同而设计的滤波算法。由于加速度计的低频特性较好，加速度计的角度可以直接得出，所以没有累计误差，长时间内性能较好，可以通过高通滤波来抑制低频。而陀螺仪的角速度积分由于误差的积累，长时间的性能较差而短时间内性能较好，通过低通来抑制高频。互补滤波则需要选择低通与高通的切换频率。互补滤波的公式为
卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。卡尔曼滤波需要对系统进行建模，同时需要干扰噪声的方差等数据。
比较三种滤波方法，但无法达到理想滤波效果。卡尔曼滤波较为精确，但对建模的精确度较高，受模型准确性的影响较大。同时，噪声方差等数值的要求难以满足，设计难度较大。互补滤波具有较好的滤波效果而均值滤波计算简单。综合考虑，我们决定先使用均值滤波作为传感器信号处理的滤波算法，后期拓展时会采用互补滤波。
在驱动部分中，我们使用PID算法作为调平衡的核心算法。在PID算法中，定值和实际输出值构成控制偏差，将偏差按比例、积分和微分通过线性组合构成控制量，对被控对象进行控制。控制算法的流程图如图1-8所示。
图1-8 控制算法的流程图
二、控制系统的制作与调试
2.1机械结构的制作与调试
小车骨架的搭建：
| 图2-1 小车骨架外观图
|
小车运动件的安装：
| 图2-2 小车运动件的位置示意图
|
小车成品样貌：
图2-3 小车成品样貌图
2.2电路系统的制作与调试
电路系统的搭建时，首先遇到的问题是引脚不够的问题。这是由于STM32开发板的5V和GND引脚只有2个，而设计的电路中传感器和两个圆周舵机都需要一个5V供电。为了解决这个问题，我们利用洞洞板将STM32开发板的5V引脚通过杜邦线引出至其中一组排针，GND引脚引出至另一组排针。传感器、圆周舵机的对应引脚分别接到两组排针上，达到拓展引脚的目的。在实际测试时，也遇到了杜邦线因小车震动而脱落、杜邦线排布混乱的问题。因此，我们获得的经验就是电路系统的排线一定要牢固可靠，防止后期出现问题，浪费时间检查。
另外，我们遇到了电机无法旋转的问题，后来经过老师的讲解了解到是电机死区的问题
图2-4 电机死区示意图
下面是在电路系统的测试过程中遇到的问题以及解决方案的总结。
表2-1 电路系统的问题以及解决方案
| 电路系统遇到的问题 |
问题描述 |
解决方案 |
| 电机死区问题 |
在一定脉冲宽度下，电机无法转动 （与理论存在差异） |
不断测量找出电机死区的临界值，经过我们的测量每个电机的死区都不一样，正反转的死区也不是对称的，我们电机死区的具体数据如下：一个电机是[-50,20]，一个电机是[-50,50]，所以我们控制电机转动的时候避开电机的死区。 |
| 电机转速不一致问题 |
两个电机在同一脉冲宽度下，转速不一致 |
试验调整两个电机的转速差，我们发现在正转的时候，在同样的脉冲宽度时，左边电机比右边电机快，我们测量得到两个电机固定的脉冲宽度差来矫正。对于反转的情况，两个电机在测量误差允许范围内同样脉冲宽度下转速一致。 |
| 电脑串口读不到传感器数据 |
串口时不时读不到传感器数据（通过控制器作为转发情况下）九轴传感器的工作电压为5~12V，由于电池电源经过DC-DC后变为5V，但是实际中可能会低于5V，传感器无法发送数据。 |
直接用电池电源给传感器供电，不经过DC-DC |
经过不断调试和修改，最终我们设计的电路系统外观和架构如图2-4和图2-5所示。
图2-5 电路系统外观图
图2-6 电路系统架构图
2.3控制程序的编写与调试
控制程序的模块架构图如下：
| 图8.控制程序模块结构图
|
下面是每个模块的实现代码：
| StartTaskUART1GY953（包含给九轴传感器发送指令，接收九轴传感器数据，对传感器数据进行处理，PID运算等） |
||||||
|
|
||||||
| StartTaskMotorCtrl（包含根据PID响应对电机转速进行调整，平衡指示灯闪烁，平衡一定时间后退等） |
||||||
|
编写程序之后，对应于响应的目的，我们会打印出输入和输出到电脑串口调试助手上进行分析和调试。出现的问题以及解决方案：
表2-2 编写程序过程中遇到的问题以及解决方案
| 编写程序遇到的问题 |
问题描述 |
解决方案 |
| StartTaskMotorCtrl与StartTaskUART1GY953运行同步问题 |
在起初编写好程序之后，分析代码没有问题，但是电机无法根据传感器数据进行运动，只会时不时的运动 |
分析问题之后，发现是两个函数的osDelay(time)不一致，以至于采样率不一致出现了问题。解决此问题只需要两个函数采样率一致就可以。 |
三、控制系统的测试与分析
3.1测试方法
评价小车性能有以下几个要素：调平衡的次数（前后移动一次为调平衡一次）；调平衡的时间；调平衡总过程跷跷板平均倾角。其中，行驶角度调整时间代表180°舵机的灵敏性；调平衡的次数反映系统的震荡特性；调平衡的时间反映系统的综合性能；平均倾角反映系统的超调量。为测试滤波效果，测试工作将在滤波和不滤波两种条件下进行。
3.2测试数据与现象
九轴传感器接收的欧拉角中的pitch角如图3-1所示。
|
|
图3-1 九轴传感器接收欧拉角示意图使用串口调试助手查看数据变化如图3-2所示。
| 图
|
小车平衡过程：
| 图
|
3.3结果分析
1）根据传感器状态调整运动状态：
由于我们小组在实际演示中只完成了小车在跷跷板上前进使跷跷板平衡这个最基本的任务，因此，我们只用到了九轴传感器的欧拉角数据，具体为pitch值。当pitch值为负值时，判定小车为‘头高尾低’状态，即未达到平衡点，故小车会前进；当pitch值为正时，判定小车为‘头低尾高’的状态，即已经超过平衡点，故小车会后退。而当角度进入所设定的平衡角度范围内时，判定为小车已找到平衡点，电机会停止转动，小车静止。
表3-1滤波前后效果对比
| 当使用未滤波的数据作为参考来控制小车运动时，可以发现传感器的数据偶尔会有较大的抖动，当这些数据进入运算之后，会导致小车偶尔抽搐，并不符合我们的预期。 |
当使用滤波后的数据作为参考之后，不再出现数据抖动，由此数据可以更好的控制小车的移动，不会让小车出现偶尔抽搐的现象，小车能够更好的实现移动和判断。 |
2）小车会在平衡角度范围内抽搐：
由于我们采用pid算法控制，而pid算法最后实现的理想结果是小车的运动收敛，即小车停止在平衡点且跷跷板平衡。而由于跷跷板轴承有摩擦、小车并没有走标准的直线等因素的影响，小车很难达到完美的理想状态，以小车向后退到达平衡点为例，由于轴承的摩擦等因素，跷跷板并不会立即产生角度变化，而是当小车走过平衡点后，给跷跷板一个更大的力，跷跷板才会变动。而由于pid算法对跷跷板角度变化的预测，小车会立即向前移动。同理，当向前到达平衡点时，跷跷板不会立刻产生反应，而是当经过平衡点后，跷跷板才会变动。由于算法对跷跷板运动的预测，小车会立即向后运动，如此反复。最后表现为小车在平衡角度范围内抽搐。
3）平衡十秒后倒车：
当小车检测到进入平衡角度后，time_delay函数开始计时。当计时未达到10秒前平衡状态被破坏，则time_delay清零，等到再次达到平衡状态后time_delay开始计时。当time_delay计时到达十秒后，直接控制小车后退，到小车退出跷跷板。
附录：main.c
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2022 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 "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef union {
uint16_t U16Data;
int16_t I16Data;
uint8_t C8Data[2];
}DataJointUnion;
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define UART1_RECV_BUFF_LEN 11
#define UART1_TRANS_BUFF_LEN 64
#define UART2_SEND_BUFF_LEN 64
#define time 100
#define M_PI 3.1415926
#define M_COS(x) cos((x*M_PI)/180)
#define M_SIN(x) sin((x*M_PI)/180)
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim2;
UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
osThreadId defaultTaskHandle;
osThreadId myTask02Handle;
osThreadId myTask03Handle;
osThreadId myTask04Handle;
osSemaphoreId myBinarySem_UART1GY953RecvHandle;
osSemaphoreId myBinarySem_UART1ToUART2TransReadyHandle;
osSemaphoreId myBinarySem_UART2SendHandle;
/* USER CODE BEGIN PV */
uint8_t UART1RecvBuff[UART1_RECV_BUFF_LEN];
uint8_t UART1TransToUART2Buff[UART1_TRANS_BUFF_LEN];
uint16_t UART1TransToUART2Size=0;
uint16_t UART2SendSize=0;
uint8_t UART2SendBuff[UART2_SEND_BUFF_LEN];
//int16_t spdleft=500;
//int16_t spdright=500;
int16_t deta_data=0;
int16_t pitch_deta=0;
int16_t spdleft=50;
int16_t spdright=50;
int16_t pitch_atti=0;
int16_t time_delay=0;
int16_t speed0=50;
int16_t pitch_times = 0; //p角同号次数
int16_t pitch_times_ctl = 1000; //平衡控制
int16_t xiangying_pid=0;//PID控制器控制量
const int FilterOrder = 21;
const double FilterHn[21] = {
-0.01299253964226, -0.0150332929185, -0.01207107368107,-0.002207438432842,
0.01529274896489, 0.03957521406373, 0.06812913211038, 0.09713578873324,
0.1221778815001, 0.13915124264, 0.1451573563434, 0.13915124264,
0.1221778815001, 0.09713578873324, 0.06812913211038, 0.03957521406373,
0.01529274896489,-0.002207438432842, -0.01207107368107, -0.0150332929185,
-0.01299253964226
};
double MagnXRawDataFIFO[21];
double MagnYRawDataFIFO[21];
double MagnZRawDataFIFO[21];
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM2_Init(void);
void StartDefaultTask(void const * argument);
void StartTaskUART1GY953(void const * argument);
void StartTaskUART2(void const * argument);
void StartTaskMotorCtrl(void const * argument);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* 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_USART1_UART_Init();
MX_USART2_UART_Init();
MX_TIM2_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* Create the semaphores(s) */
/* definition and creation of myBinarySem_UART1GY953Recv */
osSemaphoreDef(myBinarySem_UART1GY953Recv);
myBinarySem_UART1GY953RecvHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART1GY953Recv), 1);
/* definition and creation of myBinarySem_UART1ToUART2TransReady */
osSemaphoreDef(myBinarySem_UART1ToUART2TransReady);
myBinarySem_UART1ToUART2TransReadyHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART1ToUART2TransReady), 1);
/* definition and creation of myBinarySem_UART2Send */
osSemaphoreDef(myBinarySem_UART2Send);
myBinarySem_UART2SendHandle = osSemaphoreCreate(osSemaphore(myBinarySem_UART2Send), 1);
/* 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 */
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* definition and creation of myTask02 */
osThreadDef(myTask02, StartTaskUART1GY953, osPriorityIdle, 0, 128);
myTask02Handle = osThreadCreate(osThread(myTask02), NULL);
/* definition and creation of myTask03 */
osThreadDef(myTask03, StartTaskUART2, osPriorityIdle, 0, 128);
myTask03Handle = osThreadCreate(osThread(myTask03), NULL);
/* definition and creation of myTask04 */
osThreadDef(myTask04, StartTaskMotorCtrl, osPriorityIdle, 0, 128);
myTask04Handle = osThreadCreate(osThread(myTask04), NULL);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
/* Start scheduler */
osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* 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};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);
/** 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.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 25;
RCC_OscInitStruct.PLL.PLLN = 160;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
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();
}
}
/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 80-1;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 20000-1;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 500;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* @brief USART1 Initialization Function
* @param None
* @retval None
*/
static void MX_USART1_UART_Init(void)
{
/* USER CODE BEGIN USART1_Init 0 */
/* USER CODE END USART1_Init 0 */
/* USER CODE BEGIN USART1_Init 1 */
/* USER CODE END USART1_Init 1 */
huart1.Instance = USART1;
huart1.Init.BaudRate = 115200;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */
}
/**
* @brief USART2 Initialization Function
* @param None
* @retval None
*/
static void MX_USART2_UART_Init(void)
{
/* USER CODE BEGIN USART2_Init 0 */
/* USER CODE END USART2_Init 0 */
/* USER CODE BEGIN USART2_Init 1 */
/* USER CODE END USART2_Init 1 */
huart2.Instance = USART2;
huart2.Init.BaudRate = 115200;
huart2.Init.WordLength = UART_WORDLENGTH_8B;
huart2.Init.StopBits = UART_STOPBITS_1;
huart2.Init.Parity = UART_PARITY_NONE;
huart2.Init.Mode = UART_MODE_TX_RX;
huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart2.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_13, GPIO_PIN_RESET);
/*Configure GPIO pin : PC13 */
GPIO_InitStruct.Pin = GPIO_PIN_13;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
}
/* USER CODE BEGIN 4 */
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart == &huart1)
{
osSemaphoreRelease(myBinarySem_UART1GY953RecvHandle);
HAL_UART_Receive_IT(&huart1, UART1RecvBuff, UART1_RECV_BUFF_LEN);
}
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
if(huart == &huart2)
{
osSemaphoreRelease(myBinarySem_UART2SendHandle);
}
}
void MotorCtrl(uint8_t chn, int16_t spd)
{
if(spd*speed0<0){
pitch_times_ctl = pitch_times;
pitch_times = 0;
}
if(spd>1000){spd = 1000;}
if(spd<-1000){spd = -1000;}
switch(chn)
{
case 0 :
spd = spd + 1500;
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, spd); //范围500~2500，对应0.5ms~2.5ms
break;
case 1 :
spd = -1 * spd + 1500;
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, spd); //范围500~2500，对应 ????????????0.5ms~2.5ms
break;
default:break;
}
speed0 = spd;
}
double FilterCalculation(double newRawData, double *prawDataFIFO, double *pFirHn, int firOder) {
int i;
double result;
i = firOder-1;
while(i) {
prawDataFIFO[i] = prawDataFIFO[i-1];
i--;
}
prawDataFIFO[0] = newRawData;
result = 0;
for(i=0; i<firOder; i++) {
result = result + pFirHn[i]*prawDataFIFO[i];
}
return result;
}
/* USER CODE END 4 */
/* USER CODE BEGIN Header_StartDefaultTask */
/**
* @brief Function implementing the defaultTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void const * argument)
{
/* USER CODE BEGIN 5 */
/* Infinite loop */
for(;;)
{
osDelay(1);
}
/* USER CODE END 5 */
}
/* USER CODE BEGIN Header_StartTaskUART1GY953 */
/**
* @brief Function implementing the myTask02 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskUART1GY953 */
void StartTaskUART1GY953(void const * argument)
{
/* USER CODE BEGIN StartTaskUART1GY953 */
uint8_t cmd0[]={0xA5,0x15,0xBA};// acceleration data
uint8_t cmd1[]={0xA5,0x35,0xDA};// magnet data
uint8_t cmd2[]={0xA5,0x45,0xEA};// Euler data(50Hz)
uint16_t accx0 = 0;
int16_t pitch0=0; //欧拉角初始pitch
DataJointUnion tempUnion;
double firResult;
int16_t pitch_deta_1=0;
uint8_t isDirectTransToUART2=0; //1将传感器标准数据帧直接传到电脑，0表示会将数据进行处理
osDelay(1000);
//HAL_UART_Transmit(&huart1, cmd0, sizeof(cmd0), 100); //获取角速度数据
//HAL_UART_Transmit(&huart1, cmd1, sizeof(cmd1), 100); //获取地磁角数据
HAL_UART_Transmit(&huart1, cmd2, sizeof(cmd2), 100); //获取欧拉角数据
//-----------------------------------------------------------------
//pitch角度的PID控制
int16_t Kp_pitch=10;
int16_t Ki_pitch=0;
int16_t Kd_pitch=120;
//--------------------------------------------------------------------
HAL_UART_Receive_IT(&huart1, UART1RecvBuff, UART1_RECV_BUFF_LEN); //uart1接收传感器数据
/* Infinite loop */
for(;;)
{
osSemaphoreWait(myBinarySem_UART1GY953RecvHandle, osWaitForever);
UART1TransToUART2Size = 0;
if(isDirectTransToUART2) {
memcpy(UART1TransToUART2Buff, UART1RecvBuff, UART1_RECV_BUFF_LEN);
UART1TransToUART2Size = UART1_RECV_BUFF_LEN;
} else {
switch(UART1RecvBuff[2]) {
case 0x15 : // 处理角速度数据
tempUnion.C8Data[1] = UART1RecvBuff[4];
tempUnion.C8Data[0] = UART1RecvBuff[5];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ax:%05d ", tempUnion.U16Data);
// if(abs(accx0-tempUnion.U16Data)>150){
deta_data=accx0-tempUnion.U16Data;
if(deta_data>0)
{
deta_data=deta_data;
}
else{
deta_data=-deta_data;
}
accx0 = tempUnion.U16Data;
UART1TransToUART2Size = UART1TransToUART2Size + 9;
tempUnion.C8Data[1] = UART1RecvBuff[6];
tempUnion.C8Data[0] = UART1RecvBuff[7];
//sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ay:%05d ", deta_data);
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Ay:%05d ", tempUnion.U16Data);
UART1TransToUART2Size = UART1TransToUART2Size + 9;
tempUnion.C8Data[1] = UART1RecvBuff[8];
tempUnion.C8Data[0] = UART1RecvBuff[9];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Speed:%05d \r\n",tempUnion.U16Data);
UART1TransToUART2Size = UART1TransToUART2Size + 9 + 2;
break;
case 0x35 : //处理获取地磁角数据
tempUnion.C8Data[1] = UART1RecvBuff[4];
tempUnion.C8Data[0] = UART1RecvBuff[5];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Mx:%06d ", tempUnion.I16Data);
UART1TransToUART2Size = UART1TransToUART2Size + 10;
tempUnion.C8Data[1] = UART1RecvBuff[6];
tempUnion.C8Data[0] = UART1RecvBuff[7];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "My:%06d ", tempUnion.I16Data);
UART1TransToUART2Size = UART1TransToUART2Size + 10;
tempUnion.C8Data[1] = UART1RecvBuff[8];
tempUnion.C8Data[0] = UART1RecvBuff[9];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Mz:%06d \r\n", tempUnion.I16Data);
UART1TransToUART2Size = UART1TransToUART2Size + 10 + 2;
break;
case 0x45 : //处理欧拉角数据
pitch_times++;
tempUnion.C8Data[1] = UART1RecvBuff[4];
tempUnion.C8Data[0] = UART1RecvBuff[5];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "x:%06d ", time_delay);
UART1TransToUART2Size = UART1TransToUART2Size + 9;
tempUnion.C8Data[1] = UART1RecvBuff[6];
tempUnion.C8Data[0] = UART1RecvBuff[7];
//firResult = FilterCalculation((double)tempUnion.I16Data, MagnXRawDataFIFO, FilterHn, FilterOrder);
//sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "P:%06d ", tempUnion.I16Data);
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "P:%06d ", tempUnion.I16Data);
//tempUnion.I16Data=(int)firResult;
//pitch0=tempUnion.I16Data;
//firResult = FilterCalculation((double)tempUnion.I16Data, MagnXRawDataFIFO, FilterHn, FilterOrder);
pitch_atti=tempUnion.I16Data;
pitch_deta=tempUnion.I16Data-pitch0; //当前的差值
xiangying_pid=Kp_pitch*pitch_deta+Kd_pitch*(pitch_deta-pitch_deta_1)/time;//PID_xiangying
//下面code记录上次数据 （误差和具体数值）
pitch0=tempUnion.I16Data;
pitch_deta_1=pitch_deta;//记录当前的差值，以便下次使用
UART1TransToUART2Size = UART1TransToUART2Size + 9;
tempUnion.C8Data[1] = UART1RecvBuff[8];
tempUnion.C8Data[0] = UART1RecvBuff[9];
sprintf((UART1TransToUART2Buff + UART1TransToUART2Size), "Y:%06d \r\n", xiangying_pid);
UART1TransToUART2Size = UART1TransToUART2Size + 9 + 2;
break;
}
}
osSemaphoreRelease(myBinarySem_UART1ToUART2TransReadyHandle);
osDelay(time);
}
/* USER CODE END StartTaskUART1GY953 */
}
/* USER CODE BEGIN Header_StartTaskUART2 */
/**
* @brief Function implementing the myTask03 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskUART2 */
void StartTaskUART2(void const * argument)
{
/* USER CODE BEGIN StartTaskUART2 */
HAL_UART_Transmit_IT(&huart2, "Ready!", 6); //ready对应 52 65 61 64 79 21
/* Infinite loop */
for(;;)
{
osSemaphoreWait(myBinarySem_UART1ToUART2TransReadyHandle, osWaitForever);
osSemaphoreWait(myBinarySem_UART2SendHandle, 100);
memcpy(UART2SendBuff, UART1TransToUART2Buff, UART1TransToUART2Size);
UART2SendSize = UART1TransToUART2Size;
HAL_UART_Transmit_IT(&huart2, UART2SendBuff, UART2SendSize); //uart2将uart1收到的数据发到电脑
osDelay(1);
}
/* USER CODE END StartTaskUART2 */
}
/* USER CODE BEGIN Header_StartTaskMotorCtrl */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTaskMotorCtrl */
void StartTaskMotorCtrl(void const * argument)
{
/* USER CODE BEGIN StartTaskMotorCtrl */
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
/* Infinite loop */
// uint8_t flag = 0; //量程判断
// int16_t step = 500; //步长
uint8_t flag = 0; //量程判断
int16_t speed_deta=30;
for(;;)
{
if(time_delay<5000)
{
if((pitch_atti<=-10||pitch_atti>800)||(xiangying_pid>8||xiangying_pid<-8))
{
if(pitch_atti<=140) //前期前进
{
time_delay=0;
spdleft=-68;
spdright=-68;
}
else if(pitch_atti>=908){
time_delay=0;
spdleft=60;
spdright=80;
}
else //上去突变之后后退
{
spdleft=0.3*xiangying_pid;
spdright=0.3*xiangying_pid;
if(spdleft<15&&spdright<50&&spdleft>=0&&spdright>=0)
{
time_delay=time_delay+time;
HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
}
if(spdleft<0&&spdleft>-50&&spdright>-50&&spdright<0)
{
time_delay=time_delay+time;
HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
}
}
}
else
{
spdleft=0;
spdright=0;
time_delay=time_delay+time;
HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
}
}
else
{
spdleft=80;
spdright=100;
}
// if(deta_data>1500)
// {
// int16_t step = 100; //步长
// if (flag == 0) {
// spdleft =spdleft+step;
// spdright =spdright+step;
// if (spdleft >=1000) {
// spdleft = 1000;
// flag = 1;
// }
// if (spdright >=1000) {
// spdright = 1000;
// flag = 1;
// }
// } else {
// spdleft = spdleft-step;
// spdright = spdright-step;
// if (spdleft <= -1000) {
// spdleft = -1000;
// flag = 0;
// }
// if (spdright <= -1000) {
// spdright = -1000;
// flag = 0;
// }
// }
// }
// //osSemaphoreWait (myBinarySem_KeyPressHandle, osWaitForever);
//HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
// spdleft=15;
// spdright=50;
if(spdleft>0&&spdright>0)
{
MotorCtrl(0,spdleft+speed_deta);
MotorCtrl(1,spdright);
}
else
{
MotorCtrl(0,spdleft);
MotorCtrl(1,spdright);
}
//倒转 0比1大30
//正转 0比1小8
osDelay(time);
}
/* USER CODE END StartTaskMotorCtrl */
}
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM1 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM1) {
HAL_IncTick();
}
/* USER CODE BEGIN Callback 1 */
/* USER CODE END Callback 1 */
}
/**
* @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 */