基于stm32循迹小车使用mpu6050转弯

本人第一次制作循迹小车,如果想法有不合理处,还请大佬指点

起初,我的想法是使用双轮的差速进行转弯,但是对于数据的不了解,作为初学者,我突然有一个更有趣的想法。

为了提高循迹小车的转弯速度,在识别到需要转弯的路口进行mpu6050使能,通过z轴的角速度的近似积分运算;

因为我们都知道,在比较短的时间内,转过的角=角加速度*时间;当我启用定时器,对角速度进行快速的取值,在乘对应的时间,我就会得出一个短时间内相对准确的偏航角。

显然,仅仅将一秒分为9份,图形的线下面积就已经很接近于偏航角的准确值了;

但是如果精度太高,会造成硬件的压力增大,所以适当进行提高精度就可以计算一个比较稳定的偏航角;

 

void MPU6050_GetZ(int16_t *GyroX);
void MPU6050_GetZ(int16_t *GyroZ)
{
	uint8_t DataH, DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	*GyroZ = (DataH << 8) | DataL;
}

这是对z轴角速度进行取值;

void Angel_Calcu_Init(void)
{
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
	
	TIM_InternalClockConfig(TIM4);
	
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
	TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseInitStructure.TIM_Period = 20000 - 1;
	TIM_TimeBaseInitStructure.TIM_Prescaler = 72 - 1;//在此设置精度
	TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseInitStructure);
	
	TIM_ClearFlag(TIM4, TIM_FLAG_Update);
	TIM_ITConfig(TIM4, TIM_IT_Update, ENABLE);
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
	NVIC_Init(&NVIC_InitStructure);
	
	TIM_Cmd(TIM4, ENABLE);
}

void acculary_Z (void)
{
	MPU6050_GetZ(&gyroz);
	
//	if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);
//	if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4;
	Gyro_z  = gyroz;
	float temp;			
	temp=((Angle_Z_Start + Gyro_z)/2)*0.0121 ;								//进行积分运算
	Angle_Z_Start = Gyro_z;
	Angle_Z_Final += temp ;							//加入结果
}

 以下是定时器对结果的运算;

void TIM4_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM4, TIM_IT_Update) == SET)
	{
		Angle_Calcu();
		acculary_Z();
		TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
	}
}

别忘了在头文件中添加extern float Angle_Z_Final;

如果小车遇到了路口,我们就使能定时器

void turn_start(void)
{
	TIM_Cmd(TIM4, ENABLE);
}

如果角度达到了90,我们就关闭定时器,同时清零数据,以免影响下一次转弯的结果

void turn_stop(void)
{
	TIM_Cmd(TIM4, DISABLE);
	Gyro_z=0;
	Angle_Z_Start=0;
	Angle_Z_Final=0;
}

这就是我对于小车使用mpu6050进行快速转弯的理解

我们只需要识别角度,调整轮子pwm进行差速转弯,得到更理想的效果。

