电感传感器循迹——智能小车

最新推荐文章于 2023-10-12 16:48:20 发布
猛汉轻敲乱码
最新推荐文章于 2023-10-12 16:48:20 发布
阅读量2.3k
点赞数 4
文章标签：单片机 stm32 c语言
本文链接：https://blog.csdn.net/Codes_Master/article/details/117670200
版权
/*
 * @file       main.c
 * @brief      主函数
 */

#include "common.h"
#include "include.h"
#include "time.h"

#define N 1  //滑动滤波窗口
#define n 0 //异常值个数剔除


//130--1,  90--2     142-3
int servor_middle=120;  //舵机摆臂回正的脉宽，需要根据实际情况修改，现在是(155/1000)*10ms=1.55ms 1000是脉冲精度 
int servor_range=20;    //限制一下舵机摆动的幅度，防止打死造成机械损坏（大约正负25度，根据实际情况修改）
int flag=0;

float motor1_out,motor2_out; //电机输出 -0.99···+0.99

float duty_servor=0; //舵机占空比
float duty_motor=0;  //电机占空比

float p_out;
float d_out;


int16 pulse2=0;		//脉冲数
int16 pulse2_last=0;
int16 speed2=0;		//脉冲差数
int16 speedset=-800; 	//速度设定     motor-0.5---speed-1000

int16 value1,value2; //电感值 直接读取
int16 value11,value12,value13,value14,value15,value16,value17,value18;
int16 value21,value22,value23,value24,value25,value26,value27,value28;
int16 Value1=0,Value2=0;  // 滤波之后的电感值
int16 value1_buf[N]; 
int16 value2_buf[N];
int16 sort1_buf[N];
int16 sort2_buf[N];

int16 average;

char buff[10];  //缓冲区

//舵机pid
int16 Kp_servor=8;  //除以1000
int16 Ki_servor=0;   //除以1000
int16 Kd_servor=0 ; //除以1000
int16 errorSer=0;
int16 errorSer_last=0;
int16 errorSer_i=0;
int16 errorSer_d=0;

float errorSer2=0; //使用差值/和
int16 errorSer2Show=0;


//电机pid
int16 Kp_motor=100; // 除以1000
int16 Ki_motor=0;   //除以1000
int16 Kd_motor=0; //除以1000
int16 errorMot=0;
int16 errorMot_last=0;
int16 errorMot_i=0;
int16 errorMot_d=0;


int32 time_count=0; 	//5ms计数器
int flag_5=0;		//舵机标志位
int flag_20=0;		//电机，编码器标志位
int flag_100=0;		//OLED标志位


int count=0;


double period=0;

void PIT_IRQHandler();
void Motor_Out();
void PID_servor();
void PID_motor();
void getBuf(); //得到窗口
void bubbleSort(); //排序
void getValue();  //得出值
	
