STM32

超强自适应小车循迹算法开发全过程

2019-07-21 04:49发布

站内问答 / STM32/STM8
15295 25 1150
本帖最后由 xcc521 于 2018-6-20 11:37 编辑

看了好几个循迹的程序感觉都不太好，得需要调二值化阈值，还不抗干扰
然后就是开始自己写了

立一个FLAG，开始搞事情

2018.06.20，第一天

分析整体架构，罗列问题，暂定一个框架，先上一个别人的程序，11路光敏电阻采集灰度值，PID控制占空比驱动舵机转向控制方向

#include  "control.h"
#include         "nrf.h"
#include "PWM.h"
#include "adc.h"

/*************************定义变量****************************/
u16 Location_Data_Arrays[11]={0,0,0,0,0,0,0,0,0,0,0};//位置信息数据 ，传感器的电压值

u16 Manual_Location_Data_Arrays[11]={738  ,703 , 808 , 578 , 848 , 712 , 574 , 551 , 635 , 624 , 648  };//手动设置数组，传感器黑白线判断，黑白线阈值

float G_Black_Line_Arry[5];
float  G_Black_Line_Location=0;//黑线位置

float L_Location_Cnt=0;//位置计数

float Location_Parameter=0;//位置参数 计算偏移量

u32 Cross_Line_Cnt=0;

u8 Cross_Line=0;

u8 Cross_Line_Flag=0;

int  TRUN_ANGLE=  100;
//定义PID相关参数
typedef struct PID_OUT
{
        float DIR_Deviation;//误差
        float LAST_DIR_Deviation;//上次误差
        float PREV_DIR_Deviation;
        float OUTPUT;//输出量
        float P;
        float I;
        float D;

}PID;

/*****************************************************
函数名称：Line_Scan
输入：每个传感器采集到的值
输出：黑线的位置和横线的个数
功能：循线函数
****************************************************/
void Line_Scan(void)
{/***********************************
        注释部分是用来打印采集到的传感器数值
        ************************************/
//        int j=0;
        Location_Data_Arrays[0]=Get_Adc(ADC1,ADC_Channel_10);
        Location_Data_Arrays[1]=Get_Adc(ADC1,ADC_Channel_11);
        Location_Data_Arrays[2]=Get_Adc(ADC1,ADC_Channel_12);
        Location_Data_Arrays[3]=Get_Adc(ADC1,ADC_Channel_13);
        Location_Data_Arrays[4]=Get_Adc(ADC1,ADC_Channel_0);
        Location_Data_Arrays[5]=Get_Adc(ADC1,ADC_Channel_1);
        Location_Data_Arrays[6]=Get_Adc(ADC1,ADC_Channel_2);
        Location_Data_Arrays[7]=Get_Adc(ADC1,ADC_Channel_3);
        Location_Data_Arrays[8]=Get_Adc(ADC1,ADC_Channel_4);
        Location_Data_Arrays[9]=Get_Adc(ADC1,ADC_Channel_5);
        Location_Data_Arrays[10]=Get_Adc(ADC1,ADC_Channel_14);               
//        for(;j<11;j++)
//        {
//                printf("%d  ",Location_Data_Arrays[j]);
//        }
//        printf(" ");
}

void Auto_ScanBlackLine(void)
{
        int i=0;
        Line_Scan();
        for(;i<11;i++)
        {
                Manual_Location_Data_Arrays=Manual_Location_Data_Arrays*1.5;
        }
}
void Lock_BlackLine(void)
{
/*0*/
        if(Location_Data_Arrays[0]<Manual_Location_Data_Arrays[0])
                {
                        L_Location_Cnt++;
                        Location_Parameter+=5;               
                }
               
/*1*/
        if(Location_Data_Arrays[1]<Manual_Location_Data_Arrays[1])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=4;
        }
        
/*2*/
        if(Location_Data_Arrays[2]<Manual_Location_Data_Arrays[2])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=3;
        }

/*3*/
        if(Location_Data_Arrays[3]<Manual_Location_Data_Arrays[3])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=2;
        }

