智能车竞赛技术报告 | 智能车视觉 -重庆大学 - 风林火山
简 介： 本文主要介绍了基于RT Thread操作系统的智能视觉组四轮循迹智能小车系统的原理、软硬件设计以及小车制作过程，对小车的系统介绍包括车模机械结构的设计、模块电路的设计、传感器信号的处理、控制算法、神经网络算法以及整车调试的方法等。设计以恩智浦公司的单片机RT1064为控制核心，采用数字摄像头MT9V032采集赛道元素信息，增量式编码器获取小车的速度，陀螺仪获取小车姿态。程序基于RT Th
简 介： 本文主要介绍了基于RT Thread操作系统的智能视觉组四轮循迹智能小车系统的原理、软硬件设计以及小车制作过程，对小车的系统介绍包括车模机械结构的设计、模块电路的设计、传感器信号的处理、控制算法、神经网络算法以及整车调试的方法等。设计以恩智浦公司的单片机RT1064为控制核心，采用数字摄像头MT9V032采集赛道元素信息，增量式编码器获取小车的速度，陀螺仪获取小车姿态。程序基于RT Thread操作系统， IAR 作为集成编译环境，使用 无线串口、LCD 、拨码开关和按键作为辅助调试手段。经过大量的软硬件测试，实现了智能小车高速下的稳定循迹运动。
关键词： RT Thread，RT1064，MT9V032，赛道识别，神经网络
队伍名称：风林火山
参赛队员：谢文祥
梁华林
陈俊霖
带队教师：杜基赫 许亨艺
第一章 引 言
全国大学生智能汽车竞赛是由教育部高等学校自动化专业教学指导委员会主办。该竞赛以“立足培养，重在参与，鼓励探索，追求卓越” 为指导思想，旨在促进高等学校素质教育，培养大学生的综合知识运用能力、基本工程实践能力和创新意识。智能车竞赛涉及自动控制、传感技术、电子、电气、计算机、机械与汽车等多个学科，为大学生提供了一个充分展示想象力和创造力的舞台，吸引着越来越多来自不同专业的大学生参与其中，激发了大学生的创新思维，对于其实践、创新能力和团队精神的培养具有十分重要的价值。
全国大学生智能汽车竞赛组织运行模式贯彻“政府倡导、专家主办、学生主体、社会参与”的 16 字方针，充分调动各方面参与的积极性。
RT-Thread，全称是 Real Time-Thread，是一个嵌入式实时多线程操作系统，基本属性之一是支持多任务，任务与任务之间通过任务调度器进行非常快速地切换（调度器根据优先级决定此刻该执行的任务），造成多个任务在一个时刻同时运行的错觉。也是一款主要由中国开源社区主导开发的开源实时操作系统。实时线程操作系统不仅仅是一个单一的实时操作系统内核，它也是一个完整的应用系统，包含了实时、嵌入式系统相关的各个组件：TCP/IP协议栈，libc接口，图形用户界面等。
本文是本届参赛队伍风林火山对最终参赛车模的总体概括，并对制作过程中的技术细节做了展示。本文的参考文献中，《C ++ primer plus》阐述了基本的C++语言语法和程序设计思路，《模拟电子技术基础》详细讲述了电路的基本知识，《嵌入式实时操作系统：RT-Thread设计与实现》对嵌入式系统的开发调试方法做了简介，并对RT-Thread进行了详细的讲解。《智能车制作》对实际制作智能车时的总体方案进行了简要的阐述。
本文将在其后的正文部分按照顺序阐述：
1. 模型车设计制作的主要思路以及实现的技术方案概要说明；
2. 模型车结构设计；
3. 电路设计说明；
4. RT Thread 的运用和控制策略的介绍；
5. 开发工具、制作、安装、调试过程说明；
6. 模型车的主要技术参数说明。
第二章 模型车设计制作思路
2.1主控芯片的选择
AI视觉组限制芯片厂商只能为NXP，故选择官方推荐的RT1064, 该芯片的主频高，可以满足彩色摄像头使用，硬件资源丰富。
▲ 图2.1 RT1064核心板
2.2稳压电路的选择
电源模块为系统其他各个模块提供所需要的电源。设计中，除了需要考虑电压范围和电流容量等基本参数之外，还要在电源转换效率、噪声和干扰等方面进行优化。可靠的电源方案是整个硬件电路稳定可靠运行的基础。
核心板上电存在时序问题，IO不能先于内核上电，否则会导致内核无法启动。可以通过使用CR引脚来控制外围设备供电电路使能。因为当CR引脚变为高电平时，表面内核启动成功。外部电路通过三极管和Mos管设计出合理的电路。整车电源由额定电压7.4V，满电电压8.4V的锂电池供电。通过不同的稳压方案，将电池电压转换成各个模块所需要的电压。
1) 锂电池供电，正常使用时电压在7.4～8.4V。可直接用于电机供电。
2) 通过TPS76850产生5v电压，供给编码器，串口通信，陀螺仪等。
3) 使用稳压芯片AMS1117系列稳压到6V，为转向舵机和数字舵机供电。
4) 通过TPS76833产生3.3v电压，为摄像头供电
▲ 图2.2 稳压电路原理图
2.3驱动电路的选择
使用MOS桥电路驱动电机，为避免电流反冲进芯片，造成芯片损坏，利用隔离芯片为桥电路提供信号。
▲ 图2.3 驱动电路原理图
2.4电流环电路
电流环指的是电流反馈系统。一般指的是将输出电流采用负反馈的方式接入处理环节的方法，来提高电流的稳定性.在使用电流环后，可以通过电流内环度外环相结合来提高系统的性能，提高车运行的稳定性。
▲ 图2.4电流环原理图
2.5传感器的选择
比赛所用的摄像头可以分为两大类： CCD摄像头和CMOS摄像头。CCD摄像头图像对比度高、动态特性好，但供电电压比较高，需要12V的工作电压。耗电严重，拍摄的图像稳定性并不高。CMOS摄像头，体积小，图像稳定性较高，只需3.3V供电，耗电量低。总转风摄像头是一款基于MT9V032芯片设计的CMOS摄像头，是逐飞科技独家研发的一款高性能，在恩智浦竞赛市面上性能最优，最适合高速情况下的图像采集的全局快门摄像头。
▲ 图2.5总转风摄像头
2.6编码器的选择
采用逐飞科技的带方向1024线光电编码器，输出轴上的机械几何位移量转换成脉冲或数字量，实现对速度的测量，从而完成对电机的闭环控制。第三章 模型车机械安装
▲ 图2.6 1024线光电编码器
第三章 传感器安装
3.1摄像头
逐飞科技设计的130度无畸变总钻风广角摄像头，调节镜片中心离地面的高度为30cm ，视角为30°，智能车据此获得了良好且稳定的图像。
▲ 图 3.1 模型车侧视图
3.2编码器
为了实现电机闭环，提高控制精度，本车使用了1024线带方向的编码器，并对编码器传回数据进行简单的滤波处理，在不牺牲处理时间的前提下尽可能提高编码器传回数据的精确度。
3.3系统电路板的固定及连接
C车模由于电机及编码器都在车尾部，重心偏后，所以我们在不影响舵机连杆运动的情况下将电路板尽量靠前安装，通过对硬件的合适处理，减小车的重心到最低，并通过四个电子秤保证车子重心尽可能在车中心。
第四章 电路设计
4.1最小系统部分
在设计主板的时候，主要考虑的原则是电路板足够窄，同时满足电流的要求，适当增大线宽，在布局布线的时候，尽可能考虑电流的流向。
▲ 图 4.1 最小系统部分
4.2电机驱动部分
驱动电路对电流的要求更严格，电流线要格外加粗，同时为了利于散热，我进行了开窗处理，同时必须采用隔离芯片防止烧坏单片机。并且增加电流信号采集芯片，为电流环和电机保护提供硬件基础。
▲ 图 4.1 最小系统部分
第五章 基于RT Thread的控制算法
5.1程序运行流程
在AI视觉组的任务相对其他更加繁杂，包括基础循迹、数字识别、二维码识别、激光打靶等等。同时为了方便调试运行、需要TFT显示标志位、按键响应、蜂鸣器提示和LED闪烁等功能。对于这样一个智能车任务，完全可以将它分解成多个简单、容易解决的小问题，小问题逐个被解决，大问题也就随之解决了。在多线程操作系统中，也同样需要开发人员把一个复杂的应用分解成多个小的、可调度的、序列化的程序单元，当合理地划分任务并正确地执行时，这种设计能够让系统满足实时系统的性能及时间的要求在本次智能车比赛中我们主要使用了RTthread中的线程、时钟、线程同步等功能。
用于检测电池电压变化的定时器bat_timer：
rt_timer_t bat_timer = rt_timer_create("bat_timer", adc_bat_thread, RT_NULL, ADC_BAT_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(bat_timer);
void adc_bat_thread(void *parameters)
{
uint16 value = adc_mean_filter(ADC_1, ADC1_CH3_B14, 5);
f1.bat_voltage = (float)value * 3.3 / 0xFFF * 5.7;
}
用于获取陀螺仪数据的定时器gyro_timer，并在定时器线程中进行姿态解算：
rt_timer_t gyro_timer = rt_timer_create("gyro_timer", gyro_thread, RT_NULL, GYRO_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(gyro_timer);
void gyro_thread(void *parameter)
{
get_icm20602_gyro_spi();
get_icm20602_accdata_spi();
inoutdev.update_gyro_y();
inoutdev.update_gyro_z();
}
void inoutdev_ctrl::update_gyro_z()
{
LPF_1_db(35, 1000, (float)(icm_gyro_z - gyro_z_offset), &gyro_z_filter);
yaw_rate = -0.03051757f * gyro_z_filter;
if (yaw_rate < 1.3 && yaw_rate > -1.3)
yaw_rate = 0;
// rt_kprintf("%f\n", inoutdev.y_rate);
delta_z = 10.3 * yaw_rate / 1000.0;
gyro_z_angle += delta_z;
start_angle += delta_z;
if (fabs(gyro_z_angle) >= 360)
gyro_z_angle = 0;
}
用于更新电流环的定时器current_timer：
current_timer = rt_timer_create("current_timer", current_thread, RT_NULL, CURRENT_INTERVAL, RT_TIMER_FLAG_PERIODIC);
void current_thread(void *parameters)
{
if (0 == f1.chassis_update_type)
{
chassis.left_wheel.update_current();
chassis.right_wheel.update_current();
}
else if (1 == f1.chassis_update_type)
{
}
}
用于更新速度环的定时器chassis_timer：
rt_timer_t timer = rt_timer_create("chassis_timer", chassis_thread, RT_NULL, CHASSIS_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(timer);
void chassis_thread(void *parameter)
{
if (0 == f1.chassis_update_type)
{
chassis.left_wheel.update();
chassis.right_wheel.update();
chassis.update_speed();
}
else if (1 == f1.chassis_update_type)
{
chassis.left_wheel.get_speed();
chassis.right_wheel.get_speed();
chassis.update_speed();
if (chassis.speed_now[0] > 2)
{
chassis.hard_bake_cnt = 0;
chassis.left_wheel.w_motor.run(-1000);
chassis.right_wheel.w_motor.run(-1000);
}
else
{
chassis.hard_bake_cnt++;
if (chassis.hard_bake_cnt >= 3)
{
chassis.set_speed(0, 0);
f1.chassis_update_type = 0;
}
}
}
chassis.update_dis();
}
用于更新TFT显示屏的定时器display_timer，并且其线程优先级最低，以确保其他控制小车运行的线程正常运行：
rt_thread_t tid_display = rt_thread_create("display_thread", display_thread, RT_NULL, 2048, 31, 5);
rt_thread_startup(tid_display);
void display_thread(void *param)
{
while (1)
{
ui_title_fresh(NULL);
rt_thread_mdelay(100);
}
}
在工程代码中使用了基于RTthread的agile_button的库，agile_button是基于RT-Thread实现的button软件包，提供button操作的API。特性：代码简洁易懂，充分使用RT-Thread提供的API；详细注释；线程安全；断言保护；API操作简单
key1 = agile_btn_create(KEY1_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key2 = agile_btn_create(KEY2_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key3 = agile_btn_create(KEY3_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key4 = agile_btn_create(KEY4_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key5 = agile_btn_create(KEY5_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key6 = agile_btn_create(KEY6_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
key7 = agile_btn_create(KEY7_PIN, PIN_LOW, PIN_MODE_INPUT_PULLUP);
agile_btn_set_event_cb(key1, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key2, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key3, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key4, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key5, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key6, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_set_event_cb(key7, BTN_CLICK_EVENT, btn_event_cb);
agile_btn_start(key1);
agile_btn_start(key2);
agile_btn_start(key3);
agile_btn_start(key4);
agile_btn_start(key5);
agile_btn_start(key6);
agile_btn_start(key7);
同时也使用了基于RTthread的easyblink的库。小巧轻便的LED控制软件包，可以容易地控制LED开、关、反转和各种间隔闪烁，占用RAM少，可以设置成线程安全型的；同时提供 RT-Thread 标准版和 Nano 版。
特点：和其它LED同类软件相比，easyblink 有一个显著的特点，占用 RAM 特别少，其它 LED 软件一般每一个LED都需要创建一个线程，LED一多，线程数就多了，所占用的栈空间就相应的增大。而 easyblink 始终只使用一个守护线程（线程栈可以是预先分配的静态栈空间），无论多少个 LED，就一个线程。另外，有不需要移植就可以直接使用的 Nano 版，特别适合 RAM 紧张的产品。同时，也可以设置成线程安全型的。
errno_led = easyblink_init_led(LED0_PIN, PIN_LOW);
info_beep = easyblink_init_led(BEEP_PIN, PIN_HIGH);
easyblink(errno_led, -1, 200, 1000);
void inoutdev_ctrl::beep(int num)
{
easyblink(info_beep, num, 30, 80);
}
void inoutdev_ctrl::lbeep(int num)
{
easyblink(info_beep, num, 200, 400);
}
5.2定义信号量
RT-Thread操作系统支持采用信号量、互斥量、邮箱、消息队列等等方式进行线程间的通信。由于我们在智能车的设计中只是用了信号量和互斥量，我们将仅介绍信号量和互斥量。
信号量是一种轻型的用于解决线程间同步问题的内核对象，线程可以获取或释放它， 从而达到同步或互斥的目的。信号量就像一把钥匙，把一段临界区给锁住，只允许 有钥匙的线程进行访问：线程拿到了钥匙，才允许它进入临界区；而离开后把钥匙 传递给排队在后面的等待线程，让后续线程依次进入临界区。
互斥量又叫相互排斥的信号量，是一种特殊的二值性信号量。它和信号量不同的是， 它支持互斥量所有权、递归访问以及防止优先级翻转的特性。互斥量的使用比较单一，因为它是信号量的一种，并且它是以锁的形式存在。 在初始化的时候，互斥量永远都处于开锁的状态，而被线程持有的时候则立刻转为 闭锁的状态。
chassis_mutex = rt_mutex_create("chassis_mutex", RT_IPC_FLAG_FIFO);
csi_done_sem = rt_sem_create("csi_done_sem", 0, RT_IPC_FLAG_FIFO);
ano_upload_sem = rt_sem_create("ano_upload_sem", 0, RT_IPC_FLAG_FIFO);
5.3初始化线程\定时器
对需要使用的硬件模块进行初始化，然后便开始为各个模块创建自己的线程，设置线程的运行周期，对于不能使用操作系统的裸机只能通过定时器中断来实现各个模块的周期运行，然而使用RT Thread后，可以更方便的管理各个模块的运行，也不用关心底层定时器怎么写，不必担心定时器数量是否够用，RT Thread编写的智能车代码更具有可读性，
current_timer = rt_timer_create(“current_timer”, current_thread, RT_NULL, CURRENT_INTERVAL, RT_TIMER_FLAG_PERIODIC);
电流环的定时器配置，设置定时周期为CURRENT_INTERVAL，优先级为RT_TIMER_FLAG_PERIODIC
rt_timer_start 设置完成定时器的参数之后定时器不会立即启动，只有调用rt_timer_start函数之后定时器才开始计时，这个定时我们设定在主函数里面 rt_thread_mdelay(3000); rt_timer_start(current_timer);
程序延时3s后开始计时，开启电流环
rt_thread_t tid_display = rt_thread_create(“display_thread”, display_thread, RT_NULL, 2048, 31, 5);
rt_thread_startup(tid_display);
在这里创建并开启了显示屏的线程，并且分配堆栈大小为2048，设置线程优先级为31，设置时间片为5
5.4线程间通信
线程通过获取信号量来获得信号量资源实例，当信号量值大于零时，线程将获得信号量，并且相应的信号量值都会减1。在调用这个函数时，如果信号量的值等于零，那么说明当前信号量资源实例不可用，申请该信号量的线程将根据time参数的情况选择直接返回、或挂起等待一段时间、或永久等待，直到其他线程或中断释放该信号量。如果在参数time指定的时间内依然得不到信号量，线程将超时返回，返回值是-RT_ETIMEOUT。这里我们获取csi图像采集的信号，判断图像采集是否完成，完成后再进行下一步工作，实现图像的处理，路径规划，元素识别，速度控制等等。
当通过设定摄像头帧率后，摄像头完成一帧图像获取后发出信号量，在main函数中接受道信号量后继续运行，并进行后续的图像处理：
rt_sem_take(csi_done_sem, RT_WAITING_FOREVER);
rt_sem_control(csi_done_sem, RT_IPC_CMD_RESET, RT_NULL);
inoutdev的类中clock函数用于统计实际帧率：
inoutdev.clock();
void inoutdev_ctrl::clock()
{
current_time = pit_get_ms(PIT_CH0);
fps = 1000 / (current_time - last_time);
last_time = current_time;
}
5.5 控制策略选择
PID控制器是一种线性控制器，它根据给定值与实际输出值构成控制偏差。将偏差的比例§、积分(I)和微分(D)通过线性组合构成控制量，对被控对象进行控制，故称PID控制器。传统PID控制器自出现以来，凭借其结构简单、稳定性好、工作可靠、调整方便等优点成为工业控制主要技术。当被控对象的结构和参数具有一定的不确定性，无法对其建立精确的模型时，采用PID控制技术尤为方便。PID控制原理简单、易于实现，但是其参数整定异常麻烦。对于小车的速度控制系统而言，由于其为时变非线性系统不同时刻需要选用不同的PID参数，采用传统的PID控制器，很难使整个运行过程具有较好的运行效果。
▲ 图5.1 PID控制器原理框图
5.5.1 位置式PID
位置式PID中，由于计算机输出的u (k) 直接去控制执行机构(如阀门)，u(k)的值和执行机构的位置(如阀门开度)是一一对应的，所以通常称公式2为位置式PID控制算法。
位置式PID控制算法的缺点是：由于全量输出，所以每次输出均与过去的状态有关，计算时要对过去e(k)进行累加，计算机工作量大；而且因为计算机输出的u(k)对应的是执行机构的实际位置，如计算机出现故障，u(k)的大幅度变化，会引起执行机构位置的大幅度变化，这种情况往往是生产实践中不允许的，在某些场合，还可能造成严重的生产事故。因而产生了增量式PID 控制的控制算法，所谓增量式PID 是指数字控制器的输出只是控制量的增量△u(k)。
#ifndef __POS_PID__
#define __POS_PID__
#include "headfile.h"
#include "rtthread.h"
using namespace rtthread;
class pos_pid
{
public:
float kp = 0;
float ki = 0;
float kd = 0;
float error = 0;
float error_last = 0;
float error_diff = 0;
float error_history[5];
float error_diff_history[5];
float p_error = 0;
float i_error = 0;
float d_error = 0;
float last_out = 0;
float output = 0;
float target = 0;
float intergral = 0;
float anti_wind_radio = 0.5f;
float anti_windup_value = 0;
float maximum = 0;
float minimum = 0;
pos_pid() {}
pos_pid(float kp, float ki, float kd, float maximum, float minimum) : kp(kp), ki(ki), kd(kd), maximum(maximum), minimum(minimum)
{
anti_windup_value = maximum * anti_wind_radio;
}
void set_pid(float kp_p, float ki_p, float kd_p)
{
kp = kp_p;
ki = ki_p;
kd = kd_p;
}
void update(float current_point)
{
error = target - current_point;
for (int i = 5; i >= 1; i--)
error_history[i] = error_history[i - 1];
error_history[0] = error;
intergral += ki * error;
__Limit_Both(intergral, anti_windup_value);
error_diff = (error - error_last);
for (int i = 5; i >= 1; i--)
error_diff_history[i] = error_diff_history[i - 1];
error_diff_history[0] = error_diff;
p_error = kp * error;
i_error = intergral;
d_error = kd * error_diff;
last_out = p_error + i_error + d_error;
__Limit_AB(last_out, minimum, maximum);
error_last = error;
output = last_out;
}
};
#endif
5.5.2 增量式PID
当执行机构需要的是控制量的增量(例如：驱动步进电机)时，可由式2推导出提供增量的PID控制算式。
增量式PID具有以下优点：
(1) 由于计算机输出增量，所以误动作时影响小，必要时可用逻辑判断的方法关掉。
(2) 手动/自动切换时冲击小，便于实现无扰动切换。此外，当计算机发生故障时，由于输出通道或执行装置具有信号的锁存作用，故能保持原值。
(3) 算式中不需要累加。控制增量△u(k)的确定仅与最近k次的采样值有关，所以较容易通过加权处理而获得比较好的控制效果。
但增量式PID也有其不足之处：积分截断效应大，有静态误差；溢出的影响大。使用时，常选择带死区、积分分离等改进PID控制算法。
#ifndef __INC_PID__
#define __INC_PID__
#include "common_macro.h"
#include "rtthread.h"
using namespace rtthread;
class inc_pid
{
public:
float kp = 0;
float ki = 0;
float kd = 0;
float error = 0;
float error_l = 0;
float error_ll = 0;
float p_error = 0;
float i_error = 0;
float d_error = 0;
float last_out = 0;
float output = 0;
float target = 0;
float maximum = 0;
float minimum = 0;
inc_pid() {}
inc_pid(float kp, float ki, float kd, float maximum, float minimum)
: kp(kp), ki(ki), kd(kd), maximum(maximum), minimum(minimum) {}
void set_pid(float kp_p, float ki_p, float kd_p)
{
kp = kp_p;
ki = ki_p;
kd = kd_p;
}
void update(float current_point)
{
error = target - current_point;
p_error = kp * (error - error_l);
i_error = ki * error;
d_error = kd * (error - 2 * error_l + error_ll);
last_out += p_error + i_error + d_error;
__Limit_AB(last_out, minimum, maximum);
error_ll = error_l;
error_l = error;
output = last_out;
}
};
#endif
5.5.3 模糊PID
模糊PID控制，即利用模糊逻辑并根据一定的模糊规则对PID的参数进行实时的优化，以克服传统PID参数无法实时调整PID参数的缺点。模糊PID控制包括模糊化，确定模糊规则，解模糊等组成部分。小车通过传感器采集赛道信息，确定当前距赛道中线的偏差E以及当前偏差和上次偏差的变化ec，根据给定的模糊规则进行模糊推理，最后对模糊参数进行解模糊，输出PID控制参数。
▲ 图 5.2 模糊自适应PID框图
智能车会采集到赛道的相关数据，经过算法处理之后会得到与中线的偏差E，以及当前偏差和上次偏差的变化（差值）EC两个值（即此算法为2维输入，同理也可以是1维和3维，但2维更适合智能车）。例如此时车偏离中线的距离为150，而上一时刻偏离中线的距离为120，则E为150，EC为150 - 120 = 30。
其次对这两个值进行模糊化。对E进行举例，E是有范围的，即与中线的偏差是在一个区间内可行的。在这里我们假设该区间为-240到240，即小车偏离中线的最大距离为240，正负即为左右。再假设中线偏差变化率的可行区间为-40到+40。接着我们要对这两个值进行模糊化。我现在将E的区间（-240 到 240）分成8个部分，那么他们分别为-240 ~ -180，-180 ~ -120 ，-120 ~ -60，-60 ~ 0，0 ~ 60，60 ~ 120，120 ~ 180，180 ~ 240。然后我们把-180，-120，-60，0，60，120，180分别用NB，NM，NS，ZO，PS，PM，PB表示（个人理解N为negative，P为positive，B为big，M为middle，S为small，ZO为zero）。例如，当E = 170时，此时的E属于PM和PB之间，而此时的E也会对应2（或1）个隶属度。E隶属于PM（120）的百分比为（180 - 170） / （180 - 120） = 1 / 6 ，而同理隶属于PB（180）的百分比为（170 - 120） / （180 - 120） = 5 / 6 。意思就是120到180进行线性分割了，E离PM和PB哪个更近，则隶属于哪个就更大（当输出值E大于180（PB）时，则隶属度为1，隶属度值为PB，即E完全隶属于PB，同理当E小于 - 180 （NB）时也一样）。同理也可以对EC进行模糊化。
▲ 图 5.3 模糊规则表
模糊推理：对于采集回来的E和EC，我们可以推出它们各所占的隶属度，此时我们可以根据模糊规则表去找出输出值所对应的隶属度。我们假设为E的两个隶属度值为PM、PB,E属于PM的隶属度为a（a < 1），则属于PB的隶属度为（1 - a）。再假设EC的两个隶属度值为NB、NM，EC属于NM的隶属度为b，则属于NB的隶属度为（1 - b）。而在假设中，E属于PM的隶属度为a，EC属于NB的隶属度为( 1 - b )，则输出值属于ZO的隶属度为a ( 1 - b )。同理我们可以得出，当输出值属于ZO的另外两个隶属度为a * b， （ 1 - a ） * （ 1 - b） ，而输出值属于NS的隶属度为（ 1 - a ） * 1 - b。在这里我们先证明一个条件，将这四个隶属度加起来，刚好等于1。这是因为（a + （1 - a）） * （b + （1 - b）） = a * b + ( 1 - a ) * b + a * ( 1 - b ) + ( 1 - a ) * ( 1 - b )
清晰化：对于输出值，我们同样采用给予隶属度的办法。例如，我们把输出值假设为[1000，1400]（即舵机的摆角值范围）的区间同样划分为八个部分，即7个隶属值NB,NM,NS,ZO,PS,PM,PB。根据上一步所得出的结论，我们就可以用隶属度乘以相应的隶属值算出输出值的解，即（a * b + a * ( 1 - b ) + ( 1 - a ) * ( 1 - b ) ） ZO + ( 1 - a ) * b * NS。到此为止，整个模糊过程就结束了。
第六章 开发工具和基于RTT终端的上位机
6.1开发环境与调试方法
Visual Studio Code 是一个轻量级但功能强大的源代码编辑器，可在您的桌面上运行，适用于 Windows、macOS 和 Linux。它内置了对 JavaScript、TypeScript 和 Node.js 的支持，并为其他语言（例如 C++、C#、Java、Python、PHP、Go）和运行时（例如 .NET 和 Unity）提供了丰富的扩展生态系统.
▲ 图6.1 Vscode代码编辑界面
在对程序进行开发和软硬件联调的过程中需要一整套的软件开发与调试工具。程序的开发是在 IAR Embedded Workbench 下进行的，包括源程序的编写、编译和链接，并最终生成可执行文件。包括集成开发环境 IDE、处理器专家库、全芯片显示。工具、项目工程管理器、C 交叉编译器、汇编器、链接器以及调试器。使用 CMSIS DAP来下载程序，把编译好的程序下载到单片机里运行。如图6.2。
▲ 图6.2 IAR界面
6.2基于C#和串口的上位机
通过在main.cpp里用MSH FINSH导出函数命令，实现上位机通过串口终端进行命令交互和数据交换。
/**
* @brief RTT 串口终端命令导出
* @param func_name:函数名
* @param argv:命令内要执行的语句
* @retval 无
*/
#define EX_UPPER_F(func_name, ...) \
int func_name(int argc, char **argv) \
{ \
__VA_ARGS__; \
return RT_EOK; \
} \
MSH_CMD_EXPORT(func_name, func_name);
EX_UPPER_F(set_art_pid_param_cmd,
art_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));)
EX_UPPER_F(set_cur_pid_param_cmd,
chassis.left_wheel.current_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));
chassis.right_wheel.current_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));)
EX_UPPER_F(set_cur_pid_target,
chassis.left_wheel.current_pid.target = atof(argv[1]);
chassis.right_wheel.current_pid.target = atof(argv[1]);)
EX_UPPER_F(set_steer_fuzzy_p,
steer.steer_fuzzy_kp_p = atoi(argv[1]);)
EX_UPPER_F(set_remote_stop,
f1.remote_stop = atoi(argv[1]);
rt_kprintf("chassis.allele_distance: %f\n", chassis.allele_distance);)
EX_UPPER_F(set_dym_p,
dymparam.dym_p1 = atof(argv[1]);
dymparam.dym_p2 = atof(argv[2]);)
▲ 图6.3 使用Visual Studio和C#开发上位机
▲ 图6.4 C#编写的上位机
第七章 结 论
全国大学生智能汽车竞赛涵盖了控制、模式识别、传感技术、电子、电气、计算机、机械等多个学科，培养了学生的知识融合和实践动手能力。
在此份技术报告中，我们主要介绍了准备比赛时的基本思路，包括机械，电路，以及最重要的控制算法的创新思想，最主要还是RT Thread系统在智能车上的应用。在算法方面，主要使用C++编程，基于RTT的算法编写，让我们可以忽略底层，直接基于操作系统编程，节约了很多学习单片机的时间。再者，该操作系统是基于线程调度的，而智能车运行时有不同的模块，这就可以单独创建不同的线程，而RT Thread又支持线程通信，编写程序更加方便，极大增大的程序的可读性，提高的编写的效率。
在我们编写代码的过程中，我们发现一些问题。在RTT运行过程中，由于代码编写的问题（比如线程内存溢出没有提示、除数为0溢出），系统会自动重新运行，且串口终端输出不全，导致在debug时相对裸机开发难以发现问题所在。
参考文献
[1] 邱祎,熊谱翔,朱天龙.嵌入式实时操作系统：RT-Thread设计与实现[M].机械工业出版社,2019.
[2] 童诗白，华成英．模拟电子技术基础[M]．北京. 高等教育出版社．2000
[3] 李玲，桂玮珍，刘莲．C 语言程序设计教程．北京．人民邮电出版社.2010
[4] 刘火良,杨森编著.RT-Thread内核实现与应用开发实战指南 基于STM32[M].机械工业出版社,2019.
[5] 王盼宝．智能车制作．北京．清华大学出版社．2017
■ 附录A：核心算法子程序源代码
● Main函数C++源程序：
#include "main.h"
#include "PS2.h"
rt_sem_t csi_done_sem = RT_NULL;
rt_sem_t ano_upload_sem = RT_NULL;
rt_timer_t disp_timer;
rt_timer_t current_timer;
steer_ctrl steer;
chassis_ctrl chassis;
inoutdev_ctrl inoutdev;
dymparam_ctrl dymparam;
imgproc_ctrl img_proc_c;
steer_fuzzy steer_f;
rt_mutex_t chassis_mutex;
cur_pid art_pid;
data_send ano;
void adc_bat_thread(void *parameters)
{
uint16 value = adc_mean_filter(ADC_1, ADC1_CH3_B14, 5);
f1.bat_voltage = (float)value * 3.3 / 0xFFF * 5.7;
f2.garage_dir = inoutdev.switch_is_on(1);
f1.en_dis_ele = inoutdev.switch_is_on(2);
// f1.dymspeed_enable = inoutdev.switch_is_on(4);
// f1.en_dis_bake = inoutdev.switch_is_on(5);
// f1.en_fork_regco = inoutdev.switch_is_on(6);
// f1.s_cross_enable = inoutdev.switch_is_on(8);
}
void current_thread(void *parameters)
{
if (0 == f1.chassis_update_type)
{
chassis.left_wheel.update_current();
chassis.right_wheel.update_current();
}
else if (1 == f1.chassis_update_type)
{
}
}
void chassis_thread(void *parameter)
{
if (0 == f1.chassis_update_type)
{
chassis.left_wheel.update();
chassis.right_wheel.update();
chassis.update_speed();
}
else if (1 == f1.chassis_update_type)
{
chassis.left_wheel.get_speed();
chassis.right_wheel.get_speed();
chassis.update_speed();
if (chassis.speed_now[0] > 2)
{
chassis.hard_bake_cnt = 0;
chassis.left_wheel.w_motor.run(-1000);
chassis.right_wheel.w_motor.run(-1000);
}
else
{
chassis.hard_bake_cnt++;
if (chassis.hard_bake_cnt >= 3)
{
chassis.set_speed(0, 0);
f1.chassis_update_type = 0;
}
}
}
chassis.update_dis();
}
void ps2_thread(void *parameter)
{
PS2_Init();
rt_thread_mdelay(20);
while (1)
{
f1.ps2_key = PS2_DataKey();
rt_thread_mdelay(20);
f1.remote_stop++;
}
}
void ps2_time(void *parameter)
{
f1.ps2_key = PS2_DataKey();
f1.remote_stop++;
}
void art_turn_L()
{
art_pid.output = ANI_FRIUT_SERVO_TURN;
inoutdev.art_servo.set((int)art_pid.output);
}
void art_turn_R()
{
art_pid.output = -ANI_FRIUT_SERVO_TURN;
inoutdev.art_servo.set((int)art_pid.output);
}
void chassis_bake()
{
chassis.set_speed(0, 0);
f1.chassis_update_type = 1;
chassis.left_wheel.w_motor.run(-1000);
chassis.right_wheel.w_motor.run(-1000);
}
void gyro_thread(void *parameter)
{
get_icm20602_gyro_spi();
get_icm20602_accdata_spi();
inoutdev.update_gyro_y();
inoutdev.update_gyro_z();
}
void qrcode_steer_out()
{
steer.update_fuzzy_pid();
if (dymparam.speed_real < 0)
inoutdev.car_servo.set(-(int)steer.steer_out);
else
inoutdev.car_servo.set((int)steer.steer_out);
}
void fork_steer_out()
{
// find_fork_up_row();
find_up_infec();
steer.update_pid(f2.fork_up_col - 80);
if (dymparam.speed_real < 0)
inoutdev.car_servo.set(-(int)steer.steer_out);
else
inoutdev.car_servo.set((int)steer.steer_out);
}
void start_regco(int type)
{
char data_to;
switch (type)
{
case art_qrcode:
data_to = '0';
f1.aptg_regco_num = -1;
break;
case art_fork_num:
data_to = '1';
f1.fork_regco_num = -1;
break;
case art_anifuit:
data_to = '2';
f1.anifruit_regco_num = -1;
break;
case art_rects:
data_to = '3';
f1.fruit_pos_num = -1000;
break;
default:
break;
}
uart_putchar(USART_3, data_to);
rt_kprintf("sended to art: %c\n", data_to);
memset(f1.rev_num_list, -1, sizeof(f1.rev_num_list));
f1.finish_regco_flag = 0;
}
void normal_run()
{
steer.update_fuzzy_pid();
inoutdev.car_servo.set((int)steer.steer_out);
if (f2.stop_car_flag || f2.garage_sm_state == 2)
chassis.set_speed(0, 0);
else if (f1.dymspeed_enable)
chassis.set_speed(dymparam.speed_set, (int)steer.steer_out);
else
chassis.set_speed(dymparam.const_speed, (int)steer.steer_out);
if (inoutdev.switch_is_on(2))
{
int err = inoutdev.get_remote_ch1();
inoutdev.car_servo.set(err);
int spe = inoutdev.get_remote_ch2();
spe = (float)spe / 1000 * 150;
chassis.set_speed(spe, err);
// chassis.left_wheel.w_motor.run(spe);
// chassis.right_wheel.w_motor.run(spe);
}
// if (chassis.allele_distance >= 800)
// chassis.set_speed(0, 0);
}
int main(void)
{
#if 1
MT9V03X_CFG_CSI[0][1] = f1.cam_auto_exp; //自动曝光
MT9V03X_CFG_CSI[1][1] = f1.cam_bright_num; //曝光度
MT9V03X_CFG_CSI[2][1] = f1.cam_fps; //帧率
art_pid = cur_pid(1.1, 0.6, 0, 1000, 1000);
chassis_mutex = rt_mutex_create("chassis_mutex", RT_IPC_FLAG_FIFO);
csi_done_sem = rt_sem_create("csi_done_sem", 0, RT_IPC_FLAG_FIFO);
ano_upload_sem = rt_sem_create("ano_upload_sem", 0, RT_IPC_FLAG_FIFO);
inoutdev.init();
steer.init();
ano.init(115200);
chassis.init();
init_openart();
rt_timer_t bat_timer = rt_timer_create("bat_timer", adc_bat_thread, RT_NULL, ADC_BAT_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(bat_timer);
rt_timer_t gyro_timer = rt_timer_create("gyro_timer", gyro_thread, RT_NULL, GYRO_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(gyro_timer);
current_timer = rt_timer_create("current_timer", current_thread, RT_NULL, CURRENT_INTERVAL, RT_TIMER_FLAG_PERIODIC);
chassis.stop();
rt_timer_t timer = rt_timer_create("chassis_timer", chassis_thread, RT_NULL, CHASSIS_INTERVAL, RT_TIMER_FLAG_PERIODIC);
rt_timer_start(timer);
rt_thread_t tid_display = rt_thread_create("display_thread", display_thread, RT_NULL, 2048, 31, 5);
rt_thread_startup(tid_display);
#if 1
while (1)
{
rt_sem_take(csi_done_sem, RT_WAITING_FOREVER);
rt_sem_control(csi_done_sem, RT_IPC_CMD_RESET, RT_NULL);
inoutdev.clock();
if (f1.no_ele_start_flag)
{
f2.en_start = 1;
f1.no_ele_start_flag = 0;
inoutdev.car_servo.set(0);
rt_timer_stop(current_timer);
chassis.stop();
rt_thread_mdelay(2000);
rt_timer_start(current_timer);
}
f1.start_time1 = pit_get_ms(PIT_CH0);
img_proc_c.img_resize((uint8 **)mt9v03x_csi_image, (uint8 **)gray_120_160_image, 240, 320, 2);
img_proc_c.gray_to_binary((uint8 **)gray_120_160_image, (uint8 **)bin_120_160_image, 120, 160, 1);
// img_proc_c.dilate3(bin_120_160_image, tmp_120_160_image);
// img_proc_c.erode3(tmp_120_160_image, bin_120_160_image);
// img_proc_c.copy_image((uint8 **)tmp_120_160_image, (uint8 **)bin_120_160_image, 120, 160);
find_track_gray();
find_elements();
dymparam.dym_forsight();
dymparam.dym_type_speed();
steer.solve_mid_error();
/////////////////////////////////////////////////////////////////////////////////////////////
//normal sch-------------------------------------------------------------------------
/////////////////////////////////////////////////////////////////////////////////////////////
if (f2.sch_sm_state == normal)
{
normal_run();
// if (f2.fork_sm_state != 0 &&
// f2.fork_num == 1 &&
// f1.current_turns == 0 &&
// f1.en_fork_regco)
// {
// start_regco(1);
// rt_kprintf("detect fork\n");
// inoutdev.art_servo.set(0);
// chassis_bake();
// f2.fork_proc_sm = fork_hard_brake;
// f2.sch_sm_state = fork_sch;
// chassis.fork_distance = 0;
// f1.fork_start_dis = chassis.row_to_distance(f2.fork_up_row);
// rt_kprintf("fork diatance: %f\n", f1.fork_start_dis);
// }
// if (f2.tag_sm_state != 0 &&
// f1.en_anifriut_regco)
// {
// start_regco(0);
// rt_kprintf("detect tag\n");
// inoutdev.art_servo.set(0);
// chassis_bake();
// chassis.tag_distance = 0;
// f1.friut_start_time = pit_get_ms(PIT_CH0);
// f1.tag_start_dis = chassis.row_to_distance(f2.tag_row_down);
// f1.qrcode_regco_time = rt_tick_get();
// rt_kprintf("tag diatance: %f\n", f1.tag_start_dis);
// f2.sch_sm_state = qrcode_sch;
// f2.tag_proc_sm = qrcode_move_slow;
// }
}
/////////////////////////////////////////////////////////////////////////////////////////////
//fork sch---------------------------------------------------------------------------------
/////////////////////////////////////////////////////////////////////////////////////////////
if (f2.sch_sm_state == fork_sch)
{
//满占空比刹车直到速度为0
if (f2.fork_proc_sm == fork_hard_brake)
{
fork_steer_out();
inoutdev.set_art_servo(f2.fork_up_col - 80);
//已经识别到
if (f1.finish_regco_flag == 1 && f1.fork_regco_num != -1)
{
f2.fork_proc_sm = fork_wait_static;
f1.fork_regco_ed_time = rt_tick_get();
}
//未识别到
if (f1.finish_regco_flag != 1 && f1.fork_regco_num == -1 &&
dymparam.speed_real < 3)
{
f2.tag_proc_sm = fork_wait_static;
f1.fork_regco_ed_time = rt_tick_get();
}
}
//静止状态识别数字
if (f2.fork_proc_sm == fork_wait_static)
{
fork_steer_out();
chassis.set_speed(0, 0);
inoutdev.set_art_servo(f2.fork_up_col - 80);
//分析数字
if (f1.finish_regco_flag == 1 && f1.fork_regco_num != -1)
{
f2.fork_proc_sm = fork_analyze_num;
}
//三岔路识别超时
if (rt_tick_get() - f1.fork_regco_ed_time > 3000 * 10)
{
f2.fork_proc_sm = fork_backward_const_dis;
}
}
//不控制车子运动，分析数字
if (f2.fork_proc_sm == fork_analyze_num)
{
if (f1.finish_regco_flag == 1 && f1.fork_regco_num != -1)
{
f1.finish_regco_flag = 0;
if (f1.fork_regco_num == 1)
{
f2.fork1_dir = 1;
rt_kprintf("odd num, fork turn right\n");
}
else
{
f2.fork1_dir = 0;
rt_kprintf("even num, fork turn left\n");
}
find_elements();
chassis.fork_distance = 0;
// f2.fork_proc_sm = fork_leaving_fork;
f2.fork_proc_sm = fork_backward_const_dis;
}
}
//倒车一个固定距离直到三岔路可以通过
if (f2.fork_proc_sm == fork_backward_const_dis)
{
inoutdev.art_servo.set(0);
chassis.set_speed(-200, 0);
fork_steer_out();
if (chassis.fork_distance < -23)
{
chassis.fork_distance = 0;
f2.fork_proc_sm = fork_leaving_fork;
}
}
//驶离三岔路前倒车，动态距离
if (f2.fork_proc_sm == fork_backward_dym_dis)
{
chassis.set_speed(-100, 0);
inoutdev.art_servo.set(0);
fork_steer_out();
if (f1.fork_start_dis - chassis.fork_distance > 100)
{
f2.fork_proc_sm = fork_leaving_fork;
}
}
//缓慢的倒车，识别数字
if (f2.fork_proc_sm == fork_backward_slow)
{
chassis.set_speed(-20, 0);
inoutdev.art_servo.set(0);
fork_steer_out();
if (f1.finish_regco_flag == 1 && f1.fork_regco_num != -1)
{
f2.fork_proc_sm = fork_analyze_num;
}
}
//缓慢的前进，识别数字
if (f2.fork_proc_sm == fork_forward_slow)
{
chassis.set_speed(20, 0);
inoutdev.art_servo.set(0);
fork_steer_out();
if (f1.finish_regco_flag == 1 && f1.fork_regco_num != -1)
{
f2.fork_proc_sm = fork_analyze_num;
}
}
//驶离三岔路
if (f2.fork_proc_sm == fork_leaving_fork)
{
normal_run();
if (f2.fork_sm_state == 0)
{
f2.fork_proc_sm = 0;
f2.sch_sm_state = normal;
rt_kprintf("leave fork\n");
}
}
}
/////////////////////////////////////////////////////////////////////////////////////////////
//animal friut sch-------------------------------------------------------------------------
/////////////////////////////////////////////////////////////////////////////////////////////
if (f2.sch_sm_state == qrcode_sch)
{
//动物前进到码处停下
if (f2.tag_proc_sm == qrcode_forward_animal_dis)
{
chassis.set_speed(60, 0);
inoutdev.art_servo.set(0);
qrcode_steer_out();
if (chassis.tag_distance >= f1.tag_start_dis)
{
chassis_bake();
f2.tag_proc_sm = qrcode_wait_3s;
}
}
//水果前进到码处停下
if (f2.tag_proc_sm == qrcode_forward_friut_dis)
{
qrcode_steer_out();
find_qrcode_pos();
// art_pid.output = -ANI_FRIUT_SERVO_TURN;
// inoutdev.art_servo.set((int)art_pid.output);
if (f1.finish_regco_flag == 1 && f1.fruit_pos_num != -1000 &&
f1.friut_pos_list[0] != -1000 &&
f1.friut_pos_list[1] != -1000 &&
f1.friut_pos_list[2] != -1000 &&
f1.friut_pos_list[3] != -1000)
{
chassis_bake();
f2.tag_proc_sm = qrcode_laser_shot;
f1.laser_cal_time = rt_tick_get();
f1.friut_regco_ed_time = rt_tick_get();
}
else
{
chassis.set_speed(20, 0);
}
}
//缓慢前进，识别动物水果
if (f2.tag_proc_sm == qrcode_move_slow)
{
qrcode_steer_out();
chassis.set_speed(ANI_FRIUT_SPEED, 0);
//缓慢前进放弃
if (chassis.tag_distance > 140 &&
f1.finish_regco_flag != 1 && f1.anifruit_regco_num == -1)
{
rt_kprintf("no qrcode\n");
f2.tag_proc_sm = qrcode_leave_qrcode;
}
//识别到动物水果
if (f1.finish_regco_flag == 1 && f1.anifruit_regco_num != -1)
{
if (f1.anifruit_regco_num == 0)
{
rt_kprintf("animal\n");
f2.tag_proc_sm = qrcode_wait_3s;
}
else
{
rt_kprintf("friut\n");
f2.tag_proc_sm = qrcode_laser_shot;
f1.laser_cal_time = rt_tick_get();
f1.friut_regco_ed_time = rt_tick_get();
start_regco(3);
}
}
}
//识别为动物，等待3秒
if (f2.tag_proc_sm == qrcode_wait_3s)
{
chassis.set_speed(0, 0);
inoutdev.art_servo.set(0);
qrcode_steer_out();
uint32 current_time = pit_get_ms(PIT_CH0);
if (current_time - f1.friut_start_time > 3500)
{
f2.tag_proc_sm = qrcode_leave_qrcode;
}
}
//识别为水果,激光打靶
if (f2.tag_proc_sm == qrcode_laser_shot)
{
chassis.set_speed(0, 0);
qrcode_steer_out();
//没有检测到矩形，超时5秒
if (f1.finish_regco_flag != 1 && f1.fruit_pos_num == -1000 &&
rt_tick_get() - f1.friut_regco_ed_time > 10000 * 10)
{
f2.tag_proc_sm = qrcode_leave_qrcode;
inoutdev.art_servo.set(0);
}
//检测到矩形，更新PID
if (f1.finish_regco_flag == 1 && f1.fruit_pos_num != -1000)
{
art_pid.update(f1.fruit_pos_num);
inoutdev.art_servo.set((int)art_pid.output);
//激光对准时间超过3秒，继续
if ((rt_tick_get() - f1.friut_regco_ed_time > 3500 * 10) ||
(fabs(dymparam.speed_real) <= 0 &&
abs(f1.friut_pos_list[0]) <= ANI_FRIUT_LASER_THRE &&
abs(f1.friut_pos_list[1]) <= ANI_FRIUT_LASER_THRE &&
abs(f1.friut_pos_list[2]) <= ANI_FRIUT_LASER_THRE &&
abs(f1.friut_pos_list[3]) <= ANI_FRIUT_LASER_THRE))
{
rt_kprintf("qrcode_laser_shot\n");
inoutdev.art_servo.set((int)(art_pid.output + 10));
inoutdev.laser_on();
rt_thread_mdelay(1500);
inoutdev.laser_off();
f2.tag_proc_sm = qrcode_leave_qrcode;
inoutdev.art_servo.set(0);
}
f1.fruit_pos_num = -1000;
f1.finish_regco_flag = 0;
}
}
//驶出二维码区域
if (f2.tag_proc_sm == qrcode_leave_qrcode)
{
normal_run();
inoutdev.art_servo.set(0);
if (chassis.tag_distance > f1.tag_start_dis + 8)
{
f2.tag_sm_state = 2;
f2.tag_proc_sm = 0;
f2.sch_sm_state = normal;
rt_kprintf("leave tag\n");
}
}
}
f1.end_time1 = pit_get_ms(PIT_CH0);
f1.use_time1 = f1.end_time1 - f1.start_time1;
}
#endif
#endif
}
EX_UPPER_F(set_chassis_pid_param_cmd,
chassis.L_wheel_kp = atof(argv[1]);
chassis.L_wheel_ki = atof(argv[2]);
chassis.L_wheel_kd = atof(argv[3]);
chassis.R_wheel_kp = atof(argv[4]);
chassis.R_wheel_ki = atof(argv[5]);
chassis.R_wheel_kd = atof(argv[6]);
chassis.left_wheel.w_controller.set_pid(chassis.L_wheel_kp, chassis.L_wheel_ki, chassis.L_wheel_kd);
chassis.right_wheel.w_controller.set_pid(chassis.R_wheel_kp, chassis.R_wheel_ki, chassis.R_wheel_kd);)
EX_UPPER_F(set_steer_pid_param_cmd,
steer.steer_basic_kp = atof(argv[1]);
steer.steer_kp_j = atof(argv[2]);
// steer.steer_kp_max = atof(argv[3]);
steer.steer_ki = atof(argv[4]);
steer.steer_kd = atof(argv[5]);)
EX_UPPER_F(set_dym_speed_param_cmd,
dymparam.s_max_speed = atoi(argv[1]);
dymparam.s_min_speed = atoi(argv[2]);
dymparam.fore_min = atoi(argv[4]);
dymparam.fore_max = atoi(argv[5]);
dymparam.fore_fix = atoi(argv[6]);)
EX_UPPER_F(set_chassis_speed_cmd,
dymparam.const_speed = atof(argv[1]);
chassis.set_speed(dymparam.const_speed, 0);)
EX_UPPER_F(set_art_pid_param_cmd,
art_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));)
EX_UPPER_F(set_cur_pid_param_cmd,
chassis.left_wheel.current_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));
chassis.right_wheel.current_pid.set_pid(atof(argv[1]), atof(argv[2]), atof(argv[3]));)
EX_UPPER_F(set_cur_pid_target,
chassis.left_wheel.current_pid.target = atof(argv[1]);
chassis.right_wheel.current_pid.target = atof(argv[1]);)
EX_UPPER_F(set_steer_fuzzy_p,
steer.steer_fuzzy_kp_p = atoi(argv[1]);)
EX_UPPER_F(set_remote_stop,
f1.remote_stop = atoi(argv[1]);
rt_kprintf("chassis.allele_distance: %f\n", chassis.allele_distance);)
EX_UPPER_F(set_dym_p,
dymparam.dym_p1 = atof(argv[1]);
dymparam.dym_p2 = atof(argv[2]);)
电流环C++源程序：
#ifndef __CUR_PID__
#define __CUR_PID__
#include "headfile.h"
#include "rtthread.h"
using namespace rtthread;
class cur_pid
{
public:
float k1 = 0;
float k2 = 0;
float k3 = 0;
float error = 0;
float error_l = 0;
float error_square = 0;
float target_limit = 1200;
float output_limit = 999;
float output_inc = 0;
float output = 0;
float target = 0;
float maximum = 0;
float minimum = 0;
cur_pid() {}
cur_pid(float k1, float k2, float k3,
float target_limit,
float output_limit) : k1(k1),
k2(k2),
k3(k3),
target_limit(target_limit),
output_limit(output_limit) {}
void set_pid(float k1_p, float k2_p, float k3_p)
{
k1 = k1_p;
k2 = k2_p;
k3 = k3_p;
}
void update(float current_point)
{
__Limit_Both(target, target_limit);
// __Limit_AB(target, minimum, maximum);
error_l = error;
error = target - current_point;
__Limit_Both(error, target_limit);
// __Limit_AB(error, minimum, maximum);
if (error > 0)
error_square = 0.01 * error * error;
else
error_square = -0.01 * error * error;
output_inc = k1 * error + k2 * (error - error_l) + k3 * error_square;
// if (fabs(output_inc) < 5)
// output_inc = 0;
output += output_inc;
__Limit_Both(output, output_limit);
}
};
#endif
● 电机控制C++源程序：
#ifndef __MOTOR_H__
#define __MOTOR_H__
#include "headfile.h"
#include "rtthread.h"
using namespace rtthread;
//left: 50; right:
#define MOTOR_PWM_FREQ 17000 //12500 //20000
class motor
{
public:
int index;
motor() {}
motor(int index) : index(index) {}
void init()
{
if (1 == index)
{
pwm_init(PWM1_MODULE3_CHA_D0, MOTOR_PWM_FREQ, 0);
pwm_init(PWM1_MODULE3_CHB_D1, MOTOR_PWM_FREQ, 0);
}
else
{
pwm_init(PWM2_MODULE3_CHA_D2, MOTOR_PWM_FREQ, 0);
pwm_init(PWM2_MODULE3_CHB_D3, MOTOR_PWM_FREQ, 0);
}
}
void run(int thousands)
{
__Limit_Both(thousands, 1000);
thousands *= 50;
if (1 == index)
{
if (thousands >= 0)
{
pwm_duty(PWM1_MODULE3_CHA_D0, 0);
pwm_duty(PWM1_MODULE3_CHB_D1, thousands);
}
else
{
thousands = -thousands;
pwm_duty(PWM1_MODULE3_CHA_D0, thousands);
pwm_duty(PWM1_MODULE3_CHB_D1, 0);
}
}
else
{
if (thousands >= 0)
{
pwm_duty(PWM2_MODULE3_CHA_D2, thousands);
pwm_duty(PWM2_MODULE3_CHB_D3, 0);
}
else
{
thousands = -thousands;
pwm_duty(PWM2_MODULE3_CHA_D2, 0);
pwm_duty(PWM2_MODULE3_CHB_D3, thousands);
}
}
}
};
#endif
● 相关图表链接:
魔乐社区（Modelers.cn) 是一个中立、公益的人工智能社区，提供人工智能工具、模型、数据的托管、展示与应用协同服务，为人工智能开发及爱好者搭建开放的学习交流平台。社区通过理事会方式运作，由全产业链共同建设、共同运营、共同享有，推动国产AI生态繁荣发展。
更多推荐
4
0- 0
已为社区贡献184条内容
所有评论(0)