玩转撸狗系列——麦昆巡线 DF创客社区

DFHJM_IpFmV 发表于 2020-6-9 20:39:39

玩转撸狗系列——麦昆巡线

大家好！我是李诩，我又来啦！今天，我决定玩个“绝的”——撸狗系列之麦昆巡线！成品出来后，我觉得非常好玩，所以，我想来给大家分享这个项目。如果大家喜欢，可以学习一下制作这个项目！

那么本项目用到了一个PID算法。那么什么是PID算法呢？

PID 算法，是控制理论中最常用、最经典的算法，在我们的生产和生活中应用的非常广泛。它并不是什么很神圣的东西，大家一定都见过 PID 的实际应用：比如四轴飞行器，再比如平衡小车，还有汽车的定速巡航、3D 打印机上的温度控制器、恒温热水器等。就是类似于这种：需要将某一个物理量“保持稳定”的场合（比如维持平衡，稳定温度、转速等），PID 都会派上大用场。
它的公式为：

或者我们在编程时更常用的是数字式的 PID 公式：

PID 控制系统是一种闭环控制系统，闭环控制是根据控制对象输出的反馈来进行校正的控制方式。它能够根据测量出的实际值与计划值之间的误差 Error，按一定的标准来进行纠正。

根据公式，可以看到 PID 分为 3 个部分：比例调节（P）、积分调节（I）、微分调节（D）。那么这三者之间是什么关系呢？每个部分的调节对巡线会产生什么样的影响呢？我们一个一个来看。

> 注意：如标题所说，本文是写给小学生看的 PID 调速算法，所以下文中描述的是已经简化的 PID 算法，默认没有去计算每个程序的循环时间，也就是没有去计算积分和微分时间。主要有几个原因：一、是因为程序功能比较单一，每个循环时间差不多，在调节 Kd、Ki 参数时，将时间参数 T 当成一个整体进行调试了；二、省略了时间参数后，程序编写会更加简单，容易看懂与理解。

先来看看我的“小麦坤”吧！

这么可爱的“昆昆”是不是看之就很想入手一个呢？那，我们开始制作吧！

### 步骤1
准备材料

材料清单
1x麦昆: micro:bit教育机器人 V4.0 锂电池豪华套餐赠地图
1xAI视觉传感器加硅胶套
1xHusky Lens外壳

### 步骤2
拼装（有视频教程）

https://v.youku.com/v_show/id_XNDcwMjg2OTU3Ng==.html?spm=a2hzp.8244740.0.0

接下来是文字教程

1、先安装锂电池至麦昆主体上面。

2、安装Husky Lens到支架上。

3、安装Husky底座。

4、把安装好的Husky Lens安装到底座上。

5、装好micro：bit板

6、完成

### 步骤3
切割外形

### 步骤4
编写程序

