全国大学生智能汽车大赛（二）：电感采样、卡尔曼滤波、方向控制代码

全国大学生智能汽车大赛（一）：摄像头识别赛道代码

全国大学生智能汽车大赛（二）：电感采样、卡尔曼滤波、方向控制代码

全国大学生智能汽车大赛（三）：上下位机通信协议及代码

以下为正文‍️

一、电感采样函数

/*******************************
 *舵机控制
 *电感采样
 *最终修改日期：2021.6.20
 *TC264D
*******************************/
#include "headfile.h"

//向上位机发送信息用
int x_error_int,y_error_int;

direction_param steer;

kalman_param kfp0;
kalman_param kfp1;
kalman_param kfp2;
kalman_param kfp3;
kalman_param kfp4;
kalman_param kfp5;
kalman_param kfp6;



//角度环初始化
void direction_init(void)
{
    steer.xp_line = 1;         //x_P
    steer.xd_line = 0.05;      //x_D
    steer.yp_line = 1;         //y_P
    steer.yd_line = 0.05;      //y_D

    steer.xp_turn = 2;         //x_P
    steer.xd_turn = 0.03;      //x_D
    steer.yp_turn = 2;         //y_P
    steer.yd_turn = 0.03;      //y_D

    steer.xp_circle = 0.5;     //x_P
    steer.xd_circle = 0.03;    //x_D
    steer.yp_circle = 6;       //y_P
    steer.yd_circle = 0.03;    //y_D

    steer.ll = 0;//最左边
    steer.l  = 0;//次左
    steer.lm = 0;//左靠中
    steer.m  = 0;//中间
    steer.rm = 0;//右靠中
    steer.r  = 0;//次右
    steer.rr = 0;//最右边

    steer.x_error     = 0;
    steer.x_bigerror  = 0;
    steer.x_error_pre = 0;
    steer.y_error     = 0;
    steer.x_bigerror  = 0;
    steer.y_error_pre = 0;

    steer.all = 0;
    steer.x_left = 0;
    steer.x_right = 0;

    steer.conpenstte = 0;
    steer.conpenstte_total=1000;
    steer.conpenstte_fw=50;
    steer.conpenstte_add=10;
    steer.direct_flag = 0;

    steer.circle_flag = 0;
    steer.circle_stage = 0;
    steer.circle_direct = 0;

    steer.trident_flag = 0;
    steer.trident_time = 0;
    steer.trident_stage = 0;

    steer.out = 0;
 }

//卡尔曼滤波变量初始化
void kfp_init(void)
{
    kfp_init_son(&kfp0);
    kfp_init_son(&kfp1);
    kfp_init_son(&kfp2);
    kfp_init_son(&kfp3);
    kfp_init_son(&kfp4);
    kfp_init_son(&kfp5);
    kfp_init_son(&kfp6);
}
void kfp_init_son(kalman_param *kfp0)
{
    {
        kfp0->LastP = 0.02;
        kfp0->Now_P = 0;
        kfp0->out   = 0;
        kfp0->Kg    = 0;
        kfp0->Q     = 0.001;
        kfp0->R     = 0.543;
    }
}

二、卡尔曼滤波

/*************************************************************************
*  功能说明：卡尔曼滤波
*  修改时间：2021年5月26日
*************************************************************************/
int kalman_filter(kalman_param *kfp, int input)
{
     
     kfp->Now_P = kfp->LastP + kfp->Q;
     //卡尔曼增益方程差
     kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
     //更新最优值方程
     kfp->out = kfp->out + kfp->Kg * (input-kfp->out);
     //更新协方差方程
     kfp->LastP = (1-kfp->Kg) * kfp->Now_P;

     return kfp->out;
}

三、采集电感电压并归一化