/*4*/
        if(Location_Data_Arrays[4]<Manual_Location_Data_Arrays[4])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=1;
        }

        /*5*/
        if(Location_Data_Arrays[5]<Manual_Location_Data_Arrays[5])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=0;
        }
        /*6*/
        if(Location_Data_Arrays[6]<Manual_Location_Data_Arrays[6])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=-1;
        }
        /*7*/
        if(Location_Data_Arrays[7]<Manual_Location_Data_Arrays[7])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=-2;
        }
        /*8*/
        if(Location_Data_Arrays[8]<Manual_Location_Data_Arrays[8])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=-3;
        }
        /*9*/
        if(Location_Data_Arrays[9]<Manual_Location_Data_Arrays[9])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=-4;
        }
        /*10*/
        if(Location_Data_Arrays[10]<Manual_Location_Data_Arrays[10])
        {
                        L_Location_Cnt++;
                        Location_Parameter+=-5;
        }
        if(L_Location_Cnt>0)
        {
                        if(L_Location_Cnt>4)
                        {
                                Cross_Line_Cnt++;
                                if(Cross_Line_Cnt>CROSSLINE_Time)
                                {
                                        if(Cross_Line_Flag!=1)
                                        {
                                                Cross_Line++;
                                        }
                                        G_Black_Line_Location=(float)(Location_Parameter/L_Location_Cnt);
                                        Cross_Line_Flag=1;
                                        Cross_Line_Cnt=0;
                                }               
                        }        
                        else
                        {
                                Cross_Line_Cnt=0;
                                Cross_Line_Flag=2;
                                G_Black_Line_Location=(float)(Location_Parameter/L_Location_Cnt);
                        }               
        }
        else if(L_Location_Cnt==0)//丢失
        {
                G_Black_Line_Location=0;
                Cross_Line_Flag=0;
        }
                //printf("%d ",Cross_Line);
                Location_Parameter=0;
                L_Location_Cnt=0;
}

u16 ANGLE_CONEVERT(int ANGLE)
{
                u16 angle=0;
                if(ANGLE>100)         {ANGLE=100;}
                if(ANGLE<-100) {ANGLE=-100;}
                angle=ANGLE+100;
                angle*=10;
                angle+=500;//角度转化
                return angle;
}
/*****************************************************
函数名称：Diraction_Contorl
输入：黑线所在位置的偏差
输出：无
功能：方向控制函数
****************************************************/

void Diraction_Contorl(float Black_Line)
{
        int In_Servor=0;
                        PID PID_OUT;
        
                        PID_OUT.P=-75;                // 比例参数
                        PID_OUT.I=0.7;                        //积分参数
                        PID_OUT.D=2.5;                //微分参数
        
                        PID_OUT.DIR_Deviation=Black_Line;//获取偏差
                        PID_OUT.OUTPUT=        PID_OUT.P*PID_OUT.DIR_Deviation   
                                                                                        -2*PID_OUT.LAST_DIR_Deviation*PID_OUT.I
                                                                                        +PID_OUT.D*PID_OUT.PREV_DIR_Deviation;
        
                        if(PID_OUT.OUTPUT>800)         {PID_OUT.OUTPUT=800;}
                        if(PID_OUT.OUTPUT<-800) {PID_OUT.OUTPUT=-800;}//对输出限幅，防止超调损坏舵机
                        
                        In_Servor=(int)PID_OUT.OUTPUT*RATIO;                                        //对输出进行放大
                        In_Servor+=1500;                                                                                                                        //转化为舵机转动的偏角

                        TIM_SetCompare3(TIM4,In_Servor);                                                        //设置舵机工作脉冲
                                
                        PID_OUT.PREV_DIR_Deviation=PID_OUT.LAST_DIR_Deviation;        //保存前两个节拍时的偏差和前一个节拍时的偏差
                        PID_OUT.LAST_DIR_Deviation=PID_OUT.DIR_Deviation;                                //保存当前节拍时的偏差
                        if(Cross_Line==3)//判断横线的个数  
                        {
                                        Speed_Set(0,0);        //停止
                        }
                        
}               

               


再上自己写的

今天写了一点
/*************************定义变量****************************/
#define Photosensitive_resistance                                                0                                                //光敏电阻
#define Infra_red                                                                        1                                                //红外对管
#define Linear_CCD                                                                         2                                                //线性CCD
#define Camera                                                                                3                                                //摄像头
#define Electromagnetism                                                                4                                                //电磁

#define sensor_quantity                                                            11                                              //识别传感器数量
#define sensor_type                                                                  Photosensitive_resistance             //光敏电阻类型的传感器
#define default_position (sensor_quantity-1)/2//默认中间位置传感器编号

#define Ground_Type                                                                0                                                //地面类型0:黑地白线,1:白地黑线

