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

liutangplease

发布时间 2022.10.13阅读数 2252 评论数 0

一、电感采样函数

/*******************************
*舵机控制
*电感采样
*最终修改日期：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

卡尔曼滤波运动控制智能车竞赛

打赏 0

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

下一篇：应用zigbee实现智能家居项目

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

liutangplease

一、电感采样函数

二、卡尔曼滤波

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

四、推荐IO分配

为你推荐

智能车复工日记【N】：图像处理——环岛debug记录（持续更新）

全国大学生恩智浦杯智能汽车竞赛

北邮智能车仿真培训（六）—— 修改模型参数

智能车浅谈——方向控制篇

Arduino智能小车系列教程1——拼装篇

精选智能车入门——电磁循迹原理与实现

关于作者

liutangplease

34

0

150

2

机械臂速成小指南（零）：指南主要内容及分析方法

PLC学习笔记（二）：PLC结构（1）

机械臂速成小指南（二）：机械臂的应用

相关推荐

智能车浅谈——图像篇

智能车百度组巡航任务采集图片预处理(拼接多次采集的数据)

智能车仿真 —— 2020室外光电创意组线上仿真赛(二)

一些做“飞卡智能车”时候的总结

基于paddledetection在ROS中搭建红绿灯检测控制车模运动（1）—— 环境搭建准备以及软件安装

关于比赛中的识别任务：yolov5

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

谢谢谢谢

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

liutangplease

一、电感采样函数

二、卡尔曼滤波

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

四、 推荐IO分配

为你推荐

智能车复工日记【N】：图像处理——环岛debug记录（持续更新）

全国大学生恩智浦杯智能汽车竞赛

北邮智能车仿真培训（六）—— 修改模型参数

智能车浅谈——方向控制篇

Arduino智能小车系列教程1——拼装篇

精选智能车入门——电磁循迹原理与实现

评论（0）

关于作者

liutangplease

34

0

150

2

机械臂速成小指南（零）：指南主要内容及分析方法

PLC学习笔记（二）：PLC结构（1）

机械臂速成小指南（二）：机械臂的应用

相关推荐

智能车浅谈——图像篇

智能车百度组巡航任务采集图片预处理(拼接多次采集的数据)

智能车仿真 —— 2020室外光电创意组线上仿真赛(二)

一些做“飞卡智能车”时候的总结

基于paddledetection在ROS中搭建红绿灯检测控制车模运动（1）—— 环境搭建准备以及软件安装

关于比赛中的识别任务：yolov5

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

谢谢谢谢

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

四、推荐IO分配

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板