智能车电磁循迹模块化（差速舵机通用）

A_Frost

已于 2023-07-03 13:30:09 修改

阅读量3.3k

点赞数 46

分类专栏：智能车文章标签：单片机嵌入式硬件

于 2023-06-19 19:30:38 首次发布

本文链接：https://blog.csdn.net/A_Frost/article/details/130794638

版权

智能车专栏收录该内容

5 篇文章 7 订阅

订阅专栏

电磁循迹是最容易的循迹方式，因为大部分采集滤波等函数都是模块化的函数。但是像要控制的好确并不容易。我自己找代码参考的时候感觉有点乱，由于代码大部分与全局变量有关，顾前顾不到尾，单片机也不一样。这里我以模块化的方式带大家进入电磁循迹的世界，大家暂时不理解的话，可以直接复制我写的模块化函数，调用各自单片机的两个最重要的函数，即控制占空比的函数和采集电感值的函数就行。这也是电磁循迹的基础。整体其实不需要理解太多东西。先把现象做出来，车能动之后再考虑其他，这也是本文的内容。

我以stc32单片机函数为例，用的逐飞的库函数，将代码逐段讲解。

void pwm_duty(PWMCH_enum pwmch, uint32 duty)；
uint16 adc_once(ADCN_enum adcn,ADCRES_enum resolution)

这俩是逐飞的模块化函数，和智能车有关的单片机，逐飞基本有库函数，大家调用很方便的。

循迹三电感，双电感随意，这里对五个电感进行处理

大可将其分成三个阶段，即采集最佳电感值->计算控制值->联系电感值与舵机或电机

以stc32为例子，给出下面代码。

获取电感值

	for (i=0;i<10;i++){
		AD_1[i] =adc_once(Port_L1,ADC_12BIT);
		AD_2[i] =adc_once(Port_L2,ADC_12BIT);
		AD_3[i] =adc_once(Port_L3,ADC_12BIT);
		AD_4[i] =adc_once(Port_L4,ADC_12BIT);
		AD_5[i] =adc_once(Port_L5,ADC_12BIT);
	}//变量均已宏定义或定义，可以自己改名字，主要是思路

adc_once就是库函数，其他单片机类似，之后得到数组

滤波各自喜欢就好，这里可以不用理解原理，调用我的函数即可，这个函数移植是没有难度的

/**
* @brief 滑动平均滤波函数
* @param input输入 output输出 len数组长度 filter_len滤波长度
* @Usuage float input[],output[];
smooth_Data(input,output,5,3);
* @retval none
*/
void smooth_Data(float *input, float *output, int len, int filter_len)//滑动平均
{
    int i, j;
    float sum;

    for (i = 0; i < len; i++) {
        sum = 0;
        for (j = i; j > i - filter_len; j--) {
            if (j >= 0) {
                sum += input[j];
            }
        }
        output[i] = sum / filter_len;
    }
}

我们之后得到了五组电感数据滤波后的数组，但是电磁循迹选一个数据进行操作，怎么办呢？

这里有几种方法取平均，取中间任意项的平均和，取最大最小值的平均值，我这里才用去中间三个的平均值。

float find_best_value(float *arr, int len) //中间三项的平均值
{
    int mid ;  // 数组中间位置
    float sum = 0;      // 初始化总和为0
	int i;
	mid = len / 2;  
    // 计算中间三个数据的总和
    for ( i = mid - 1; i <= mid + 1; i++) {
        sum += arr[i];
    }
    // 计算平均值并返回
    return sum / 3;
}

最佳电感值获取

#define Port_L1 ADC_P17 // L
#define Port_L2  ADC_P14// R
#define Port_L3  ADC_P16//M
#define Port_L4  ADC_P13//VL 
#define Port_L5  ADC_P10//VR