void  main(void)
{

	///初始化配置
       
	//初始化舵机 FTM1
	FTM_PWM_init(FTM1,FTM_CH0,100,servor_middle);   //舵机 PWM  PTA12输出，频率为100hz
	 

	//初始化电机 FTM0
	
	FTM_PWM_init(FTM0,FTM_CH0,10*1000,0);          // 电机 PWM  PTC1输出，频率为10Khz、
	FTM_PWM_init(FTM0,FTM_CH1,10*1000,0);          // 电机 PWM  PTC2输出，频率为10Khz、
	FTM_PWM_init(FTM0,FTM_CH2,10*1000,0);          // 电机 PWM  PTC3输出，频率为10Khz、
	FTM_PWM_init(FTM0,FTM_CH3,10*1000,0);          // 电机 PWM  PTC4输出，频率为10Khz、
 
	//初始化 OLED
	OLED_Init(); 
	OLED_Draw_Logo();	//显示logo
	DELAY_MS(2000); 	//等待2000ms
	OLED_CLS();			//清空显示

	//初始化编码器2 FTM2
	
	FTM_QUAD_Init(FTM2); //初始化正交解码计数通道

	//初始化电感传感器
	
	adc_init(ADC0_DP1);  //初始化AD模块
	adc_init(ADC0_DM1);  
	adc_init(ADC1_DP1);  
	adc_init(ADC1_DM1);
	
	//开启定时器0，设置5ms进入一次中断函数
	pit_init_ms(PIT0,5); //5ms定时中断
	set_vector_handler(PIT0_VECTORn ,PIT_IRQHandler);//中断向量表
	EnableInterrupts; //打开中断 
	enable_irq (PIT0_IRQn); //使能中断



	///主程序
	while(1)
		{
		
                  
                  
		//控制舵机
		if (flag_5==1)
			{
			flag_5=0;	//清除标志位
			//滑动滤波
			getBuf();
			bubbleSort();
			getValue();
			average=(Value1+Value2)/2;
                                
                     if( Value1<100 || Value2<100)
                     {
                       duty_servor=LIMIT(duty_servor*1.4,servor_range,-servor_range);
                     FTM_PWM_Duty(FTM1,FTM_CH0,servor_middle+(int)duty_servor); 
                     }
                     else {
                        PID_servor();
                       duty_servor=LIMIT(duty_servor*1.4,servor_range,-servor_range);
			// pid 控制一下 duty_servor
			FTM_PWM_Duty(FTM1,FTM_CH0,servor_middle+(int)duty_servor);    //舵机控制输出，在舵机中点附近左右摆动
			
                     }
                        
                        }

            if(Value1>3700 && Value2>3700){
                  
             
               DELAY_MS(110);
                flag=flag+1;
                if(flag==1){
                  
                 duty_servor=-12;
                  FTM_PWM_Duty(FTM1,FTM_CH0,servor_middle+(int)duty_servor);
                   motor1_out=0.2; //转化为实际占空比
			motor2_out=0.2;  
			Motor_Out();
                
                  if ((Value1<=10||Value2<=10)&& average<=200)
			{
			//电机停转
                          while(true){
                        motor1_out=0; //转化为实际占空比
			motor2_out=0;  
			Motor_Out(); 
                          }
			}
                  DELAY_MS(2000);
                  
                  }
                else if (flag==2){
                while(true){
                        motor1_out=0; //转化为实际占空比
			motor2_out=0;  
			Motor_Out(); 
                          }
                }
                
                }
    
		//控制电机
		if(flag_20==1)
			{
			
			flag_20=0; 	//清除标志位
			//控制编码器
			pulse2=FTM_QUAD_get(FTM2);	//获取FTM2模块的读数
			speed2=pulse2-pulse2_last;
			pulse2_last=pulse2;

			//控制电机
			
                        
                        if(abs(errorSer)>800)
                        {
                          speedset=-400;
                        }
                        else
                        {
                          speedset=-650;
                        }
                        
                        
                        
                        
                        
                        
                        
                        
                        
                        //pid控制一下duty_motor
			
                        PID_motor();
                        
                          
                        motor1_out=duty_motor*0.01*1.1; //转化为实际占空比
			motor2_out=duty_motor*0.01*1.1;  
			Motor_Out();
                        
                       
                        
                         //判断一个异常值，理论上不应该产生，出现了直接停
                        
			}
		//控制显示器
		if(flag_100==1)
			{
			flag_100=0; 	//清除标志位
			//显示电机pid
			
                        //显示舵机pid
			OLED_P6x8Str(2,2,"Sp");
			sprintf(buff,"%d",Kp_servor);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(10+8,2,buff); //将数值显示在液晶屏幕上
			OLED_P6x8Char(' '); 		//末尾放个空格防止显示错误（末尾不刷新）
			OLED_P6x8Str(10+32,2,"i");
			sprintf(buff,"%d",Ki_servor);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(10+40,2,buff); //将数值显示在液晶屏幕上
			OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
                        OLED_P6x8Str(10+64,2,"d");
			sprintf(buff,"%d",Kd_servor);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(10+72,2,buff); //将数值显示在液晶屏幕上
			OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
   			
                        OLED_P6x8Str(2,3,"avg");
   			sprintf(buff,"%d",average);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(2+24,3,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
                        OLED_P6x8Str(58,3,"err");
   			sprintf(buff,"%d",errorSer);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(58+24,3,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）

                       

			OLED_P6x8Str(20,4,"Value1=");
   			OLED_P6x8Str(20,5,"Value2=");
   			sprintf(buff,"%d",Value1);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(20+56,4,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
                        sprintf(buff,"%d",Value2);  //将读数Value2转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
                        OLED_P6x8Str(20+56,5,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）

			OLED_P6x8Str(20,6,"set=");
   			OLED_P6x8Str(20,7,"speed=");
   			sprintf(buff,"%d",speedset);	//将读数Value1转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
			OLED_P6x8Str(20+56,6,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
                        sprintf(buff,"%d",speed2);  //将读数Value2转换为字符串 存在buff 里面 不懂的百度 sprintf 函数
                        OLED_P6x8Str(20+56,7,buff); //将数值显示在液晶屏幕上
                        OLED_P6x8Char(' ');         //末尾放个空格防止显示错误（末尾不刷新）
		}

		//跑出跑道
		if ((Value1<=10||Value2<=10)&& average<=200)
			{
			//电机停转
			motor1_out=0; //转化为实际占空比
			motor2_out=0;  
			Motor_Out();

			//调整pid,按钮检测不太会，先空着
                       DELAY_MS(2000); 
			
			}
                
                //停止
                
                
                  
                  


		
               
			   
}                
}

//5ms一次定时中断函数
void PIT_IRQHandler()  
{
    PIT_Flag_Clear(PIT0);       //清中断标志位
    time_count++; 				//计数器增加

	// 更改标志位的值
	flag_5=1;
	if (time_count%4==0)
		{
		flag_20=1;
		}
	if(time_count%20==0)
		{
		flag_100=1;
		}
}

//电机控制输出函数
void Motor_Out() 
{
  
 motor1_out=LIMIT(motor1_out,0.4,-0.4);  //电机的输出值限制在-1~1 之间，-1代表反转最快，1代表正转最快；
 motor2_out=LIMIT(motor2_out,0.4,-0.4);
  //电机1
  if(motor1_out>=0) //占空比为正，正转
  {
     FTM_PWM_Duty(FTM0,FTM_CH0,(int)(motor1_out*10000));//占空比精度为10000 ，占空比*占空比精度
     FTM_PWM_Duty(FTM0,FTM_CH2,0);
  }
  else   //为负就反转
  {
     FTM_PWM_Duty(FTM0,FTM_CH0,0);
     FTM_PWM_Duty(FTM0,FTM_CH2,(int)(-motor1_out*10000));
  }
  
  //电机2
    if(motor2_out>=0) //占空比为正，正转
  {
     FTM_PWM_Duty(FTM0,FTM_CH1,(int)(motor2_out*10000)); 
     FTM_PWM_Duty(FTM0,FTM_CH3,0);
  }
  else   //反转
  {
     FTM_PWM_Duty(FTM0,FTM_CH1,0);
     FTM_PWM_Duty(FTM0,FTM_CH3,(int)(-motor2_out*10000));
  }
}

//舵机pid调整，全部使用全局变量
void PID_servor()
{
	errorSer=Value1-Value2;
	errorSer2= (Value1-Value2)*1.0/(Value1+Value2);
        errorSer2Show=errorSer2*4000;
        errorSer=errorSer2Show;
        
        
	errorSer_i+=errorSer;
	errorSer_d=-errorSer+errorSer_last;
	errorSer_last=errorSer;
        d_out=Kd_servor*errorSer_d;
        p_out=Kp_servor*errorSer;
	duty_servor=Kp_servor*errorSer+Kd_servor*errorSer_d+Ki_servor*errorSer_i;
        duty_servor/=1000;
	duty_servor=-duty_servor;
}

	
void PID_motor()
{
	errorMot=speedset-speed2;
        if (errorMot<=200 && abs(errorMot)<=20000 )
        {
          errorMot_i+=errorMot;
	}
        errorMot_d=errorMot-errorMot_last;
	errorMot_last=errorMot;
          duty_motor=Kp_motor*errorMot+Kd_motor*errorMot_d+Ki_motor*errorMot_i;
          duty_motor/=1000;
	  duty_motor=-duty_motor;
}

//滑动滤波

void getBuf() //建立一个N长的窗口，把数据存储到value*_buf里面
{ 
      int16 count; 
      
      value1=adc_once(ADC0_DP1,ADC_12bit);  //获取电感模块的读数
      value2=adc_once(ADC0_DM1,ADC_12bit);  

      for ( count=0;count<N-1;count++) 
      {
        value1_buf[count]=value1_buf[count+1];
        //sum1+=value1_buf[count+1];
      }
      value1_buf[N-1]=value1;
      //sum1+=value1;
      //Value1=sum1/N;
      
      for ( count=0;count<N-1;count++) 
      {
        value2_buf[count]=value2_buf[count+1];
        //sum2+=value2_buf[count+1];
      }
      value2_buf[N-1]=value2;
      //sum2+=value2;
      //Value2=sum2/N;
}

//排序到 sort*_buf里面
void bubbleSort() 
{
	
    int i, j;
	int16 temp;

	//复制数组
	for(i=0; i<N;i++)
		{

		sort1_buf[i]=value1_buf[i];
		sort2_buf[i]=value2_buf[i];
		}
	//对数组进行排序
    for (i = 0; i < N - 1; i++)
		{
        for (j = 0; j < N - 1 - i; j++)
        	{
			if (sort1_buf[j] > sort1_buf[j + 1]) 
				{
                temp = sort1_buf[j];
                sort1_buf[j] = sort1_buf[j + 1];
                sort1_buf[j + 1] = temp;
        	    }
			if (sort2_buf[j] > sort2_buf[j + 1]) 
				{
                temp = sort2_buf[j];
                sort2_buf[j] = sort2_buf[j + 1];
                sort2_buf[j + 1] = temp;
        	    }
			
		}
	}
}


//去掉N-3个极大，和N-3个极小。剩下地取平均
void getValue()
{
	int64  sum1=0; 
	int64 sum2=0;

	int i=N;
	for (i=n;i<N-n;i++)
		{

		sum1+=sort1_buf[i];
		sum2+=sort2_buf[i];
		}
	Value1=sum1/(N-2*n);
	Value2=sum2/(N-2*n);
}
待补充
猛汉轻敲乱码
关注
4
点赞
踩
41

收藏

觉得还不错? 一键收藏
打赏
0
评论
电感传感器循迹——智能小车

/** @file main.c* @brief 主函数*/#include "common.h"#include "include.h"#include "time.h"#define N 1 //滑动滤波窗口#define n 0 //异常值个数剔除//130--1, 90--2 142-3int servor_middle=120; //舵机摆臂回正的脉宽，需要根据实际情况修改，现在是(155/1000)*10ms=1.55ms 1...
复制链接

扫一扫