/*************************************************************************
*  功能说明：七个电感ADC初始化函数；
*  修改时间：2021年5月18日
*************************************************************************/
void inductor_init (void)
{
    adc_init(ADC_0, ADC0_CH0_A0);
    adc_init(ADC_0, ADC0_CH1_A1);
    adc_init(ADC_0, ADC0_CH2_A2);
    adc_init(ADC_0, ADC0_CH3_A3);
    adc_init(ADC_0, ADC0_CH4_A4);
    adc_init(ADC_0, ADC0_CH5_A5);
    adc_init(ADC_0, ADC0_CH6_A6);
    adc_init(ADC_0, ADC0_CH7_A7);
}
/*************************************************************************
*  函数名称：void inductor_measure(void)
*  功能说明：采集电感电压并归一化；
*  修改时间：2021年5月19日
*************************************************************************/
void inductor_measure (void)
{

    steer.ll = adc_mean_filter(ADC_0, ADC0_CH0_A0, ADC_12BIT,5);       // 第1个电感，与赛道平行
    steer.l  = adc_mean_filter(ADC_0, ADC0_CH1_A1, ADC_12BIT,5);       // 第2个电感，与赛道垂直
    steer.lm = adc_mean_filter(ADC_0, ADC0_CH5_A5, ADC_12BIT,5);       // 第3个电感，与赛道平行
    steer.m  = adc_mean_filter(ADC_0, ADC0_CH2_A2, ADC_12BIT,5);       // 第4个电感，与赛道平行
    steer.rm = adc_mean_filter(ADC_0, ADC0_CH6_A6, ADC_12BIT,5);       // 第5个电感，与赛道平行
    steer.r  = adc_mean_filter(ADC_0, ADC0_CH3_A3, ADC_12BIT,5);       // 第6个电感，与赛道垂直
    steer.rr = adc_mean_filter(ADC_0, ADC0_CH4_A4, ADC_12BIT,5);       // 第7个电感，与赛道平行
    //BatVolt = ADC_Read(10);  // 刷新电池电压

//    int i = 0;
//    if (L_ADC[0] < ad_min[0])        ad_min[0] = L_ADC[0];     // 刷新最小值
//    else if (L_ADC[0] > ad_max[0])   ad_max[0] = L_ADC[0];     // 刷新最大值
//    if (L_ADC[1] < ad_min[1])        ad_min[1] = L_ADC[1];
//    else if (L_ADC[1] > ad_max[1])   ad_max[1] = L_ADC[1];
//    if (L_ADC[2] < ad_min[2])        ad_min[2] = L_ADC[2];
//    else if (L_ADC[2] > ad_max[2])   ad_max[2] = L_ADC[2];
//    if (L_ADC[3] < ad_min[3])        ad_min[3] = L_ADC[3];
//    else if (L_ADC[3] > ad_max[3])   ad_max[3] = L_ADC[3];
//    if (L_ADC[4] < ad_min[4])        ad_min[4] = L_ADC[4];
//    else if (L_ADC[4] > ad_max[4])   ad_max[4] = L_ADC[4];
//    if (L_ADC[5] < ad_min[5])        ad_min[5] = L_ADC[5];
//    else if (L_ADC[5] > ad_max[5])   ad_max[5] = L_ADC[5];
//    if (L_ADC[6] < ad_min[6])        ad_min[6] = L_ADC[6];
//    else if (L_ADC[6] > ad_max[6])   ad_max[6] = L_ADC[6];
//    for (i=0; i<7; i++)
//    {
//        L_normal[i] = (L_ADC[i] - ad_min[i]) * 100 / (ad_max[i] - ad_min[i]);        // 各偏移量归一化到0--100以内
//    }
//    Magnetic = L_normal[0] + L_normal[1] + L_normal[2] + L_normal[3]+ L_normal[4] + L_normal[5] + L_normal[6]; // 磁场整体强度
//    Magnetic_left  = L_normal[0] + L_normal[1] + L_normal[2];    // 左前侧磁场整体强度
//    Magnetic_right = L_normal[4] + L_normal[5] + L_normal[6];    // 右前侧磁场整体强度

四、 推荐IO分配

摄像头：8个数据口，一个串口，两eru中断
        数据口：00_0 00_1 00_2 00_3 00_4 00_5 00_6 00_7
        配置串口：摄像头RX：02_2     摄像头TX：02_3
        VSY：02_0
        HREF：程序不需要，所以不接
        PCLK：02_1

        四路运放 A0 A1 A2 A3 等

四个编码器：
    LSB：33_7   DIR：33_6
    LSB：02_8   DIR：00_9
    LSB：10_3   DIR：10_1
    LSB：20_3   DIR：20_0

8路pwm：

        21_2 21_3 21_4 21_5 02_4 02_5 02_6 02_7

ICM20602：
    CLK：    P20_11  
    MOSI：    P20_14 
    MISO：    P20_12 
    CS：    P20_13
    SPI0

TFT屏幕：
    CLK     15_3 
    MOSI     15_5 
    MISO     15_4    //实际上TFT没有这个引脚 这里仅仅占位而已  
    CS0     15_2 
    BL         15_4     //复用控制背光
    REST     15_1 
    DC         15_0  
    SPI2

舵机：P33_9

        尽量不要使用以下引脚，以下引脚属于boot引脚，不合理的使用容易导致单片机无法启动等问题，因此建议大家尽量不要使用：
        P14_2 
        P14_3
        P14_4
        P14_5
        P14_6 
        P10_5 
        P10_6