#define SAMPLES 5
#define FILTER_LEN 3
float AD_L=0,AD_R=0,AD_M=0;
float AD_LV=0,AD_RV=0;
void smooth_Data(float *input, float *output, int len, int filter_len)//滑动平均
{
    int i, j;
    float sum;

    for (i = 0; i < len; i++) {
        sum = 0;
        for (j = i; j > i - filter_len; j--) {
            if (j >= 0) {
                sum += input[j];
            }
        }
        output[i] = sum / filter_len;
    }
}
float find_best_value(float *arr, int len) //中间三项的平均值
{
    int mid ;  // 数组中间位置
    float sum = 0;      // 初始化总和为0
	int i;
	mid = len / 2;  
    // 计算中间三个数据的总和
    for ( i = mid - 1; i <= mid + 1; i++) {
        sum += arr[i];
    }
    // 计算平均值并返回
    return sum / 3;
}
void Inductor_Smooth_Filter_Get_val()
{
	int i;
	float sensor_raw1[SAMPLES];
	float sensor_raw2[SAMPLES];
	float sensor_raw3[SAMPLES];
	float sensor_raw4[SAMPLES];
	float sensor_raw5[SAMPLES];

	float filtered_value1[SAMPLES];
	float filtered_value2[SAMPLES];
	float filtered_value3[SAMPLES];
	float filtered_value4[SAMPLES];
	float filtered_value5[SAMPLES];
	for (i=0;i<SAMPLES;i++){
		sensor_raw1[i] =adc_once(Port_L1,ADC_12BIT);
		sensor_raw2[i] =adc_once(Port_L2,ADC_12BIT);
		sensor_raw3[i] =adc_once(Port_L3,ADC_12BIT);
		sensor_raw4[i] =adc_once(Port_L4,ADC_12BIT);
		sensor_raw5[i] =adc_once(Port_L5,ADC_12BIT);
	}   
     smooth_Data(sensor_raw1,filtered_value1,SAMPLES ,FILTER_LEN);
     smooth_Data(sensor_raw2,filtered_value2,SAMPLES ,FILTER_LEN );
     smooth_Data(sensor_raw3,filtered_value3,SAMPLES ,FILTER_LEN );
     smooth_Data(sensor_raw4,filtered_value4,SAMPLES ,FILTER_LEN );
     smooth_Data(sensor_raw5,filtered_value5,SAMPLES ,FILTER_LEN );
        AD_L=find_best_value(filtered_value1,SAMPLES);
        AD_R=find_best_value(filtered_value2,SAMPLES);
        AD_M=find_best_value(filtered_value3,SAMPLES);
        AD_LV=find_best_value(filtered_value4,SAMPLES);
        AD_RV=find_best_value(filtered_value5,SAMPLES);
}

其实大部分学校都有祖传的滤波，这里列举我使用的一种滤波，符合自己的使用习惯就好。

这时我们可以得到用于循迹的电感值AD_L,AD_R,AD_AD_LV,AD_RV那么下一步就是与计算出控制值。

计算控制值

这里给出两个算法。

//三电感循迹算法 计算的值左加右减
float Inductor_Fllow_Trail_Control_Val(float AD_input_L,float AD_input_R,float AD_input_M ,float P,float D)
{
	static float last_error=0;
	float error;
	float output;
	error=(AD_input_M-AD_input_L)/(AD_input_M+AD_input_L)-(AD_input_M-AD_input_R)/(AD_input_M+AD_input_R);
	output=(P*error+D*(error-last_error));
	last_error=error;
	return output;
}
//双电感控制算法
float Inductor_Fllow_Trail_Control_Val_2(float AD_input_L,float AD_input_R,float P,float D)
{
	static float last_error=0;
	float error;
	float output;
	error=(AD_input_R-AD_input_L)/(AD_input_L+AD_input_R);
	output=(P*error+D*(error-last_error));
	last_error=error;
	return output;
}
//这里可以定义三个变量Speed_L,Speed_R和一个Corret_val
//Corret_val=Inductor_Fllow_Trail_Control_Val_2(AD_L,AD_R,0.2,0.5);
//然后Speed_L+Corret_val;Speed_R=+Corret_val;之后给带入占空比函数就可以循迹了
//这样比较好移植
//下面会给出模块化的控制代码

之前算出的AD_L,AD_R,AD_M就有用武之地了，带入我的函数就可以用了。

控制差速或舵机

这里顺便给出我控制占空比的函数。调用的时候宏定义和函数如果不是stc32的话改以下就行了

注意左右是相对的，板子的引脚可能不一样但是，改宏定义改成我的CarForward函数效果就行

float Speed_Right,Speed_Left;
//电机
#define A_in_1 PWMA_CH4N_P17//右电机 
#define A_in_2 PWMA_CH4P_P66
#define B_in_1 PWMA_CH1P_P60
#define B_in_2 PWMA_CH2P_P62

