[基于STM32底盘控制与ROS上层导航小车制作]第三节 stm32线速度标定

系列文章目录

第一节 stm32电机驱动与编码器读取反馈

第二节 stm32电机pid控制

第三节 stm32线速度标定

第四节 stm32添加mpu6050得到angle角度 

第五节 实现STM32与ubuntu系统下的ROS串口DMA通信，传输底盘速度等信息

第六节 ROS计算和发布里程计与tf变换

系列教材包含

底盘搭建与stm32代码编写，ros激光雷达建图与导航编写，实现动态避障与导航

底盘包含

两轮差速轮（自制）

四轮全向轮（所属团队大家一起搭建）

底盘不同就是在底盘的运动分解与控制，ros部分里程计计算和本地规划器不同，两轮用的base_local_Planner，四轮用的teb_local_planner,来规划发布速度控制底盘

gjhhust / Repositories · GitHubhttps://github.com/gjhhust?tab=repositories

---

目录

系列文章目录

前言

一、pid介绍

二、pid简单调整方法

总结

---

---

前言

今天来为大家介绍底盘实际运动整定方法

---

一、位置环控制小车行驶1m

  位置环控制小车行驶1m这个也是pid控制，定义为position_PID这时我们应该不陌生了，明确三个量，setpoint就是100cm，current_value就是当前以及行驶距离，这个需要我们计算，pid输出的话，可以有两个选择，可以使占空比，也可以是速度，反正只要把握pid的作用，当前值距离目标值越近我们输出会改变，这里设置不一样的输出方式时候注意，加入你设置速度为输出，那么你的当前值计算就要靠速度来推导，占空比同理，因为pid就是靠这层关系调整输出的。

  我选择速度作为pid输出，这样就可以不破坏原来的代码，添加几句pid计算出当前需要的速度然后赋值给上一节中的速度pid的setpoint，也就是position_PID的输出(速度)，替换50

TIM_SetCompare1(TIM1,PID_Loc(50, left_speed_now, &left_PID));

//新的代码
TIM_SetCompare1(TIM1,PID_Loc(speed_left_SetPoint , left_speed_now, &left_PID));

position_PID需要计算current_value如下

//标定使用
positionNow += (rightSpeedNow*0.005 + leftSpeedNow*0.005 ) / 2;

因为我是两轮底盘，所以计算已行驶的距离就是靠数学推导得到如上，别的底盘都行，自己写好计算表达式即可。

实现函数如下

/**********************************************************************************************************
*函 数 名: PID_init
*功能说明: 初始化pid
*形    参: 无
*返 回 值: 无
**********************************************************************************************************/
void PID_init(void){
    left_speed_PID.kp = 20;
    left_speed_PID.ki = 0.5;
    left_speed_PID.kd = 0;

    right_speed_PID.kp = 20;
    right_speed_PID.ki = 0.5;
    right_speed_PID.kd = 0;

    position_PID.kp = 3;
    position_PID.ki = 0;
    position_PID.kd = 0.5;
	
}
/**********************************************************************************************************
*函 数 名: goto_1m
*功能说明: 直线走一米
*形    参: 无
*返 回 值: 无
**********************************************************************************************************/
void goto_1m(void){

	speed_left_SetPoint = PID_Loc(100cm,positionNow,&position_PID);
    speed_right_SetPoint = PID_Loc(100cm,positionNow,&position_PID);
	
}
/**********************************************************************************************************
*函 数 名: motor_direction
*功能说明: 电机方向选择
*形    参: 无
*返 回 值: 无
**********************************************************************************************************/
void motor_direction(void)
{
	if(speed_left_SetPoint <0)
	{
		motor_L_back();
	}else
	{
		motor_L_move();
	}
	
	
	if(speed_right_SetPoint <0)
	{
		motor_R_back();
	}else
	{
		motor_R_move();
	}
	
	if(speed_left_SetPoint  == 0)motor_L_stop();
	if(speed_right_SetPoint == 0)motor_R_stop();
}

/**********************************************************************************************************
*函 数 名: TIM6_IRQHandler
*功能说明: 车辆控制周期 
*形    参: 无
*返 回 值: 无
**********************************************************************************************************/
void TIM6_IRQHandler(void)   //TIM3中断
{
		if(TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源
		{
			
			pid_flag++;
			
			
			TIM_ClearITPendingBit(TIM6, TIM_IT_Update);   //清除TIMx的中断待处理位:TIM 中断源
					 
			Get_Motor_Speed(&left_speed_now,&right_speed_now);
			
	
			
			
			//电机控制 10ms一次
			if(pid_flag == 2){
					//选择电机方向
				motor_direction();
				TIM_SetCompare1(TIM1,PID_Loc(speed_left_SetPoint , left_speed_now, &left_PID));
				TIM_SetCompare2(TIM1,PID_Loc(speed_right_SetPoint , right_speed_now, &left_PID));	
				pid_flag = 0;
			}
			
			
			
			
			//标定使用
			positionNow += (F103RC_chassis.rightSpeedNow*0.005 + F103RC_chassis.leftSpeedNow*0.005 ) / 2;
			
			
		}
}

主函数内调用PID_init与goto_1m即可，可以先把车架起来，看车转一会有没有减慢转速并慢慢停下，如果dubug查看positionNow快到100时候减速不明显（实际上运行会冲过头），可以减小p。

最终放到地上可以看到小车加速到达快100cm时减速，最后保持，如果超了回倒车。最后观察保持的位置与初位置关系，来修改编码器速度计算公式，为其乘个系数。

注意，这里开始就涉及pid的输出正负问题，因为我们的电机的方向时选择好的，根据速度正负，所以速度pid的结果直接用即可，负的速度就是倒转，但是speed_PID的输出，也就是占空比可没有负值，因此我们需要取绝对值

同时对于速度我们限制上下线，这里看自己的需求，我一般会限制最大速度50cm/s。

---

总结

这样我们的车也符合实际了