### 代码
```
/*!
* MindPlus
* maqueen
*
*/
#include <Maqueen_Motor.h>
#include <DFRobot_HuskyLens.h>

// 动态变量
volatile float mind_n_x1, mind_n_offset, mind_n_error, mind_n_Kp, mind_n_V_turn,
            mind_n_V_straight, mind_n_V_left, mind_n_V_right, mind_n_Ki, mind_n_Kd,
            mind_n_lastError, mind_n_integral, mind_n_derivative;
// 函数声明
void DF_MaiKunYunDongKongZhi_ZuoLunSuDu(float mind_n_V_left, float mind_n_V_right);
void DF_PXunXian();
void DF_PuTongXunXian1();
void DF_PIDCanShuChuShiHua();
void DF_PuTongXunXian2();
void DF_PDCanShuChuShiHua();
void DF_PuTongXunXian3();
void DF_PCanShuChuShiHua();
void DF_PIDXunXian();
void DF_PDXunXian();
// 创建对象
DFRobot_HuskyLens huskylens;
Maqueen_Motor motor;

// 主程序开始
void setup() {
   huskylens.beginI2CUntilSuccess();
   DF_PIDCanShuChuShiHua();
}
void loop() {
   mind_n_x1 = huskylens.readArrowParameter(1).xOrigin;
   DF_PIDXunXian();
}

// 自定义函数
void DF_MaiKunYunDongKongZhi_ZuoLunSuDu(float mind_n_V_left, float mind_n_V_right) {
   if ((mind_n_V_left>=0)) {
            motor.motorRun(motor.LEFT,motor.CW,mind_n_V_left);
   }
   else {
            motor.motorRun(motor.LEFT,motor.CCW,(abs(mind_n_V_left)));
   }
   if ((mind_n_V_right>=0)) {
            motor.motorRun(motor.RIGHT,motor.CW,mind_n_V_right);
   }
   else {
            motor.motorRun(motor.RIGHT,motor.CCW,(abs(mind_n_V_right)));
   }
}
void DF_PXunXian() {
   mind_n_error = (mind_n_x1 - mind_n_offset);
   mind_n_V_turn = (mind_n_Kp * mind_n_error);
   mind_n_V_left = (mind_n_V_straight + mind_n_V_turn);
   mind_n_V_right = (mind_n_V_straight - mind_n_V_turn);
   DF_MaiKunYunDongKongZhi_ZuoLunSuDu(mind_n_V_left, mind_n_V_right);
}
void DF_PuTongXunXian1() {
   if ((mind_n_x1==160)) {
            // 直行
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(60, 60);
   }
   if ((mind_n_x1<160)) {
            // 左转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(35, 85);
   }
   if ((mind_n_x1>160)) {
            // 右转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(85, 35);
   }
}
void DF_PIDCanShuChuShiHua() {
   mind_n_V_straight = 120;
   mind_n_offset = 160;
   mind_n_Kp = 0.55;
   mind_n_Ki = 0.00025;
   mind_n_Kd = 3.5;
   mind_n_error = 0;
   mind_n_lastError = 0;
   mind_n_integral = 0;
   mind_n_derivative = 0;
}
void DF_PuTongXunXian2() {
   if (((mind_n_x1>=150) && (mind_n_x1<=170))) {
            // 直行
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(70, 70);
   }
   if ((mind_n_x1<150)) {
            // 左转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(40, 100);
   }
   if ((mind_n_x1>170)) {
            // 右转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(100, 40);
   }
}
void DF_PDCanShuChuShiHua() {
   mind_n_V_straight = 120;
   mind_n_offset = 160;
   mind_n_Kp = 0.55;
   mind_n_Kd = 3.5;
   mind_n_error = 0;
   mind_n_lastError = 0;
   mind_n_derivative = 0;
}
void DF_PuTongXunXian3() {
   if (((mind_n_x1>=150) && (mind_n_x1<=170))) {
            // 直行
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(80, 80);
   }
   if (((mind_n_x1>=130) && (mind_n_x1<150))) {
            // 左小转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(70, 90);
   }
   if (((mind_n_x1>=110) && (mind_n_x1<130))) {
            // 左中转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(60, 100);
   }
   if ((mind_n_x1<110)) {
            // 左大转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(40, 120);
   }
   if (((mind_n_x1>170) && (mind_n_x1<=190))) {
            // 右小转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(90, 70);
   }
   if (((mind_n_x1>190) && (mind_n_x1<=210))) {
            // 右中转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(100, 60);
   }
   if ((mind_n_x1>210)) {
            // 右大转
            DF_MaiKunYunDongKongZhi_ZuoLunSuDu(120, 40);
   }
}
void DF_PCanShuChuShiHua() {
   mind_n_V_straight = 120;
   mind_n_offset = 160;
   mind_n_Kp = 0.55;
   mind_n_error = 0;
}
void DF_PIDXunXian() {
   mind_n_error = (mind_n_x1 - mind_n_offset);
   mind_n_integral = (mind_n_integral + mind_n_error);
   mind_n_derivative = (mind_n_error - mind_n_lastError);
   mind_n_V_turn = (((mind_n_Kp * mind_n_error) + (mind_n_Ki * mind_n_integral)) + (mind_n_Kd * mind_n_derivative));
   mind_n_V_left = (mind_n_V_straight + mind_n_V_turn);
   mind_n_V_right = (mind_n_V_straight - mind_n_V_turn);
   mind_n_lastError = mind_n_error;
   DF_MaiKunYunDongKongZhi_ZuoLunSuDu(mind_n_V_left, mind_n_V_right);
}
void DF_PDXunXian() {
   mind_n_error = (mind_n_x1 - mind_n_offset);
   mind_n_derivative = (mind_n_error - mind_n_lastError);
   mind_n_V_turn = ((mind_n_Kp * mind_n_error) + (mind_n_Kd * mind_n_derivative));
   mind_n_V_left = (mind_n_V_straight + mind_n_V_turn);
   mind_n_V_right = (mind_n_V_straight - mind_n_V_turn);
   mind_n_lastError = mind_n_error;
   DF_MaiKunYunDongKongZhi_ZuoLunSuDu(mind_n_V_left, mind_n_V_right);
}
```
### 步骤5
最后来看看效果吧！