uint16_t current_sonsor_data[sensor_quantity];                                                                    //传感器数据缓存
/*
        获取传感器数据保存到current_sonsor_data
        data_:传感器读取到的数据数组
*/
void get_Sonsor_data(uint16_t * data_)
{
        uint8_t value;
        for(value=0;value<sensor_quantity;value++)
        {
                current_sonsor_data[value] = *(data_ + value);
        }
}

uint8_t gen_Sensor_state(uint16_t * sensor_data,uint16_t * default_sensor_data)
{
        uint8_t value;
        switch(sensor_type)
        {
                case Photosensitive_resistance:                                                                                                //光敏电阻类型的传感器
                        if(0 == Ground_Type)
                        {
                                //
                        }
                        else
                        {
                                //
                        }
                        break;
                case Infra_red:                                                                                                                        //红外传感器
                        break;
                case Linear_CCD:                                                                                                               //线性CCD
                        break;
                case Camera:                                                                                                                    //摄像头
                        break;
                case Electromagnetism:                                                                                                     //电磁
                        break;
                default:
                        break;
        }
}


PS:Keil上排好版的到这就对不齐了，不太适应哎。。。

先从光敏电阻开始，之前写过一个自己计算阈值的，然后自己比较误差最大的传感器编号，这样哒
int my_abs(int num)
{
        return num>=0?num:(-num);
}
uint16_t no_line_data[9]={3324,2989,2892,2368,2919,2439,2981,3310,2375};
uint16_t line_data[9]={4095,4095,4095,3780,4095,3714,4095,4095,3194};

uint16_t calc_line(void)
{
        //
        uint16_t sensor = 0;
        int error = 385;
        uint8_t temp;
        for(temp = 0;temp < 9;temp++)
        {
                if(my_abs(AVG_AD[temp]-no_line_data[temp])<error)
                {
                        sensor &= (~1 << ( 8 - temp ));
                        sensor |= ( 0 << ( 8 - temp ));
                        printf("0_");
                }
                else if(my_abs(AVG_AD[temp]-line_data[temp])<error)
                {
                        sensor &= (~1 << ( 8 - temp ));
                        sensor |= ( 1 << ( 8 - temp ));
                        printf("1_");
                }
                else
                {
                        printf("X_");
                }
        }
        printf(" ");
        return sensor;
}

uint16_t test_data[9];//自动计算的灰度平均值

void Test_Line(void)
{
        int times = 150;
        uint8_t val;
        MinMaxNormalization(TrackingArry,9,50);
        HAL_Delay(200);
        for(val=0;val<9;val++)
        {
                test_data[val] = AVG_AD[val];
        }
        while(times--)
        {
                MinMaxNormalization(TrackingArry,9,50);
                for(val=0;val<9;val++)
                {
                        test_data[val] += AVG_AD[val];
                        test_data[val] /= 2;
                }
                HAL_Delay(5);
        }
}

uint8_t error_test(void)//判断线的位置
{
        uint8_t val,error_num;
        int error[9],cur_max;
        MinMaxNormalization(TrackingArry,9,50);
        HAL_Delay(5);
#if LINE_COLOR        //WHITE
        for(val=0;val<9;val++)
        {
                error[val] = test_data[val] - AVG_AD[val];//计算误差
                printf("%d_",error[val]);
        }
        if((test_data[4]-AVG_AD[4] >= 600) && (test_data[5]-AVG_AD[5] >= 600) && (test_data[6]-AVG_AD[6] >= 600))
        {
          return 0XFF;
        }
#else                                                //BLACK
        for(val=0;val<9;val++)
        {
                error[val] = AVG_AD[val] - test_data[val];//计算误差
                printf("%d_",error[val]);
        }
#endif
        printf(" ");
        
        for(val=0;val<8;val++)//对误差进行排序
        {
                if(val == 0)
                {
                        error_num = error[1]>error[0]?2:1;
                        cur_max = error[1]>error[0]?error[1]:error[0];
                }
                else
                {
                        if(error[val+1]>cur_max)
                        {
                                error_num++;
                        }
                }
        }
        
        printf("error_num:%d_ ",error_num);
        return error_num;
}


大框架里面暂时还是用这个吧，后面有时间再重写，今天先到这，明天继续

友情提示: 此问题已得到解决，问题已经关闭,关闭后问题禁止继续编辑，回答。
25条回答
xcc521
2019-07-22 21:44
 精彩回答 2  元偷偷看……0人看过

一周热门 更多>