STM32-使用编码器计算WHEELETC小车（各车轮）速度及代码实现

落日滴橙子

已于 2024-03-15 15:24:19 修改

阅读量4.1k

点赞数 23

文章标签： stm32 嵌入式硬件单片机

于 2024-03-11 21:24:13 首次发布

本文链接：https://blog.csdn.net/qq_44104742/article/details/136625142

版权

小车怎么通过读取编码器数值计算车轮速度？

1.读取编码器数值

编码器固定在车轮上，随车轮转动而产生脉冲信号。每转动一定角度，编码器输出一定数量的脉冲。通过测量一定时间内编码器产生的脉冲数量，可以得到车轮的转速。

2.数值极性判断

不同的车型或安装方式可能会导致编码器读数的正负方向不同。因此，需要根据小车的具体情况，调整编码器数值的极性，以确保计算出的速度方向正确。

3.转速计算

将编码器的脉冲数转换为车轮的旋转角度或转速。通常，编码器的规格会说明每转一圈对应的脉冲数，通过这个信息可以计算车轮的转速。

这里转速的计算为M法（频率测量法）：这种方法是在一个固定的定时时间内（以秒为单位），统计这段时间的编码器脉冲数，计算速度值。设编码器单圈总脉冲数为C（编码器精度，编码器精度通常指的是编码器每转一圈产生的脉冲数），在时间T0内，统计到的编码器脉冲数为M0，则转速n的计算公式为：

转轴转速 = 单位时间内的计数值 / 编码器总分辨率

如果是减速电机

输出轴转速 = 转轴转速 / 减速比

4.速度计算

最后，根据车轮的直径或周长，可以将车轮的转速转换为车轮的线速度，也就是车轮的移动速度。速度的计算公式为：速度 = 转速 × 车轮周长。其中，车轮周长 = π × 车轮直径。

4. 编码器的使用 — [野火]电机应用开发实战指南—基于STM32 文档

5.代码实现

主要函数

具体到代码实现中，首先通过读取编码器的函数（如Read_Encoder()）获取各个车轮的编码器原始数据。然后，根据小车的型号和安装方式，适当调整数值的极性。最后，利用控制频率（CONTROL_FREQUENCY）、车轮周长（Wheel_perimeter）和编码器精度（Encoder_precision）来计算每个车轮的速度。

MOTOR_A.Encoder=Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;

这里Encoder_A_pr是测量得到的脉冲数，CONTROL_FREQUENCY=100（这里是因为读取脉冲数的时间为10ms读取一次，所以需要将10ms得到的脉冲数*100得到1s内的脉冲数），Wheel_perimeter是车辆轮子的周长，Encoder_precision是编码器倍频后的总分辨率（实际是编码器总分辨率 *减速比）

void Balance_task(void *pvParameters)
{ 
	  u32 lastWakeTime = getSysTickCnt();
    while(1)
    {	
			// This task runs at a frequency of 100Hz (10ms control once)
			//此任务以100Hz的频率运行（10ms控制一次）
			vTaskDelayUntil(&lastWakeTime, F2T(RATE_100_HZ)); 

            //Get the encoder data, that is, the real time wheel speed, 
			//and convert to transposition international units
			//获取编码器数据，即车轮实时速度，并转换位国际单位
			Get_Velocity_Form_Encoder();   

            .....

    }
}

void Get_Velocity_Form_Encoder(void)
{
	  //Retrieves the original data of the encoder
	  //获取编码器的原始数据
		float Encoder_A_pr,Encoder_B_pr,Encoder_C_pr,Encoder_D_pr; 
		OriginalEncoder.A=Read_Encoder(2);	
		OriginalEncoder.B=Read_Encoder(3);	
		OriginalEncoder.C=Read_Encoder(4);	
		OriginalEncoder.D=Read_Encoder(5);	

	  //Decide the encoder numerical polarity according to different car models
		//根据不同小车型号决定编码器数值极性
		switch(Car_Mode)
		{
			case Mec_Car:       Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break; 
			case Omni_Car:      Encoder_A_pr= OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break;
			case Akm_Car:       Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break;
			case Diff_Car:      Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break; 
			case FourWheel_Car: Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr=-OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break; 
			case Tank_Car:      Encoder_A_pr=-OriginalEncoder.A; Encoder_B_pr= OriginalEncoder.B; Encoder_C_pr= OriginalEncoder.C;  Encoder_D_pr= OriginalEncoder.D; break; 
		}
		
		//The encoder converts the raw data to wheel speed in m/s
		//编码器原始数据转换为车轮速度，单位m/s
		MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;  
		MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
		MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; 
}

参数设置

//Encoder data reading frequency
//编码器数据读取频率
#define   CONTROL_FREQUENCY 100

//The encoder octave depends on the encoder initialization Settings
//编码器倍频数，取决于编码器初始化设置
#define   EncoderMultiples  4


