综合分析

麦克纳姆轮速度合成公式

从右前开始，逆时针转一圈为这个公式里的1 2 3 4号

从这个公式里可以看出，麦克纳姆轮底盘有一个特点就是，可以朝任意方向移动，而我们小车的定为是由前方和侧方两个激光测距实现，也就是小车在场地上的x、y轴坐标我们可以实时的已知。

那么小车从A走到B，只需要知道任意时刻小车的坐标(x,y)和目标位置(x0,y0)，小车可以直接沿着两点的连线走过去，且这个过程是动态调整的。

简单思考一下，要实现这个功能还有三个注意事项：
1.小车的Vx和Vy不能是一个固定值，需要是一个梯形的形状，这样可能会稳定
2.第二点就是要保证x、y轴坐标的有效，小车的姿态必须是固定的，因此陀螺仪的校准程序必须无懈可击
3.速度需要实时更新

陀螺仪实时校准

根据速度合成公式，可以看出w作用在1、4和2、3两组电机上，并没有对每个电机单独作用，假设小车没有Vx和Vy的速度，只有w，那么可以得到一个固定的V1~V4的值，但是如果考虑到直流电机给固定的PWM但是实际的电机有差异，速度不一样，或者是小车轮子出现了打滑的情况，这样小车就有了Vx和Vy的分速度，这个速度如何弥补呢？

目前想到的解决方案只有编码器，但是做项目不能一口气做的太复杂，编码器我认为是一个比较复杂的东西，如果本阶段任务完成后，可以回学校和队友们一起研究编码器的问题。

我的想法是对w进行一个PI的控制。

组织任务

工训物流车是由一个接着一个任务工作的，那么任务之间的组织是程序的一个大主干，如何组织是一个很复杂的问题。

为了厘清思路，从功能入手来设计程序结构，我们列几个程序需要实现的功能：
1.可以简单的增添和删除新的任务，即任务模块化
2.可以在两个任务中间增加或者删除任务，不影响任务的连贯性
3.可以实现任务与任务之间的延迟，类似delay
4.任务完成后需要有完成标志，并指向下一个任务。

根据以上四点，我们可以把任务想象成一间间的屋子，屋子里有需要做的任务，且门口上挂有牌子。小明是一个打工人，他的任务就是进入指定的屋子完成任务，任务完成后会得知下一间屋子是哪个，再从口袋里拿出一张张地图，这一张张地图包括每间屋子的路线，从这些地图里找到下一个屋子的路线然后走过去，接着重复。那么如何做delay呢？我们可以设置一间什么任务都没有的小黑屋，让小明走进去，再走出来，再走进去，每次走要消耗时间，我们在一开始告诉小明要进去几次，就能保证delay了。

分析总结

我们需要一个定时器中断、一个小车底盘的速度控制程序，一个w的PI控制程序，在定时器中断里对两个激光测距和陀螺仪的数据进行处理，再在while里进行速度的分配。

定时器中断

配置CubeMx

使用定时器2作为定时中断的定时器，配置如下：

keil中添加处理函数

//启动定时器中断模式计数HAL_TIM_Base_Start_IT(&htim2);找到弱符号周期运行回调函数原型，并在tim.c中自定义该回调函数
__weak void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)

测试代码

HAL_TIM_Base_Start_IT(&htim2);void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{if(htim->Instance == TIM2){printf("hello!\r\n");}
}

成功！

底盘速度控制程序

获取系统时钟

对Angle角度要进行求导，获得dw，以备不时之需，故需要获得系统时间：

