STM32

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

2019-07-21 04:49发布

站内问答 / STM32/STM8
15392 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 09:27
本帖最后由 xcc521 于 2018-6-22 14:49 编辑

占楼更新
2018.06.22
/*************************定义变量****************************/
#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 ground_data[sensor_quantity];                                //地面采集的值(黑地白线时为黑 {MOD}地面上的采集值，白地黑线上时为白地上的采集值),必要时调用采集函数采集该值
int16_t         line_D_value[sensor_quantity];                                //保存(当前)采集值和地面值之间的差值
uint16_t current_sonsor_data[sensor_quantity];//传感器当前采集数据缓存
float          calc_Proportion = 0.6f;                                                        //误差有效计算比例,越大容错性越强，精确度越低
uint16_t * ADC_Value;
/*
        获取传感器数据保存到current_sonsor_data
        data_:传感器读取到的数据数组
        type:7:是否计算当前测量偏差（0:不计算，1:计算）
                         6:偏差计算方式（0:当前减去基准，1:基准减去当前）
                         5-1:保留
                         0:获取类型（0:正常采集数据，1:采集基准(地面)数据）
*/
void get_Sonsor_data(uint16_t * data_,uint8_t type)
{
        uint8_t value;
        for(value=0;value<sensor_quantity;value++)
        {
                switch(type & 0xFE)
                {
                        case 0:                                                                                                                                                                                //采集正常数据，做线分析使用
                                current_sonsor_data[value] = *(data_ + value);
                                break;
                        case 1:                                                                                                                                                                                //采集地面数据，做对比基准使用
                                ground_data[value] = *(data_ + value);
                                break;
                        default:
                                break;
                }
                if(type & 0x80)
                {
                        if(type & 0x40)
                        {
                                line_D_value[value] = ground_data[value] - current_sonsor_data[value];
                        }
                        else
                        {
                                line_D_value[value] = current_sonsor_data[value] - ground_data[value];
                        }
                }
        }
}

/*
        获取传感器检测状态
        sensor_data:当前传感器数据
        default_sensor_data:基准数据
        type:硬件分压方式 0：光照-AD正比例 1：光照-AD反比例
*/
uint32_t get_Sensor_state(uint16_t * sensor_data,uint16_t * default_sensor_data,uint8_t type)
{
        uint8_t value = 0;
        uint8_t temp = 0;
        uint32_t res = 0;
        switch(sensor_type)
        {
                case Photosensitive_resistance:                                                                                                                //光敏电阻类型的传感器
                        if(0 == Ground_Type)                                                                                                                                                //绿地白线情况下计算线位置
                        {
                                if(0 == type)
                                {
                                        for(temp = 0;temp < sensor_quantity;temp++)
                                        {
                                                if(current_sonsor_data[temp] - ground_data[temp] >= line_D_value[temp] * calc_Proportion)        //当前结果比地面大于误差的60%比例
                                                {
                                                        res |= 1 << temp;                                                                                                                                                                                                                                                                                                //对应位设为1，表示白线
                                                }
                                                                                                                                                                                                                                                                                                                                                                                                                        //否则不设1为0，表示黑线
                                        }
                                }
                                else
                                {
                                        for(temp = 0;temp < sensor_quantity;temp++)
                                        {
                                                if(ground_data[temp] - current_sonsor_data[temp] >= line_D_value[temp] * calc_Proportion)        //当前结果比地面大于误差的60%比例
                                                {
                                                        res |= 1 << temp;                                                                                                                                                                                                                                                                                                //对应位设为1，表示白线
                                                }
                                                                                                                                                                                                                                                                                                                                                                                                                        //否则不设1为0，表示黑线
                                        }
                                }
                        }
                        else                                                                                                                                                                                                                                                                                                                                                                                //黑地白线
                        {
                                if(0 == type)
                                {
                                        for(temp = 0;temp < sensor_quantity;temp++)
                                        {
                                                if(current_sonsor_data[temp] - ground_data[temp] >= line_D_value[temp] * calc_Proportion)        //当前结果比地面大于误差的60%比例
                                                {
                                                        res |= 1 << temp;                                                                                                                                                                                                                                                                                                //对应位设为1，表示白线
                                                }
                                                                                                                                                                                                                                                                                                                                                                                                                        //否则不设1为0，表示黑线
                                        }
                                }
                                else
                                {
                                        for(temp = 0;temp < sensor_quantity;temp++)
                                        {
                                                if(ground_data[temp] - current_sonsor_data[temp] >= line_D_value[temp] * calc_Proportion)        //当前结果比地面大于误差的60%比例
                                                {
                                                        res |= 1 << temp;                                                                                                                                                                                                                                                                                                //对应位设为1，表示白线
                                                }
                                                                                                                                                                                                                                                                                                                                                                                                                        //否则不设1为0，表示黑线
                                        }
                                }
                        }
                        break;
                case Infra_red:                                                                                                                                                                                //红外传感器
                        break;
                case Linear_CCD:
                        break;
                case Camera:
                        break;
                case Electromagnetism:
                        break;
                default:
                        break;
        }
        return res;
}

/*
        采集地面数据
        times：采集次数
        delay：采集时间间隔
*/
void get_Ground_data(uint8_t times,uint8_t delay)
{
        uint8_t val;
        ADC_Value = (uint16_t *)(ADC_Data);
        for(val = 0;val < times;val ++)                                                                                                                                        //计算10次背景平均值
        {
                uint8_t temp;
                get_ADC_Value();                                                                                                                                                                                //获取第一次传感器数据
                get_Sonsor_data(ADC_Value,0x01);
                ground_data[temp] += ADC_Value[temp];
                ground_data[temp] /= 2;
                delay_ms(delay);
        }
}

/*
        获取最大误差
        times:测试的次数
        delay:大概测量周期
*/
void get_D_error_value(uint8_t times,uint8_t delay)
{
        uint8_t val;
        ADC_Value = (uint16_t *)(ADC_Data);
        uint16_t max_val[sensor_quantity],min_val[sensor_quantity];
        get_Ground_data(100,5);                                                                                                                                                                //采集地面平均值作为比较基准
        for(val = 0;val < 10;val ++)                                                                                                                                        //当作基准数据
        {
                max_val[val] = min_val[val] = current_sonsor_data[val];
        }
        while(times--)
        {
                get_ADC_Value();                                                                                                                                                                                //获取传感器数据
                for(val = 0;val < sensor_quantity;val++)
                {
                        if(*(ADC_Value+val) > current_sonsor_data[val])
                        {
                                max_val[val] = *(ADC_Value+val);
                        }
                        else if(*(ADC_Value+val) < current_sonsor_data[val])
                        {
                                min_val[val] = *(ADC_Value+val);
                        }
                }
                delay_ms(delay);
        }
        for(val = 0;val < sensor_quantity;val++)
        {
                if((max_val[val] != 0)&&(min_val[val] != 0))
                line_D_value[val] = max_val[val] - min_val[val];
        }
        //还有个小BUG，如果第一次测试结果非常特殊，可能会导致最大值或最小值的某些元素为0，因为初始的数据无法判断那个是最小那个是最大，而且也不确定测试的数据一定会比这个大或者小
        //后期可以通过大数据计算的平均值作为比较基准
        //2018.06.22新增10次平均值作为极限值基准比较，防止了元素为0的出现
}

一周热门 更多>