https://v.youku.com/v_show/id_XNDcwNDMzNzUwNA==.html?spm=a2h0c.8166622.PhoneSokuUgc_24.dtitle

当然本文讲的是最基本的 PID 算法，这个算法在实际应用中还有很多可以进一步完善的地方。基于 PID 控制理论设计的新算法也很多，但是最基本的原理都是类似的。

最后用一张动图来总结一下本文讲的 PID 算法。从下图中，我们可以直观理解 PID 中三个参数 Kp、Ki、Kd 的作用。图中红色虚线为目标位置，蓝色实线为系统输出。

- 当调节 Kp 参数时，可以让系统输出越来越接近目标位置，但是存在抖动、以及存在稳定误差调节力度不够的问题；
- 调节 Ki 参数，可以弥补 Kp 参数调节力度不够的问题，但是也会增加抖动；
- 调节 Kd 参数，会让抖动减小，变化曲线更加平滑。

## 补充思考

在本文中，我们利用哈士奇摄像头得到了黑线位置，这里我们只用了黑线的其中一个位置参数：起点坐标 x1。但是另外三个参数都没有用上（起点坐标 y1，终点坐标 x2、y2）。

请你思考，是否将另外几个参数利用起来效果会更好呢？我们在巡线过程中，发现哈士奇屏幕中的黑线经常是斜着的，是否让黑线整体保持竖直，也就是让黑线与哈士奇屏幕的中轴线重合效果会更好呢？不仅去调节黑线与屏幕中间位置的偏差，还要去调节黑线的方向。

赶紧用本文讲的 PID 算法试试吧，你能挑战更快的速度么？

点击链接可以查看相关文章：
我对『PID算法』的理解 —— 原理介绍:https://mc.dfrobot.com.cn/thread-14783-1-1.html

用于乐高机器人的PID控制器：https://bbs.cmnxt.com/thread-5688-1-1.html

撸狗初体验 | 手把手教你上手 HuskyLens 哈士奇人工智能摄像头：https://mc.dfrobot.com.cn/thread-299462-1-1.html

我的项目分享结束谢谢大家的收看！
喜欢我的文章可以在下方留言、点赞、和转发，谢谢

teu 发表于 2020-6-9 20:49:20

重复的？

teu 发表于 2020-6-9 20:53:18

teu 发表于 2020-6-9 20:49
重复的？
https://mc.dfrobot.com.cn/thread-305824-1-1.html

DFHJM_IpFmV 发表于 2020-6-10 10:19:54

teu 发表于 2020-6-9 20:53
https://mc.dfrobot.com.cn/thread-305824-1-1.html

昨天修改了一下，加了个演示视频{:7_220:}

DFHJM_IpFmV 发表于 2020-6-10 10:27:23

昨天那个重复的被自动删除了

页: [1]

DF创客社区 - 分享创造的喜悦's Archiver

玩转撸狗系列——麦昆巡线