本人刚刚接触32单片机,如果做的不好请大佬见谅。

  • 16
    点赞
  • 29
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
由于GPS循迹小车的代码涉及到硬件和软件的部分,需要根据具体的硬件电路和功能需求进行编写,所以无法给出一份完整的代码。不过,以下是一个基本的搭配neo-6m和mpu-6050模块的GPS循迹小车的代码框架,供参考: ``` #include "stm32f10x.h" #include "stdio.h" #include "stdlib.h" #include "math.h" #define MPU6050_ADDRESS 0xD0 #define NEO6M_ADDRESS 0x42 // 定义MPU6050寄存器地址 #define MPU6050_SMPLRT_DIV 0x19 #define MPU6050_CONFIG 0x1A #define MPU6050_GYRO_CONFIG 0x1B #define MPU6050_ACCEL_CONFIG 0x1C #define MPU6050_ACCEL_XOUT_H 0x3B #define MPU6050_ACCEL_XOUT_L 0x3C #define MPU6050_ACCEL_YOUT_H 0x3D #define MPU6050_ACCEL_YOUT_L 0x3E #define MPU6050_ACCEL_ZOUT_H 0x3F #define MPU6050_ACCEL_ZOUT_L 0x40 #define MPU6050_TEMP_OUT_H 0x41 #define MPU6050_TEMP_OUT_L 0x42 #define MPU6050_GYRO_XOUT_H 0x43 #define MPU6050_GYRO_XOUT_L 0x44 #define MPU6050_GYRO_YOUT_H 0x45 #define MPU6050_GYRO_YOUT_L 0x46 #define MPU6050_GYRO_ZOUT_H 0x47 #define MPU6050_GYRO_ZOUT_L 0x48 // 定义NEO-6M寄存器地址 #define NEO6M_GPRMC 0x01 // 定义串口波特率 #define BAUDRATE 9600 // 定义小车控制接口 #define LEFT_FORWARD_GPIO GPIO_Pin_0 #define LEFT_BACKWARD_GPIO GPIO_Pin_1 #define RIGHT_FORWARD_GPIO GPIO_Pin_2 #define RIGHT_BACKWARD_GPIO GPIO_Pin_3 // 定义小车速度 #define SPEED 50 // 定义小车方向 #define LEFT 0 #define RIGHT 1 // 定义PI控制参数 #define KP 1 #define KI 0.5 // 定义角度转弧度的常量 #define DEG_TO_RAD 0.01745329251994329576923690768489 // 定义全局变量 float gyro_x, gyro_y, gyro_z; // 陀螺仪数据 float accel_x, accel_y, accel_z; // 加速度计数据 float pitch, roll, yaw; // 欧拉角 float target_yaw; // 目标航向角 float error_yaw; // 航向角误差 float integral_error_yaw; // 航向角误差积分 float pwm_left, pwm_right; // 左右轮PWM输出 // 定义函数原型 void TIM2_IRQHandler(void); void TIM3_IRQHandler(void); void USART1_IRQHandler(void); void init_GPIO(void); void init_USART1(void); void init_I2C(void); void init_TIM2(void); void init_TIM3(void); void read_MPU6050(void); void read_NEO6M(void); void control_car(void); float get_yaw(void); int main(void) { // 初始化GPIO、USART1、I2C、TIM2、TIM3 init_GPIO(); init_USART1(); init_I2C(); init_TIM2(); init_TIM3(); // 启动TIM2、TIM3 TIM_Cmd(TIM2, ENABLE); TIM_Cmd(TIM3, ENABLE); while(1) { // 读取MPU6050、NEO-6M数据 read_MPU6050(); read_NEO6M(); // 控制小车 control_car(); } } // TIM2中断服务函数 void TIM2_IRQHandler(void) { // 清除更新中断标志位 TIM_ClearITPendingBit(TIM2, TIM_IT_Update); // 读取陀螺仪数据 I2C_AcknowledgeConfig(I2C1, ENABLE); I2C_GenerateSTART(I2C1, ENABLE); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Transmitter); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); I2C_SendData(I2C1, MPU6050_GYRO_XOUT_H); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); I2C_GenerateSTART(I2C1, ENABLE); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_MODE_SELECT)); I2C_Send7bitAddress(I2C1, MPU6050_ADDRESS, I2C_Direction_Receiver); while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); gyro_x = (I2C_ReceiveData(I2C1) << 8 | I2C_ReceiveData(I2C1)); gyro_y = (I2C_ReceiveData(I2C1) << 8 | I2C_ReceiveData(I2C1)); gyro_z = (I2C_ReceiveData(I2C1) << 8 | I2C_ReceiveData(I2C1)); I2C_GenerateSTOP(I2C1, ENABLE); // 计算欧拉角 roll = atan2(accel_y, accel_z) * 180 / M_PI; pitch = atan2(-accel_x, sqrt(accel_y * accel_y + accel_z * accel_z)) * 180 / M_PI; yaw = get_yaw(); // 获取航向角 } // TIM3中断服务函数 void TIM3_IRQHandler(void) { // 清除更新中断标志位 TIM_ClearITPendingBit(TIM3, TIM_IT_Update); // 控制小车 control_car(); } // USART1中断服务函数 void USART1_IRQHandler(void) { // ... } // 初始化GPIO void init_GPIO(void) { // ... } // 初始化USART1 void init_USART1(void) { // ... } // 初始化I2C void init_I2C(void) { // ... } // 初始化TIM2 void init_TIM2(void) { // ... } // 初始化TIM3 void init_TIM3(void) { // ... } // 读取MPU6050数据 void read_MPU6050(void) { // ... } // 读取NEO-6M数据 void read_NEO6M(void) { // ... } // 控制小车 void control_car(void) { // ... } // 获取航向角 float get_yaw(void) { // ... } ```

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值