unsigned int sysclock = 0;  
unsigned int lastsysclock = 0;
unsigned int dt=0;void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{if(htim->Instance == TIM2){lastsysclock=sysclock;sysclock = HAL_GetTick();//返回从系统运行开始经过的时间，默认情况下单位为msdt=sysclock-lastsysclock;printf("%d\r\n",dt);}
}

成功！

跨文件变量调用

所有有关速度的函数都被我放置在了一个叫“Auto_Control.c”的文件里，这里应该算是偷了个懒，不愿意改之前写好的小框架了，所以Auto_Control大量的调用了main.c里定义的全局变量，其实这样的话并不好。调用方法用了关键字extern法，在main.c里定义变量：

float dis1;
float dis2;
float Angle=0,lastAngle=0;unsigned int dt=0;float x,y;
float last_x,last_y;float V1=0,V2=0,V3=0,V4=0;float w;float Vx=0,Vy=0;bool mission_complete=false;

然后再在Auto_Control.c里用关键字法再定义一次:

extern float dis1;
extern float dis2;
extern float Angle,lastAngle;extern unsigned int dt;extern float x,y;
extern float last_x,last_y;extern float V1,V2,V3,V4;extern float w;extern float Vx,Vy;extern bool mission_complete;

这样的话就可以直接对这些变量进行操作了。

更改电机控制程序

下列文件都在moto.c里

老版的电机控制程序：

void control(int moto, int pwm1, int pwm2)
{if(moto==1){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, pwm1);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2, pwm2);    //修改比较值，修改占空比pwm2/7200}if(moto==2){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_3, pwm1);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm2);    //修改比较值，修改占空比pwm2/7200}if(moto==3){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_1, pwm1);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_2, pwm2);    //修改比较值，修改占空比pwm2/7200}if(moto==4){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, pwm1);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, pwm2);    //修改比较值，修改占空比pwm2/7200}

有一个很关键的确定就是当不能很直观的控制速度的正负，因此改成了下面这个：

新版的电机控制程序：

void drive(int moto,float speed)
{if(moto==1){if(speed>=0){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, 0);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2, speed);    //修改比较值，修改占空比pwm2/7200}if(speed<0){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_1, speed);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_2, 0);    //修改比较值，修改占空比pwm2/7200}}if(moto==2){if(speed>=0){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_3, 0);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, speed);    //修改比较值，修改占空比pwm2/7200}if(speed<0){__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_3, speed);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, 0);    //修改比较值，修改占空比pwm2/7200}}if(moto==3){if(speed>=0){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_1, 0);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_2, speed);    //修改比较值，修改占空比pwm2/7200}if(speed<0){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_1, speed);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_2, 0);    //修改比较值，修改占空比pwm2/7200}}if(moto==4){if(speed>=0){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, 0);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, speed);    //修改比较值，修改占空比pwm2/7200}if(speed<0){__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_3, speed);    //修改比较值，修改占空比pwm1/7200__HAL_TIM_SetCompare(&htim4, TIM_CHANNEL_4, 0);    //修改比较值，修改占空比pwm2/7200}}

对于void drive(int moto,float speed) 这个函数，只需要输入想要控制的电机，和想要控制的速度，就可以了，如果是正的就是向前，负的就是向后，这个是为了控制方便简化的程序。

定义四个全局变量：

float V1=0,V2=0,V3=0,V4=0;

在main.c的while里，添加下列四行代码：

while (1)
{drive(1,V1);drive(2,V2);drive(3,V3);drive(4,V4);
}

这样当V1~V4改变的时候，会立刻反映到电机的pwm波上。

电机速度叠加的各函数

不知为何添加了math.h之后还是无法调用abs函数，也不难，先自己写一个：

float custom_abs(float target)//不知道为何无法使用abs，自己写个abs
{if (target>=0)	return target;else return -target;
}

w的计算函数

定义变量,为了方便以后添加D环节，就把微分项加上了：

float kp=7,ki=0,kd=0;//数值目前是随便设定的
float dAngle;//上一次和这一次的角度差
float excursion_w=0;//角度的微分，我也不清楚为何起这个名字
float Add_Angle=0;//角度的积分
float Add_Max=100;//积分上限

使用到的全局变量：

extern float Angle,lastAngle;
extern float w;

相关的函数——来自main.c的uart2串口中断获取Angle的函数：

if(huart->Instance == USART2){if(RxByte==0x53)	start_u2=true;if(start_u2==true){RxBuff[Rx_Count++]=RxByte;if(Rx_Count>=7){Rx_Count=0;start_u2=false;lastAngle=Angle;if(((((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0)!=0)	Angle = (((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0;if(Angle >=180 && Angle <=360)	Angle=Angle-360;//从0~360映射到-180~180//printf("Angle = %f\r\n",Angle);}}if(Rx_Count>=254){Rx_Count=0;}while(HAL_UART_Receive_IT(&huart2,&RxByte,1)==HAL_OK);}

void Calculate_angular_velocity(void) 函数本体：

void Calculate_angular_velocity(void)
{dAngle=Angle-lastAngle;excursion_w=dAngle/dt;//对Angle进行求导Add_Angle+=Angle;//对Angle进行积分if(Add_Angle>=Add_Max)	Add_Angle=Add_Max;//抗积分饱和if(Add_Angle<=-Add_Max)	Add_Angle=-Add_Max;w=kp*Angle+ki*Add_Angle+kd*excursion_w;//调整wif(w>=200)	w=200;//把w限制在|0-200|之内else if(w<=-200)	w=-200;printf("Angle = %f\r\n",Angle);printf("w = %f\r\n",w);
}

说实话，确实也用过PID，但是我也不知道我写的对不对，很有可能错的一塌糊涂。
以上函数可以获取最终的w值

Vx和Vy的计算

使用到的全局变量：

extern float Vx,Vy;

相关的函数——main.c里串口中断获取两个激光测距的数据dis1和dis2：

	if(huart->Instance == UART5){if(RxDByte1==0x81)	start_u5=true;if(start_u5==true){RxDbuff1[RxD1_Count++]=RxDByte1;if(RxD1_Count>=3){RxD1_Count=0;start_u5=false;dis1 = (((short)RxDbuff1[2]<<8)|RxDbuff1[1]);printf("dis1 = %f\r\n",dis1);}}if(RxD1_Count>=254){RxD1_Count=0;}while(HAL_UART_Receive_IT(&huart5,&RxDByte1,1)==HAL_OK);		}if(huart->Instance == USART6){if(RxDByte2==0x81)	start_u6=true;if(start_u6==true){RxDbuff2[RxD2_Count++]=RxDByte2;if(RxD2_Count>=3){RxD2_Count=0;start_u6=false;dis2 = (((short)RxDbuff2[2]<<8)|RxDbuff2[1]);printf("dis2 = %f\r\n",dis2);}}if(RxD2_Count>=254){RxD2_Count=0;}while(HAL_UART_Receive_IT(&huart6,&RxDByte2,1)==HAL_OK);		}

void Calculate_the_direction_velocity(int target_x,int target_y) 主体，这个函数可以根据输入的目标x轴y轴坐标，分配Vx和Vy，30这个数字也只是随意设定的。

void Calculate_the_direction_velocity(int target_x,int target_y)
{Vx=dis1-target_x;//大部分时间是满速的，有一个最小速度30，映射到|30-100|，但注意这不是最终速度Vy=dis2-target_y;if(Vx>0)	Vx+=30;else if(Vx<0)	Vx-=30;if(Vy>0)	Vy+=30;else if(Vy<0)	Vy-=30;if(Vx>=100)	Vx=100;else if(Vx<=-100)	Vx=-100;if(Vy>=100)	Vy=100;else if(Vy<=-100)	Vy=-100;
}

速度的叠加

使用到的全局变量：

extern float V1,V2,V3,V4;
extern float Vx,Vy;
extern float w;

根据麦克纳姆轮底盘速度合成公式写就可以：

void Calculated_combined_velocity(void)
{V1=(Vy-Vx+w)/4;V2=(Vy+Vx-w)/4;V3=(Vy-Vx-w)/4;V4=(Vy+Vx+w)/4;printf("V1=%.1f V2=%.1f V3=%.1f V4=%.1f\r\n",V1,V2,V3,V4);
}

简单的组织任务程序

这个程序大概意思就是输入目标位置后，小车会跑到那个位置，期间的w根据陀螺仪不断调整，而Vx和Vy则根据两个激光测距的数据不断调整，最后x轴和y轴还有角度都到达了之后，会发出mission_complete这个全局变量信号通知我已经完成任务：

void run_to(int target_x,int target_y)//输入目标位置，小车会走到目标位置并发出任务完成标志信号
{Calculate_angular_velocity();	//获取到wCalculate_the_direction_velocity(target_x,target_y);//获取到Vx和VyCalculated_combined_velocity();//叠加if(custom_abs(dis1-target_x)<10)	Is_x_ok=true;	else Is_x_ok=false;if(custom_abs(dis2-target_y)<10)	Is_y_ok=true;	else Is_y_ok=false;if(custom_abs(Angle)<10)	Is_Angle_ok=true;else Is_Angle_ok=false;if(Is_x_ok && Is_y_ok && Is_Angle_ok){Is_x_ok=false;Is_y_ok=false;Is_Angle_ok=false;mission_complete=true;}if(mission_complete)	{Vx=0;Vy=0;w=0;mission_complete=false;}}

这个程序只有两个输入变量，target_x和target_y分别是目标x坐标和目标y坐标，实际上还应该有第三个输入变量（在我之前arduino的程序里是写了的），就是下一任务代号next_Action_code，方便转到下一个任务上，但是目前没有测试到多任务的部分，所以只写了这个，而且其实这个也没有测试。

小车目前的照片和简单测试

只测试了w的调整程序是否正常，目前来讲是没问题的。

小车的目前照片：

总结

目前来讲，我有一个很致命的问题没有解决，就是线实在是太难连了，我迫切的需要抓紧时间焊个能稍微简化一下线路的PCB板。所以接下来我又两个任务：

• 写完组织任务代码
• 设计并且焊接完PCB板

全部代码

最后附加上全部的代码：
Auto_control.c:

#include "Auto_Control.h"
#include "stdio.h"
#include "math.h"
#include "stdbool.h"
#include "string.h"extern float dis1;
extern float dis2;
extern float Angle,lastAngle;extern unsigned int dt;extern float x,y;
extern float last_x,last_y;extern float V1,V2,V3,V4;extern float w;float custom_abs(float target)//不知道为何无法使用abs，自己写个abs
{if (target>=0)	return target;else return -target;
}float kp=7,ki=0,kd=0;
float dAngle;
float excursion_w=0;
float Add_Angle=0;
float Add_Max=100;//积分上限void Calculate_angular_velocity(void)
{dAngle=Angle-lastAngle;excursion_w=dAngle/dt;//对Angle进行求导Add_Angle+=Angle;//对Angle进行积分if(Add_Angle>=Add_Max)	Add_Angle=Add_Max;//抗积分饱和if(Add_Angle<=-Add_Max)	Add_Angle=-Add_Max;w=kp*Angle+ki*Add_Angle+kd*excursion_w;//调整wif(w>=200)	w=200;//把w限制在|0-200|之内else if(w<=-200)	w=-200;printf("Angle = %f\r\n",Angle);printf("w = %f\r\n",w);
}extern float Vx,Vy;
void Calculate_the_direction_velocity(int target_x,int target_y)
{Vx=dis1-target_x;//大部分时间是满速的，有一个最小速度30，映射到|30-100|，但注意这不是最终速度Vy=dis2-target_y;if(Vx>0)	Vx+=30;else if(Vx<0)	Vx-=30;if(Vy>0)	Vy+=30;else if(Vy<0)	Vy-=30;if(Vx>=100)	Vx=100;else if(Vx<=-100)	Vx=-100;if(Vy>=100)	Vy=100;else if(Vy<=-100)	Vy=-100;
}void Calculated_combined_velocity(void)
{V1=(Vy-Vx+w)/4;V2=(Vy+Vx-w)/4;V3=(Vy-Vx-w)/4;V4=(Vy+Vx+w)/4;printf("V1=%.1f V2=%.1f V3=%.1f V4=%.1f\r\n",V1,V2,V3,V4);
}bool Is_x_ok=false;
bool Is_y_ok=false;
bool Is_Angle_ok=false;
extern bool mission_complete;void run_to(int target_x,int target_y)//输入目标位置，小车会走到目标位置并发出任务完成标志信号
{Calculate_angular_velocity();	//获取到wCalculate_the_direction_velocity(target_x,target_y);//获取到Vx和VyCalculated_combined_velocity();//叠加if(custom_abs(dis1-target_x)<10)	Is_x_ok=true;	else Is_x_ok=false;if(custom_abs(dis2-target_y)<10)	Is_y_ok=true;	else Is_y_ok=false;if(custom_abs(Angle)<10)	Is_Angle_ok=true;else Is_Angle_ok=false;if(Is_x_ok && Is_y_ok && Is_Angle_ok){Is_x_ok=false;Is_y_ok=false;Is_Angle_ok=false;mission_complete=true;}if(mission_complete)	{Vx=0;Vy=0;w=0;mission_complete=false;}}

Auto_control.h:

#ifndef __AUTO_CONTROL_H
#define	__AUTO_CONTROL_Hvoid Calculated_combined_velocity(void);
void Calculate_angular_velocity(void);
void Calculate_the_direction_velocity(int target_x,int target_y);
void run_to(int target_x,int target_y);#endif

main.c:

/* USER CODE BEGIN Header */
/********************************************************************************* @file           : main.c* @brief          : Main program body******************************************************************************* @attention** <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.* All rights reserved.</center></h2>** This software component is licensed by ST under BSD 3-Clause license,* the "License"; You may not use this file except in compliance with the* License. You may obtain a copy of the License at:*                        opensource.org/licenses/BSD-3-Clause********************************************************************************/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdbool.h"
#include "string.h"
#include "stdio.h"
#include "moto.h"
#include "Auto_Control.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 *//* USER CODE END PM *//* Private variables ---------------------------------------------------------*//* USER CODE BEGIN PV */
uint8_t RxByte;//串口每接收一个数据，就存入RxByte中
uint8_t RxBuff[256];
uint16_t Rx_Count;
float Angle=0,lastAngle=0;
bool start_u2=false;uint8_t RxDByte1;
uint8_t RxDbuff1[256];
uint16_t RxD1_Count;
float dis1;//规定dis1为x轴距离
bool start_u5=false;uint8_t RxDByte2;
uint8_t RxDbuff2[256];
uint16_t RxD2_Count;
float dis2;//规定dis2为y轴距离
bool start_u6=false;float x,y;
float last_x,last_y;float V1=0,V2=0,V3=0,V4=0;
float Vx=0,Vy=0;float w;unsigned int sysclock = 0;  
unsigned int lastsysclock = 0;
unsigned int dt=0;bool mission_complete=false;/* 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 *//* 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_TIM3_Init();MX_TIM4_Init();MX_USART1_UART_Init();MX_USART2_UART_Init();MX_USART3_UART_Init();MX_UART5_Init();MX_USART6_UART_Init();MX_UART4_Init();MX_TIM2_Init();/* USER CODE BEGIN 2 */HAL_UART_Receive_IT(&huart2,&RxByte,1);HAL_UART_Receive_IT(&huart5,&RxDByte1,1);HAL_UART_Receive_IT(&huart6,&RxDByte2,1);HAL_TIM_Base_Start_IT(&htim2);HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);//使能8个电机pwm通道HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */HAL_Delay(3000);while (1){drive(1,V1);drive(2,V2);drive(3,V3);drive(4,V4);/* 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_SCALE1);/** 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 = 4;RCC_OscInitStruct.PLL.PLLN = 72;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();}
}/* USER CODE BEGIN 4 */
//个人的自定义函数部分void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{if(htim->Instance == TIM2){lastsysclock=sysclock;sysclock = HAL_GetTick();//返回从系统运行开始经过的时间，默认情况下单位为msdt=sysclock-lastsysclock;Calculate_angular_velocity();Calculated_combined_velocity();}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{UNUSED(huart);if(huart->Instance == USART2){if(RxByte==0x53)	start_u2=true;if(start_u2==true){RxBuff[Rx_Count++]=RxByte;if(Rx_Count>=7){Rx_Count=0;start_u2=false;lastAngle=Angle;if(((((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0)!=0)	Angle = (((short)RxBuff[6]<<8)|RxBuff[5])/32768.0*180.0;if(Angle >=180 && Angle <=360)	Angle=Angle-360;//从0~360映射到-180~180//printf("Angle = %f\r\n",Angle);}}if(Rx_Count>=254){Rx_Count=0;}while(HAL_UART_Receive_IT(&huart2,&RxByte,1)==HAL_OK);}if(huart->Instance == UART5){if(RxDByte1==0x81)	start_u5=true;if(start_u5==true){RxDbuff1[RxD1_Count++]=RxDByte1;if(RxD1_Count>=3){RxD1_Count=0;start_u5=false;dis1 = (((short)RxDbuff1[2]<<8)|RxDbuff1[1]);printf("dis1 = %f\r\n",dis1);}}if(RxD1_Count>=254){RxD1_Count=0;}while(HAL_UART_Receive_IT(&huart5,&RxDByte1,1)==HAL_OK);		}if(huart->Instance == USART6){if(RxDByte2==0x81)	start_u6=true;if(start_u6==true){RxDbuff2[RxD2_Count++]=RxDByte2;if(RxD2_Count>=3){RxD2_Count=0;start_u6=false;dis2 = (((short)RxDbuff2[2]<<8)|RxDbuff2[1]);printf("dis2 = %f\r\n",dis2);}}if(RxD2_Count>=254){RxD2_Count=0;}while(HAL_UART_Receive_IT(&huart6,&RxDByte2,1)==HAL_OK);		}}
//printf重定向到串口1
int fputc(int ch, FILE *f)
{HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xffff);return ch;
}
int fgetc(FILE *f)
{uint8_t ch = 0;HAL_UART_Receive(&huart1, &ch, 1, 0xffff);return ch;
}
/* 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 *//************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

PS

一直有只猫在干扰我，很影响效率，(^▽)