技术日志——第四篇

最新推荐文章于 2022-07-26 16:00:14 发布
最新推荐文章于 2022-07-26 16:00:14 发布
阅读量632 收藏 6
点赞数 3
分类专栏： 工训竞赛物流车技术日志
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_43586313/article/details/114442363
版权

工训竞赛物流车技术日志2021.3.6

综合分析

麦克纳姆轮速度合成公式

在这里插入图片描述
从右前开始，逆时针转一圈为这个公式里的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();//返回从系统运行开始经过的时间，默认情况下单位为ms
		dt=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;//调整w
	
	if(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();	//获取到w
	Calculate_the_direction_velocity(target_x,target_y);//获取到Vx和Vy
	Calculated_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;//调整w
	
	if(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();	//获取到w
	Calculate_the_direction_velocity(target_x,target_y);//获取到Vx和Vy
	Calculated_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_H

void 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();//返回从系统运行开始经过的时间，默认情况下单位为ms
		dt=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

一直有只猫在干扰我，很影响效率，(▽)
在这里插入图片描述

风中的聂先森
关注
专栏目录
ELK日志平台建设——设计篇——分布式日志处理系统设计
程序员光剑
08-08 1597
ELK(Elasticsearch Logstash Kibana)是目前最流行的开源日志分析工具。本系列文章主要介绍日志平台建设的一些关键技术细节和方案。首先要了解这些技术分别是什么，它们解决了什么问题，如何使用。然后逐步深入到各个技术组件内部，详细地剖析其工作原理及其优缺点。最后通过实例和场景进行对比，为读者提供完整的指导和实践建议。
100天精通Python（基础篇）——第32天：日志logging模块基础+代码实战
努力让自己发光，对的人才能迎着光而来
04-06 1万+
一、日志基础知识 1、日志是什么 2、日志的作用 3、日志的级别 4、日志的内容 5、怎么使用日志 二、logging基础介绍 1、logging库日志级别 2、logging工作流程 3、日志输出格式 4、基本使用 5、代码实战 指定日志输出级别 输出日志到文件
麦克纳姆轮（Mecanum Wheel）运动学分析
qq_44343584的博客
03-22 2万+
0.1 麦克纳姆轮底盘示意图 0.2 运动学逆解概念 -逆运动学是决定要达成所需要的姿势所要设置的关节可活动对象的参数的过程。 一、 底盘运动的分解 我们知道，刚体在平面内的运动可以分解为三个独立分量：X轴平动、Y轴平动、yaw 轴自转。如下图所示，底盘的运动也可以分解为三个量： 以上三个量一般都视为四个轮子的几何中心（矩形的对角线交点）的速度： 二、计算出轮子轴心位置的速度 定义： 那么...
STM32单片机麦克纳姆轮小车以及操纵杆控制程序代码.zip
05-06
STM32单片机麦克纳姆轮小车以及操纵杆控制，这个是在我大二电子设计竞赛的准备工作中完成了最基本的驱动功能：能蓝牙手机控制上下左右及斜向总共8个方向的平移，还有原地正反转功能。当时想着使用操纵杆来遥控控制呢，只是学习比赛繁忙，就忘了去实现了。最后是工作后才完成的。 毕业工作后整理东西才发现当时有这个想法，而且也觉得很有趣，并且在想应该不止8个方向的平移，理论能任意方向移动，于是就尝试写代码实现，理论先实现任意平移，再结合操纵杆控制实现任意平移，结合自带按键可旋转（其实操作杆是先实现的，控制其8方向平移，再开发任意方向平移）。 先实现遥控器部分：操纵杆是有x、y运动轴组成可360°随意转动的，因为两个轴对应有滑动变阻器，所以单片机采用2个adc来读取对应的值，结合滑动范围0-4096（12bit）可以判断此时操纵杆的姿态，与2048对比得出其位于哪个象限。在操纵杆这边就直接读取adc，然后根据串口的自定义命令格式进行传输就可以了。我这里就用 #x轴adc值，y轴adc值* 来通知小车的平移姿态，但还要有自旋转，还好这个操纵杆也带了一个按键，藏在操作杆下面，于是可以用来切换控制旋转。这
全方位移动全向轮、麦克纳姆轮底盘运动学逆解详解（内含电机输出方程）
Cheney的博客
08-07 1万+
全方位移动全向轮、麦克纳姆轮底盘运动学逆解详解（内含电机输出方程） 分析不多，直接上代码，本人亲测可用
三种常见的移动底盘运动学模型分析
qq_52785580的博客
07-26 1万+
对四轮差速小车、麦克纳姆轮小车以及两轮差速小车运动学模型的学习总结。
麦克纳姆轮全向移动机器人运动效率分析
qq_19598969的博客
07-06 856
关注同名微信公众号“混沌无形”，阅读更多有趣好文！ 原文链接：https://mp.weixin.qq.com/s/GWhlXsuY6QYyoZydaSYpjQ（包含原文PDF百度云下载链接） 在章节2.1中分析，麦轮平台的各个轮子产生的力会相互抵消一部分，因此同样转矩产生的净推力效率较低，也就是一部分功率被内耗掉了，效率不如普通轮胎。且麦轮运动过程中同时存在纵向和横向分力，所以做机构设计时需要对电机轴（或联轴器）等加保护。 精彩的理论论证过程见原文链接（含全文下载链接） ...
第七次作业解析——日志文件程序
12-21
1. **标题**：用于标识每篇日志的主题，由用户自行输入。 2. **作者**：记录日志的作者，同样由用户输入。 3. **时间戳**：包含日期和时间，用于追踪日志创建的具体时刻。这可以通过Java的`java.sql.Date`和`java....
spring——第十四节素材.rar
10-04
标题中的“spring——第十四节素材.rar”表明这是一个关于Spring框架的教程材料，可能是课程的第14节内容。Spring是Java开发中最广泛使用的轻量级框架，它提供了丰富的功能来简化企业级应用的开发，包括依赖注入、...
数据库应用技术——SQL Server 2008篇(第3版)
01-19
《数据库应用技术——SQL Server 2008篇(第3版)》是一部全面解析SQL Server 2008数据库管理系统的专著，适合初学者和有一定基础的IT专业人士使用。书中深入浅出地介绍了数据库的基本概念、设计原理以及SQL Server ...
麦克纳姆轮运动原理_麦克纳姆轮介绍
weixin_39546312的博客
11-16 1万+
先看个大号Mecanum轮的应用图“第一部分：Mecanum轮是什么 麦克纳姆轮最早是美国麦克纳姆公司为了在航母上移动物资的机器人而设计的全方位移动轮子，相对全向轮来说，麦轮可以通过其转速和安装方法来合成在任意方向的合力。所以它可以让车全方向移动，所以麦轮的车需要四个电机单独驱动每个轮子。国内某军工产品，国产骄傲啊全向移动意味着可以在平面内做出任意方向平移同时自转的动作。为了实现全向移...
四轮麦轮的运动学解析
dxmcu的专栏
10-17 7067
四轮mecanum的正逆运动学解析 google来的英文资料,不嫌麻烦的可以看看     逆运动学解析：         V1 = Vx - Vy - k * Wv         V2 = Vx + Vy - K * Wv         V3 = Vx - Vy + k * Wv         V4 = Vx + Vy + K * Wv     正运动学解析:      ...
robomaster麦轮运动解算
qq_53671582的博客
04-09 1万+
麦轮的正逆运动学分析
麦轮速度解算
十兮
10-08 3740
麦克纳姆轮简介 依靠各自机轮的方向和速度，这些力的最终合成在任何要求的方向上产生一个合力矢量从而保证了这个平台在最终的合力矢量的方向上能自由地移动，而不改变机轮自身的方向。在它的轮缘上斜向分布着许多小滚子，故轮子可以横向滑移。小滚子的母线很特殊，当轮子绕着固定的轮心轴转动时，各个小滚子的包络线为圆柱面，所以该轮能够连续地向前滚动。麦克纳姆轮结构紧凑，运动灵活，是很成功的一种全方位轮。有4个这种新型轮子进行组合，可以更灵活方便的实现全方位移动功能。——百度百科 参数设定 已知条件：小车在以底盘平面.
麦克纳姆轮速度分解计算及里程计计算
weixin_42679205的博客
03-31 6841
麦克纳姆轮速度分解计算及里程计计算 文章目录前言一、速度解算过程二、里程计计算过程逆运动学求解速度总结 前言 麦克纳姆轮在现如今的机器人应用中十分广泛,经我自己的查阅资料和整理,得到下面的解算过程 一、速度解算过程 在该模型中,使用的是O-正方形模型为例,具体计算模型如下图所示(左上为1,右上为2,右下为3,左下为4) 如有不理解的地方,可参见以下链接:麦轮正运动学解算原理 在计算中,是在stm32中对速度及里程计进行解算,具体代码如下: void ChassisVelSet(float vx,
麦克纳姆轮运动特性分析
qq_19598969的博客
06-28 1633
关注同名微信公众号“混沌无形”，阅读更多有趣好文！ 原文链接：https://mp.weixin.qq.com/s/sjb3O91auADKN7iDsut0jA（包含原文PDF百度云下载链接） 摘要：本文先描述了麦克纳姆轮的构型及基本特点，接着对单个麦克纳姆轮进行了受力分析，指出沿辊子轴线的静摩擦力为有效力，并在此基础上分析了麦轮线速度和平行于辊子轴线的分速度之间的关系，给出速度分解方程；最后，总结分析了麦克纳姆轮的在实际使用过程中的特点及问题。 总结分析：麦轮运动过程中存在较大滚动..
麦克纳姆轮运动学分析
weixin_33859231的博客
05-15 6695
麦克纳姆轮运动学分析 是最常见的安装方式 麦轮底盘的正逆运动学模型 以O-长方形的安装方式为例，四个轮子的着地点形成一个矩形。正运动学模型（forward kinematic model）将得到一系列公式，让我们可以通过四个轮子的速度，计算出底盘的运动状态；而逆运动学模型（inverse kinematic model）得到的公式则是可以根据底盘的运动状态解算出四...
浅谈麦克纳姆轮全向移动平台之——运动学
沐棋的博客
04-06 1万+
0. 麦克纳姆轮全向移动平台简介 麦克纳姆轮百度词条 还是自己去看百度词条算了，不说太多废话了，简单看看长什么样就行啦。 1. 运动学逆解 入正题入正题，我们直接来聊聊麦克纳姆轮的运动学逆解，正解就不说了，一般来讲逆解比正解麻烦一些，而且会逆解就会正解。正解跳过不说，相信小伙伴们会根据逆解过程得出正解的。 1.0 首先按照惯例建立一个小车坐标系 在本篇博文中所有的速度矢量都是相对于小车坐标系来...
ROS麦克纳姆轮底盘制作（上）
热门推荐
熊猫飞天
10-07 3万+
目录 1.底盘硬件框图 2、麦克纳姆轮模型介绍 2.1逆解运动学模型 2.2正解运动学模型 3 底盘软件框架 4 参考资料 在这一个专题中，我们将介绍如何搭建利用麦克纳姆轮搭建一个ROS底盘。参考标准的ROS底盘，ROS底盘应该具备接收导航节点发送的“cmd_vel”这个话题上的控制命令（小车X,Y,Z三个方向的速度以及三个方向的角速度），同时更加底盘轮子的转速信息预估底盘的位...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值