//Black tire, tank_car wheel diameter
//黑色轮胎、履带车轮直径
#define	  Black_WheelDiameter   0.065
//#define	  Tank_WheelDiameter 0.047
#define	  Tank_WheelDiameter 0.043

//Motor_gear_ratio
//电机减速比
#define   HALL_30F    30


//Number_of_encoder_lines
//编码器精度
#define		Photoelectric_500 500

计算Encoder_precision

由于

Encoder_precision=EncoderMultiples*Robot_Parament.EncoderAccuracy*Robot_Parament.GearRatio;

结合上面参数可以知道Encoder_precision=4*500*30（（经过倍频后的总分辨率）编码器精度=编码器倍频数*（物理）编码器精度（这里应该是码盘上透光线槽的数目其实就等于分辨率，也叫多少线，较为常见的有5-6000 线）*电机减速比）

经过倍频后的总分辨率）编码器精度=编码器倍频数*（物理）编码器精度（这里应该是码盘上透光线槽的数目其实就等于分辨率，也叫多少线，较为常见的有5-6000 线）。但是这里把电机的减速比也加入进来，其实是由

转轴转速 = 单位时间内的计数值 / 编码器总分辨率

输出轴转速 = 转轴转速 / 减速比

输出轴转速=单位时间内的计数值 / （编码器总分辨率 *减速比）

void Robot_Select(void)
{
	//The ADC value is variable in segments, depending on the number of car models. Currently there are 6 car models, CAR_NUMBER=6
  //ADC值分段变量，取决于小车型号数量，目前有6种小车型号，CAR_NUMBER=6
	Divisor_Mode=2048/CAR_NUMBER+5;
	Car_Mode=(int) ((Get_adc_Average(Potentiometer,10))/Divisor_Mode); //Collect the pin information of potentiometer //采集电位器引脚信息	
  if(Car_Mode>5)Car_Mode=5;

	switch(Car_Mode)
	{
		case Mec_Car:       Robot_Init(MEC_wheelspacing,         MEC_axlespacing,          0,                     HALL_30F, Photoelectric_500, Mecanum_75);            break; //麦克纳姆轮小车
		case Omni_Car:      Robot_Init(0,                        0,                        Omni_Turn_Radiaus_109, HALL_30F, Photoelectric_500, FullDirecion_60);       break; //全向轮小车
		case Akm_Car:       Robot_Init(Akm_wheelspacing,         Akm_axlespacing,          0,                     HALL_30F, Photoelectric_500, Black_WheelDiameter);   break; //阿克曼小车
		case Diff_Car:      Robot_Init(Diff_wheelSpacing,        0,                        0,                     HALL_30F, Photoelectric_500, Black_WheelDiameter);   break; //两轮差速小车
		case FourWheel_Car: Robot_Init(Four_Mortor_wheelSpacing, Four_Mortor_axlespacing,  0,                     HALL_30F, Photoelectric_500, Black_WheelDiameter);   break; //四驱车 
		case Tank_Car:      Robot_Init(Tank_wheelSpacing,        0,                        0,                     HALL_30F, Photoelectric_500, Tank_WheelDiameter);    break; //履带车
	}
	
	
	//Check the parameters//自检相关参数
	switch(Car_Mode)
  {
	 case Mec_Car:       CheckPhrase1=8, CheckPhrase2=14; break; //麦克纳姆轮小车
	 case Omni_Car:      CheckPhrase1=6, CheckPhrase2=10; break; //全向轮小车
	 case Akm_Car:       CheckPhrase1=4, CheckPhrase2=7;  break; //阿克曼小车
	 case Diff_Car:      CheckPhrase1=4, CheckPhrase2=7;  break; //两轮差速小车
	 case FourWheel_Car: CheckPhrase1=8, CheckPhrase2=11; break; //四驱车 
	 case Tank_Car:      CheckPhrase1=4, CheckPhrase2=7;  break; //履带车
  }
}


void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter)
{

....

//motor_gear_ratio
//电机减速比
 Robot_Parament.GearRatio=gearratio; 
//Number_of_encoder_lines
 //编码器精度(编码器线数)	
 Robot_Parament.EncoderAccuracy=Accuracy;
//Encoder value corresponding to 1 turn of motor (wheel)
	//电机(车轮)转1圈对应的编码器数值
	Encoder_precision=EncoderMultiples*Robot_Parament.EncoderAccuracy*Robot_Parament.GearRatio;

...

}

计算Wheel_perimeter

车轮周长=pi*车轮直径

void Robot_Init(double wheelspacing, float axlespacing, float omni_turn_radiaus, float gearratio,float Accuracy,float tyre_diameter) // 
{

	//Diameter of driving wheel
  //主动轮直径	
  Robot_Parament.WheelDiameter=tyre_diameter;       
	
	
	//Driving wheel circumference
  //主动轮周长	
	Wheel_perimeter=Robot_Parament.WheelDiameter*PI;
	
}