#define Steel_Port PWMB_CH2_P75//舵机
float I_Kp=0.5,I_Kd=0.5;
void Motor_B_RUN(uint8 commond, uint32 MotorDuty1)//左
{ 
	if(MotorDuty1>=8000)
	{
	MotorDuty1=8000;
	}

	 if(commond==2)//commond =2 反转
	{
				pwm_duty(B_in_1,MotorDuty1);       //引脚
				pwm_duty(B_in_2,0);     		//引脚
	}
	else if(commond==1)//commond =1 正转转
	{
				pwm_duty(B_in_1,0);          //引脚
				pwm_duty(B_in_2, MotorDuty1);  	//引脚
	}
}

void Motor_A_RUN(uint8 commond, uint32 MotorDuty2)//右
{ 
	if(MotorDuty2>=8000)MotorDuty2=8000;
	 if(commond==2)//commond =2 反转
	{
			pwm_duty(A_in_1,0);          
            pwm_duty(A_in_2,MotorDuty2);			
	}
	else if(commond==1)//commond =1 正转
	{
			pwm_duty(A_in_1,MotorDuty2);          
            pwm_duty(A_in_2, 0);			
	}
}


//CarForward(speedL,speedR);
void CarForward(int16 A_duty,int16 B_duty)
{
	if(A_duty>=0)Motor_A_RUN(1,A_duty);//output	
	else if(A_duty<=0)Motor_A_RUN(2,-A_duty);	
	if(B_duty>=0)Motor_B_RUN(1,B_duty);
	else if(B_duty<=0)Motor_B_RUN(2,-B_duty);
}

//
float Mid_Steer_val=10.0;//随便给的值，我用的是差速
void Fllow_Trail_Done(float Vl0,float Vr0)
{ 			
	    float Inductance_Control_val=0;
        float Correct_Val;
		Inductance_Control_val=Inductor_Fllow_Trail_Control_Val_2(AD_L,AD_R,I_Kp,I_Kd);//横双电感循迹
	Correct_Val=Inductance_Control_val;
	//差速循迹
	Speed_Right=Vr0-Correct_Val;
	Speed_Left=Vl0+Correct_Val;
	CarForward(Speed_Left,Speed_Right);	
    //舵机循迹
    pwm_duty(Steel_Port ,Mid_Steer_val-Correct_Val);//减还是加可以测以下，我的并不一定准确
    //上舵机试一下，反了就改成pwm_duty(Steel_Port ,Mid_Steer_val+Correct_Val);
}

整体代码框架就是先初始化，逐飞的库函数里面基本都有，之后你先写一个控制速度的函数，其实也不用写，就是直接调用那个占空比控制的函数。我只是写出函数方便嵌套调用罢了。

下面给出主函数

#define Port_L1 ADC_P17 // L
#define Port_L2  ADC_P14// R
#define Port_L3  ADC_P16//M
#define Port_L4  ADC_P13//VL 
#define Port_L5  ADC_P10//VR
#define A_in_1 PWMA_CH4N_P17//右电机 
#define A_in_2 PWMA_CH4P_P66
#define B_in_1 PWMA_CH1P_P60
#define B_in_2 PWMA_CH2P_P62

#define Steel_Port PWMB_CH2_P75//舵机
void All_Init()
{
	//18TFT初始化
	lcd_init();
	//驱动初始化
	pwm_init(A_in_1, 17000, 0); pwm_init(A_in_2, 17000, 0); 
	pwm_init(B_in_1, 17000, 0); pwm_init(B_in_2, 17000, 0);
	//电感采集初始化
	adc_init(Port_L1,ADC_SYSclk_DIV_2);//AD_L
	adc_init(Port_L2,ADC_SYSclk_DIV_2);//AD_R
	adc_init(Port_L3,ADC_SYSclk_DIV_2);//AD_M
	adc_init(Port_L4,ADC_SYSclk_DIV_2);
	adc_init(Port_L5,ADC_SYSclk_DIV_2);
}
float I_Kp=0.5,I_Kd=0.5;//调节参数即可
void main()
{
	EA=0;
	board_init();		
	All_Init();	//bee=0;
	EA=1;
//注意多重调用
       while(1)
    {
        Inductor_Smooth_Filter_Get_val()；
        Fllow_Trail_Done(2000,2000);
    }
}

之后其实就是调参的过程，调代码的时候注意要再.h里面声明以下，不然调用不了，然后float I_Kp=0.5,I_Kd=0.5;变量记得再电感处理的那个文件里面声明。这就是基本的电磁循迹函数了，大家可以在此基础上再加点东西

觉得有用的话给我点个赞呗(≧﹏ ≦)