机器人割草机开源分享

2074368 2023-06-15 | zip | 0.00 MB | 次下载 | 2积分

资料介绍

描述

概述

让你的草坪在夏日的阳光下焕然一新对于房主来说通常是一件可怕的家务事。机器人割草机可以消除夏季最令人生畏的家务之一。它看起来类似于机器人吸尘器。你可以看着它在你的草坪上飞来飞去，让你的草坪保持最佳状态。为割草机配备支持物联网的功能，例如蓝牙或 Wi-Fi 连接、GPS 导航和灵活的调度功能，可以让您在任何地方使用移动应用程序控制割草机并跟踪割草进度。

特征

割草机由锂电池供电，非常安静，可以发送低电量警报。
测量割草路径。
调整切割高度。
自动路线规划。
使用移动应用程序进行远程控制。

脚步

第 1 步：硬件设计

割草机包括四个硬件部件。

无线连接系统：涂鸦的网络模块允许在任何地方在移动应用程序上远程控制和查看割草机状态。
动力系统：12V锂电池组配合减速电机推动橡胶轮胎车轮前进。
割草控制系统：两个伺服系统可调节切割高度，切割刀片和驱动轮采用无刷直流电机。
定位导航系统：集成GNSS模块和磁传感器，实现导航定位能力。

无线连接系统

将您的 MCU 与涂鸦的网络模块对接，让您的产品支持物联网并连接到云端。

涂鸦提供一站式物联网开发服务，包括典型物联网产品的三大要素，即网络模块、移动应用和云服务。借助涂鸦MCU SDK、一体机APP和控制面板，您可以轻松专注于应用开发，轻松将您的产品接入涂鸦IoT平台，快速享受云服务。

MCU SDK 是根据您在涂鸦物联网平台上定义的产品特性自动生成的。为了方便与协议的接口，MCU SDK 内置了对通信和协议解析的支持。您可以将此 SDK 添加到现有项目中并完成所需的配置以实现 MCU 程序开发。

硬件资源要求

MCU 的 SDK 要求如下。如果你的 MCU 没有足够的资源，你可以参考 SDK 中的函数来对接涂鸦的协议。

内存：4 KB
RAM：需要大约 100 字节的 RAM，具体取决于数据点 (DP) 的数据长度。如果启用 OTA 更新，它必须大于 260 字节。
嵌套函数：9 级。

选择网络模块

涂鸦针对物联网项目的各种需求，提供了一系列不同协议的模块，如Wi-Fi 模块、Wi-Fi 和蓝牙 LE 模块。

为了简化开发过程，您可以选择涂鸦三明治评估套件。

电源

通常，市场上的割草机由气体或电池供电。

燃气割草机只要有足够的燃料并且比电池供电的割草机更强大，就可以跑很远。然而，它比电池选项产生更多的噪音，并在整个气体燃烧过程中产生排放。

我们选择环保锂电池组为我们的割草机供电，以减轻噪音和污染影响。

电池

建议使用 3S 或 4S 12V 锂离子电池。锂离子电池的充放电倍率可以达到15C甚至超过20C，保证了强大的动力推动车轮前进。如果您不选择锂离子电池，请选择使用寿命长且最大输出电流大于 5A 的电池。

降压转换器

我们需要一个降压转换器来为不同的系统提供输出电压。

降压转换器LM2596提供 3.3V、5V、12V 的固定输出电压和可调输出版本。它具有以下特点。

输入电压范围高达 40 V。
可调版本输出电压范围：在线路和负载条件下最大为 1.2V 至 37V ±4%。
3A 输出负载电流。
高效率。
热关断和限流保护。

齿轮马达

减速电机可以定义为直流电机的延伸。它有一个连接到电机的齿轮组件，以增加扭矩并降低速度。

减速电机的额定电压必须与电池的额定电压相匹配。12V 电机的电源电压范围为 11V 至 16V。

我们选用MG513直流12V减速电机，额定输出电流0.36A。其减速比为1/60，空载转速为183 RPM。电机可提供 2 kgf.cm 的扭矩和 6 kg 的最大负载。使用 65 mm 橡胶轮，割草机的速度可以达到 0.5 m/s。

电机具有内置霍尔效应传感器。基于磁感应，我们可以得到电机的转速，然后测量所经过的速度和距离。

电机驱动器

东芝的TB6612FNG驱动器是一款基于 MOSFET 的双通道 H 桥集成电路。它具有以下特点。

电源电压：VM = 15V（最大值）。
输出电流：I = 1.2A（平均）/3.2A（峰值）。
输出低导通电阻：0.5Ω。
内置热关断电路和低电压检测电路。
PWM信号的开关频率：100 kHz。

TB6612FNG 不需要散热器，因为它比基于双极结型晶体管 (BJT) 的驱动器（如 L298N）效率更高。外围电路简单，加电源滤波电容即可驱动电机。

TB6612FNG有一对驱动器，可以独立控制两个电机。因此，四个电机只需要两个 TB6612FNG IC。

当STBY引脚为高电平时，两个输入信号IN1和IN2可以是顺时针（CW）、逆时针（CCW）、短制动和停止模式等四种模式之一。

示意图如下。

割草控制系统

割草控制系统可以调节割草高度并控制割草操作。

无刷直流电机

无刷电机没有电刷，因此在有刷电机运行时不会产生电火花，大大降低了电火花对射频遥控设备的干扰。无刷，运行时摩擦力大大降低，运行平稳，噪音低很多。这一优势是对稳定性和长使用寿命的巨大支持。

我们使用 900 Kv 的无刷电机。选择电机时，请考虑 Kv 额定值和扭矩两个因素。无刷电机的 Kv 额定值是电机的空载转速与连接到线圈的导线上的峰值电压之比。一般来说，电机的Kv越大，电机所能产生的扭矩就越小。

尺寸方面，推荐使用2212电机。

电子速度控制器 (ESC)

电子速度控制器 (ESC) 是一种控制和调节电动机速度的电子电路。

我们使用 20A 电调。确保 ESC 的输出电流与无刷电机的输出电流匹配。

它具有以下特点。

用于多旋翼控制器的特殊核心程序大大提高了油门响应。
特别优化的软件，与盘式电机具有出色的兼容性。
除定时外的所有设置都是预设的，使用简单，高度智能，自适应。
油门信号线采用双绞线设计，有效降低信号传输中产生的串扰，使飞行更加平稳。
专用驱动芯片的MOSFET驱动效果比由分立元件组成的普通驱动电路要好得多。
具有超低电阻的 MOSFET 带来高性能和大电流耐受性。

伺服

伺服系统包含直流电机、控制电路、减速器、反馈机构和其他电路。它可以根据输入信号转到特定位置。其内部的电位器或角度传感器可以感应输出轴的位置，从而使控制板据此准确控制输出轴的转动。

通常，舵机有三根线：棕色或黑色一根是地线，红色一根是电源线，橙色或白色一根是信号线。

舵机可以工作在 4V 到 6V 之间，推荐使用 5V 电压。伺服旋转的角度是通过调整PWM信号的占空比来实现的。标准 PWM 信号的周期固定为 20 ms (50 Hz)。脉冲宽度在 0.5 ms 到 2.5 ms 之间，对应伺服旋转角度 0° 到 180°。

两个舵机和一些金属圆盘组成一个升降结构来调整切割高度。

定位导航系统

定位导航系统可实现定位和定向。

全球导航卫星系统定位

全球导航卫星系统 (GNSS) 是一个卫星网络，用于广播时间和轨道信息，用于导航和定位测量。目前，美国的全球定位系统（GPS）、俄罗斯的全球导航卫星系统（GLONASS）、中国的北斗卫星导航系统（BDS）和欧盟的伽利略系统都是全面运行的全球导航卫星系统。

我们使用涂鸦的GUC300 GNSS 模块进行定位和导航。

GUC300由高度集成的GNSS芯片UFirebird-UC6226NIS和外围电路组成。它具有内置SAW滤波器、低噪声放大器（LNA）、26 MHz温度补偿晶体振荡器（TCXO）等元件。它同时支持 GPS 和 BDS 卫星导航系统。

GUC300 具有以下特点。

定位引擎：

64个同步跟踪通道

热启动时间：<1.2秒

冷启动灵敏度：-145 dBm。跟踪灵敏度：-158 dBm

数据更新率：高达 10 Hz

定位引擎：

RF子系统采用宽带设计。输入信号的中心频率约为 1575 MHz。

接收和跟踪 GPS 的 1575.42 MHz L1 信号

接收和跟踪北斗卫星导航系统1561.098 MHz B1信号

GUC300默认接收GPS和BDS信号，可定制为GPS+BDS、GPS+GLONASS或单GNSS系统。您可以提交服务单以请求所需的 GNSS 固件。

GUC300 默认每秒输出$GNRMC, $GNGGA, 和$GPGSV句子。串口波特率为9600。我们可以解析$GNGGA句子。

$GNGGA,071520.00,3018.12971,N,12003.84423,E,1,20,1.47,60.5,M,,M,,\*61

上述句子的字段描述如下。$GNGGA, <1>, <2>, <3>, <4>, <5>, <6>, <7>, <8>, <9>, M, <10>, M, <11> , <12>*xx

\$GNGGA：句子类型标识符。此示例指示 GGA 协议头。
<1>：UTC时间，格式为hhmmss.sss.
<2>：纬度，格式为ddmm.mmmm. 插入前导零。
<3>：纬度半球，N或S（北纬或南纬）。
<4>：经度，格式为dddmm.mmmm. 插入前导零。
<5>：半球经度，E或W（东经或西经）。
<6>：GPS定位状态。0: 初始化。1: 单点定位。2: 差别。3: 无效的 PPS。4: 固定解决方案。5: 浮点解。6: 估计。7：手动输入的固定值。8：模拟模式。9: WAAS different.xx: 从$to开始的所有 ASCII 字符的 XOR 校验值\*。
<7>：使用的卫星数，范围从00到12。插入前导零。
<8>：水平精度稀释度（HDOP），范围从0.5到99.9。HDOP越小，卫星分布越好。
<9>：海平面高度，范围从-9999.9到9999.9。
以米为单位。
<10>：地球椭球体相对于大地水准面的高度，范围从-9999.9到9999.9。
以米为单位。
<11>：距离上次接收到差分 GPS 信号的秒数。如果不采用差分定位，则该字段为空。
<12>：差分参考基站标签，范围从0000到1023。插入前导零。
：回车，结束标记。
：换行，结束标签。

电路原理图

GUC300原理图

GUC300 电路板

电子罗盘传感器

我们选择了 3 轴电子罗盘传感器QMC5883L 。

电子罗盘由一个 3D 磁阻传感器、一个 2 轴倾角仪和一个 MCU 组成。3D磁阻传感器用于监测地磁场的变化，倾角仪可以测量感应方向与地平面之间的角度（相对于重力）。MCU 处理输入信号并输出数据以补偿硬铁和软铁失真。磁力计包括 x、y 和 z 方向的三个磁阻传感器，以相应地测量地磁场强度。传感器在每个方向上的灵敏度已根据地磁场矢量进行了最佳校准。交叉轴灵敏度已降至最低。来自传感器的模拟输出信号将被放大，然后发送到 MCU 进行处理。

我们可以使用下面的公式来计算方向。

方向.x=atan2((double)Mag_data.y, (double)Mag_data.x)*57.3+180;

Mag_data.x分别是 X 轴和Mag_data.yY 轴的数据。atan2是平面的正 x 轴与其上坐标 (x, y) 给定的点之间的弧度角。将弧度值乘以 57.3 以将弧度转换为度数。最后，将转换后的值加上 180 度。

对于计算结果，真南为0或360，西为90，北为180，东为270。

组装设备

设计一个安装轮子、伺服系统和其他组件的底板。下载底板结构文件。

整个组件看起来像这样。

我们需要设计一个PCB布局来放置电机驱动器和电源等组件。确保为舵机、电机和电子罗盘的接口预留空间，以保持项目整洁。您还可以添加一些功能，例如用于警报的蜂鸣器和用于信号的 LED。

电路原理图

原理图，示意图

印刷电路板

最后，将 GPS、微控制器和电源单元安装在 PCB 上，PCB 组装完成。

第 2 步：创建产品

1.登录涂鸦IoT平台。

2.点击创建。

3.向下滚动页面并单击找不到类别？在左下角。

4.填写所需信息并点击创建。

Product Name 、Product Description和Product Model是用户定义的。
选择Wi-Fi-Bluetooth作为协议，然后单击Create 。

5.在自定义函数部分，单击添加以创建所需的函数。在函数定义步骤中，您可以根据需要添加标准函数或创建自定义函数。

我们设置了以下功能。

标准功能：方向控制
自定义功能：刀片位置、刀片转速、切割长度、切割宽度、锯齿割草模式。

6. 在设备面板选项卡的第二步中，选择所需的面板。在设备面板的第二步，可以选择调试友好的DIY风格面板。

7.在硬件开发的第三步，选择涂鸦标准模块SDK 。

8.向下滚动页面并找到下载文档。点击全部下载，下载MCU SDK开发所需的全部文件。

平台上的操作就完成了。您可以使用涂鸦智能 App 使用虚拟设备进行远程控制。

第 3 步：固件开发

在应用程序开发之前，您需要将 MCU SDK 移植到您的项目中。

然后将通用固件和涂鸦的授权刷入网络模块。这样，割草机就可以与云进行通信。

演示例程还包括 FreeRTOS，以方便您的开发。

以上准备工作完成后，就可以进行应用开发了。

下载示例代码：tuya-iotos-embeded-mcu-demo-wifi-ble-samrt-lawn-mower

电机、伺服和电调驱动器

将四个电机连接到每个车轮以进行运动。两个舵机用于控制 ESC 以及连接到 ESC 的刀片的位置。ESC 控制和调节刀片旋转的速度。

编写上述组件的初始化程序，在servo_motor.c.

为输出 GPIO 定义宏。

#define MOTOR_PORT_1                        GPIOA
#define MOTOR_PIN_1                         GPIO_PIN_15
#define MOTOR_PORT_1_P                      GPIOD
#define MOTOR_PIN_1_P                       GPIO_PIN_0
#define MOTOR_PORT_1_N                      GPIOD
#define MOTOR_PIN_1_N                       GPIO_PIN_7
#define MOTOR_PORT_2                        GPIOB
#define MOTOR_PIN_2                         GPIO_PIN_9
#define MOTOR_PORT_2_P                      GPIOD
#define MOTOR_PIN_2_P                       GPIO_PIN_1
#define MOTOR_PORT_2_N                      GPIOD
#define MOTOR_PIN_2_N                       GPIO_PIN_2
#define MOTOR_PORT_3                        GPIOB
#define MOTOR_PIN_3                         GPIO_PIN_10
#define MOTOR_PORT_3_P                      GPIOD
#define MOTOR_PIN_3_P                       GPIO_PIN_3
#define MOTOR_PORT_3_N                      GPIOD
#define MOTOR_PIN_3_N                       GPIO_PIN_4
#define MOTOR_PORT_4                        GPIOB
#define MOTOR_PIN_4                         GPIO_PIN_11
#define MOTOR_PORT_4_P                      GPIOD
#define MOTOR_PIN_4_P                       GPIO_PIN_8
#define MOTOR_PORT_4_N                      GPIOD
#define MOTOR_PIN_4_N                       GPIO_PIN_9
#define MOTOR_CHANNEL_1                     TIMER_CH_0
#define MOTOR_CHANNEL_2                     TIMER_CH_1
#define MOTOR_CHANNEL_3                     TIMER_CH_2
#define MOTOR_CHANNEL_4                     TIMER_CH_3
#define SERVO_PORT_1                        GPIOC
#define SERVO_PIN_1                         GPIO_PIN_6
#define SERVO_PORT_2                        GPIOC
#define SERVO_PIN_2                         GPIO_PIN_7
#define BLADE_MOTOR_PORT                    GPIOC
#define BLADE_MOTOR_PIN                     GPIO_PIN_8

调用初始化接口开启GPIO时钟，指定输出GPIO为普通模式或PWM模式。然后，调用timer_config设置 PWM 参数。

void servo_motor_init(void)
{
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_GPIOC);
rcu_periph_clock_enable(RCU_GPIOD);

/*Configure PD1~8 Output motor Positive and Negative pin to drive wheels_1~4*/
gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);

/*Configure PA15(TIMER1_CH0) Output PWM pulse to drive wheels_1*/
gpio_mode_set(MOTOR_PORT_1, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_1);
gpio_output_options_set(MOTOR_PORT_1, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_1);
gpio_af_set(MOTOR_PORT_1, GPIO_AF_1, MOTOR_PIN_1);
/*Configure PB9(TIMER1_CH1) Output PWM pulse to drive wheels_2*/
gpio_mode_set(MOTOR_PORT_2, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_2);
gpio_output_options_set(MOTOR_PORT_2, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_2);
gpio_af_set(MOTOR_PORT_2, GPIO_AF_1, MOTOR_PIN_2);
/*Configure PB10(TIMER1_CH2) Output PWM pulse to drive wheels_3*/
gpio_mode_set(MOTOR_PORT_3, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_3);
gpio_output_options_set(MOTOR_PORT_3, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_3);
gpio_af_set(MOTOR_PORT_3, GPIO_AF_1, MOTOR_PIN_3);
/*Configure PB11(TIMER1_CH3) Output PWM pulse to drive wheels_4*/
gpio_mode_set(MOTOR_PORT_4, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_4);
gpio_output_options_set(MOTOR_PORT_4, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_4);
gpio_af_set(MOTOR_PORT_4, GPIO_AF_1, MOTOR_PIN_4);
/*Configure PC6(TIMER2_CH0) Output PWM pulse to drive servo no.1*/
gpio_mode_set(SERVO_PORT_1, GPIO_MODE_AF, GPIO_PUPD_NONE, SERVO_PIN_1);
gpio_output_options_set(SERVO_PORT_1, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,SERVO_PIN_1);
gpio_af_set(SERVO_PORT_1, GPIO_AF_2, SERVO_PIN_1);

/*Configure PC7(TIMER2_CH1) Output PWM pulse to drive servo no.2*/
gpio_mode_set(SERVO_PORT_2, GPIO_MODE_AF, GPIO_PUPD_NONE, SERVO_PIN_2);
gpio_output_options_set(SERVO_PORT_2, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,SERVO_PIN_2);
gpio_af_set(SERVO_PORT_2, GPIO_AF_2, SERVO_PIN_2);

/*Configure PC8(TIMER2_CH2) Output PWM pulse to drive blade motor*/
gpio_mode_set(BLADE_MOTOR_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, BLADE_MOTOR_PIN);
gpio_output_options_set(BLADE_MOTOR_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,BLADE_MOTOR_PIN);
gpio_af_set(BLADE_MOTOR_PORT, GPIO_AF_2, BLADE_MOTOR_PIN);

timer_config();
}
void timer_config(void)
{
uint16_t i = 0;

/* TIMER1 configuration: generate PWM signals with different duty cycles:
TIMER1CLK = SystemCoreClock / 120 = 1MHz */
timer_oc_parameter_struct timer_ocintpara;
timer_parameter_struct timer_initpara;
rcu_periph_clock_enable(RCU_TIMER1);
rcu_periph_clock_enable(RCU_TIMER2);
rcu_timer_clock_prescaler_config(RCU_TIMER_PSC_MUL4);
timer_struct_para_init(&timer_initpara);
timer_deinit(TIMER1);
timer_deinit(TIMER2);
/* TIMER1 configuration */
timer_initpara.prescaler         = 119;
timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
timer_initpara.counterdirection  = TIMER_COUNTER_UP;
timer_initpara.period            = 500; /* 500*(1/1MHZ) = 500us */
timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
timer_initpara.repetitioncounter = 0;
timer_init(TIMER1,&timer_initpara);
/* TIMER2 configuration */
timer_initpara.prescaler         = 119;
timer_initpara.alignedmode       = TIMER_COUNTER_EDGE;
timer_initpara.counterdirection  = TIMER_COUNTER_UP;
timer_initpara.period            = 20000; /* 20000*(1/1MHZ) = 20ms */
timer_initpara.clockdivision     = TIMER_CKDIV_DIV1;
timer_initpara.repetitioncounter = 0;
timer_init(TIMER2,&timer_initpara);
timer_channel_output_struct_para_init(&timer_ocintpara);
timer_ocintpara.ocpolarity  = TIMER_OC_POLARITY_HIGH;
timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
timer_ocintpara.ocnpolarity  = TIMER_OCN_POLARITY_HIGH;
timer_ocintpara.outputnstate = TIMER_CCXN_DISABLE;
timer_ocintpara.ocidlestate  = TIMER_OC_IDLE_STATE_LOW;
timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
for(i = 0; i < 4; i++) {

timer_channel_output_config(TIMER1,i,&timer_ocintpara);
timer_channel_output_pulse_value_config(TIMER1,i,0);
timer_channel_output_mode_config(TIMER1,i,TIMER_OC_MODE_PWM0);
timer_channel_output_shadow_config(TIMER1,i,TIMER_OC_SHADOW_DISABLE);
}
for(i = 0; i < 3; i++) {

timer_channel_output_config(TIMER2,i,&timer_ocintpara);
timer_channel_output_pulse_value_config(TIMER2,i,0);
timer_channel_output_mode_config(TIMER2,i,TIMER_OC_MODE_PWM0);
timer_channel_output_shadow_config(TIMER2,i,TIMER_OC_SHADOW_DISABLE);
}
/* auto-reload preload enable */
timer_auto_reload_shadow_enable(TIMER1);
timer_auto_reload_shadow_enable(TIMER2);
/* TIMER enable */
timer_enable(TIMER1);
timer_enable(TIMER2);
}

调整电机和舵机的PWM占空比，将占空比调整的操作封装为通用接口。以连接到车轮的电机为例。car_moving用于根据输入方向和速度调整正负电平和占空比，从而控制车轮。

void car_moving(MOVING_DIRECTION direction, uint16_t speed_value)
{
uint8_t i;
switch(direction) {

case forward:

gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);

for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
case right:

gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);

//Since the two sets of wheels on the right are always faster than the left in the actual commissioning process, 60 is added to compensate
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_1,speed_value + 60);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_3,speed_value + 60);

timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_2,speed_value);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_4,speed_value);
break;
case left:

gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_1,speed_value);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_3,speed_value);

timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_2,speed_value + 50);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_4,speed_value + 50);
break;
case backward:

gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_set(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);

for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
case stop:

gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);

for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
default:
break;
}
}

用于线性运动校正的比例积分微分 (PID) 算法

由于四个轮子由独立的电机控制，即使 PWM 输出设置为相同的占空比，错误和其他因素也会导致割草机偏离线性运动。因此，我们使用PID算法来动态调整每个车轮的实际转速。直线运动表示四个轮子同时移动相同的距离。由于四个轮子的直径相同，因此每个轮子的转速应该相同。因为电机转速直接决定了电机编码器输出的脉冲数，所以我们以电机编码器单位时间内实时输出的脉冲数作为PID算法的输入，占空比增量作为控制对象。我们可以不断微调脉冲，使四个电机达到相同的值。

通过外部中断对脉冲进行计数，收集电机编码器输出脉冲的单位时间增量。中的movement_system_init()函数movement.c包括外部中断的配置。

void movement_system_init(void)
{
rcu_periph_clock_enable(RCU_SYSCFG);

gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_0);
nvic_irq_enable(EXTI0_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN0);
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_0);

gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
nvic_irq_enable(EXTI1_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN1);
exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_1);
gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_2);
nvic_irq_enable(EXTI2_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN2);
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_2);

gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_3);
nvic_irq_enable(EXTI3_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN3);
exti_init(EXTI_3, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_3);
}

void EXTI0_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_0)){
move_state.encoder_pulse[0]++;
}
exti_interrupt_flag_clear(EXTI_0);
}
void EXTI1_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_1)){
move_state.encoder_pulse[1]++;
}
exti_interrupt_flag_clear(EXTI_1);
}
void EXTI2_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_2)){
move_state.encoder_pulse[2]++;
}
exti_interrupt_flag_clear(EXTI_2);
}
void EXTI3_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_3)){
move_state.encoder_pulse[3]++;
}
exti_interrupt_flag_clear(EXTI_3);
}

实现forward_correction()接口，将收集到的脉冲pulse_num、最后一个脉冲pulse_num_old和所需的脉冲增量standard_increment传递给 PID 控制函数。调用single_motor_speed_set()根据计算结果调整各电机的PWM占空比。

int e[4]={0},e1[4]={0},e2[4]={0}; // PID offset
float uk[4]={0.0},uk1[4]={0.0},duk[4]={0.0}; // PID output
float Kp=0.8,Ki=0.3,Kd=0.9; // PID control factors.
int out[4] = {0};
static void forward_correction(uint32_t *pulse_num, uint32_t *pulse_num_old, uint32_t standard_increment)
{
uint8_t i = 0;

for(i = 0;i < 4;i++) {
e[i] = standard_increment - (pulse_num[i] - pulse_num_old[i]);
duk[i] = (Kp*(e[i]-e1[i])+Ki*e[i])/100;
uk[i] = uk1[i] + duk[i];
out[i] = (int)uk[i];
uk1[i] = uk[i];
e2[i] = e1[i];
e1[i] = e[i];
single_motor_speed_set(i, (uint16_t)(200+(out[i]*5)));
}
return;
}

当运动轮询处理程序进入switch直线运动的分支时，您可以调用forward_correction()来调整电机速度，然后将分配encoder_pulse给encoder_pulse_old。

void movement_logic_handle(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;
uint8_t i = 0;
MOVING_DIRECTION turning_state;

switch(p->todo_type) {

......
case on_the_move:

if(forward_sampling_time_flag == 20) { //20*20ms = 400ms
for(i = 0;i < 4;i++) {
pulse_test[i] = p->encoder_pulse[i]-p->encoder_pulse_old[i];
}
forward_correction(p->encoder_pulse,p->encoder_pulse_old,390);

for(i = 0;i < 4;i++) {
p->encoder_pulse_old[i] = p->encoder_pulse[i];
}
forward_sampling_time_flag = 0;
}
forward_sampling_time_flag++;

todo_judge();

break;
......
default:
break;
}

}

让割草机旋转 90 度

以之字形割草模式为例。当割草机到达直线运动的终点时，它必须转动 90 度。该todo_judge功能用于确定旋转运动，该运动也基于来自电机编码器的脉冲数。将长度转换为脉冲数。当脉冲达到规定值时，割草机将转动 90 度。割草机需要确定从长边或宽边转向以及向左或向右转向。todo_judge()函数有判断switch。分支指示割草机是case从长边转向还是从宽边转向。case用于判断直线运动中的割草机是否应转弯。如果是，则转弯状态将更改（change_to_do(turning);），并且案例分支p->run_step_flag = width_right;将在割草机在下一个行驶阶段进入此功能时更改。如果宽度达到规定值，则表明曲折割草作业完成。

static void todo_judge(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;

switch(p->run_step_flag) {

case length_right:
if(pulse_to_distance(p->encoder_pulse[0]) >= (p->range_length_mm - 10)) {
if((p->current_angle + 900) > 3600) {
p->target_angle = p->current_angle + 900 - 3600;
}else{
p->target_angle = p->current_angle + 900;
}
change_to_do(turning);
p->run_step_flag = width_right;
}
break;
case width_right:
if(pulse_to_distance(p->encoder_pulse[0]) >= 100) {
if((p->current_angle + 900) > 3600) {
p->target_angle = p->current_angle + 900 - 3600;
}else{
p->target_angle = p->current_angle + 900;
}
change_to_do(turning);
p->run_step_flag = length_left;
width_remain_mm -= 100;
}
break;
case length_left:
if(pulse_to_distance(p->encoder_pulse[0]) >= (p->range_length_mm - 10)) {
if(p->current_angle < 900) {
p->target_angle = 3600 - (900 - p->current_angle);
}else{
p->target_angle = p->current_angle - 900;
}
change_to_do(turning);
p->run_step_flag = width_left;
}
break;
case width_left:
if(pulse_to_distance(p->encoder_pulse[0]) >= 100) {
if(p->current_angle < 900) {
p->target_angle = 3600 - (900 - p->current_angle);
}else{
p->target_angle = p->current_angle - 900;
}
change_to_do(turning);
p->run_step_flag = length_right;
width_remain_mm -= 100;

}
break;
default:
break;
}

if(width_remain_mm <= 0) {
change_to_do(to_stop);
move_state.bow_mode_switch = pdFALSE;
}
}

上述例程中，p->target_angle是目标方位角，p->current_angle是当前方位角，用于控制割草机的转向角。目标方位角是割草机即将转弯时的方位角加减90度得到的。并且当前方位角是根据地磁传感器的数据计算得出的。该QMC5883L.c文件包含 I2C QMC5883L 的驱动程序代码。在这里，我们使用QMC5883L_GetAngle()来获取当前方位角。

void move_control_task(void *pvParameters)
{
float_xyz Mag_angle;
uint8_t test_flag = 50;
vTaskDelay(200);
QMC_Init();
QMC5883L_GetAngle(&Mag_angle);
move_state.current_angle = (int16_t)Mag_angle.x;
vTaskDelay(500);
while(1) {

if(test_flag){
vTaskDelay(20);
test_flag--;
}else if(test_flag == 0) {
vTaskDelay(20);
movement_logic_handle();
}
QMC5883L_GetAngle(&Mag_angle);
move_state.current_angle = (int16_t)Mag_angle.x;
}
}

方位角可用于确定转弯操作。该angle_correction()界面可以确定割草机应该左转还是右转或直行。该angle_correction()接口在移动轮询处理程序turning的case分支中调用。

static MOVING_DIRECTION angle_correction(void)
{
int16_t target,current = 0;
target = move_state.target_angle;
current = move_state.current_angle;
if(target < current) {
if(current - target <= 20) {
return forward;
}
if(current - target <= 1800) {
return left;
}else{
return right;
}
}else if(target > current) {
if(target - current <= 20) {
return forward;
}
if(target - current <= 1800) {
return right;
}else{
return left;
}
}else if(current == target) {
return forward;
}
}
void movement_logic_handle(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;
uint8_t i = 0;
MOVING_DIRECTION turning_state;

switch(p->todo_type) {

......
case on_the_move:

if(forward_sampling_time_flag == 20) { //20*20ms = 400ms
for(i = 0;i < 4;i++) {
pulse_test[i] = p->encoder_pulse[i]-p->encoder_pulse_old[i];
}
forward_correction(p->encoder_pulse,p->encoder_pulse_old,390);

for(i = 0;i < 4;i++) {
p->encoder_pulse_old[i] = p->encoder_pulse[i];
}
forward_sampling_time_flag = 0;
}
forward_sampling_time_flag++;

todo_judge();

break;
case turning:

turning_state = angle_correction();
if(turning_state == right) {
car_full_turn(right,150);
turning_sampling_time_flag = 0;
}else if(turning_state == left) {
car_full_turn(left,150);
turning_sampling_time_flag = 0;
}else if(turning_state == forward) {
car_moving(stop,0);
turning_sampling_time_flag++;
}

if(turning_sampling_time_flag >= 25) {
p->todo_type = on_the_move;
car_moving(forward,200);
forward_sampling_time_flag = 0;
turning_sampling_time_flag = 0;
for(i = 0;i < 4;i++) {
p->encoder_pulse[i] = 0;
p->encoder_pulse_old[i] = 0;
}
}
break;
......
default:
break;
}

}

设置刀片位置和速度

刀片位置由两个伺服系统控制。如果要在刀片上下移动时保持刀片的水平位置不变，则两个舵机必须在相反的方向上改变相同的角度。两个舵机的相反方向的角度变化必须相同。封装一个用于设置刀片位置的接口，以位置枚举值作为输入。

void blade_position_set(BLADE_POSITION value)
{
switch(value) {

case low:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,3100);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,3000);
break;

case medium:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,2800);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,2700);
break;

case high:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,2600);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,2500);
break;

default:
break;
}
}

叶片速度由 ESC 控制并封装在blade_speed_set();.

void blade_speed_set(BLADE_SPEED speed)
{
switch(speed) {

case init:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1500);
break;

case off:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,0);
break;

case low_speed:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1800);
break;

case medium_speed:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1900);
break;

case high_speed:

timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,2000);
break;

default:
break;
}
}

5.接收GNSS数据

涂鸦 GNSS 模块 GUC300 模块通过串口通信向 MCU 发送 GNSS 数据。我们使用USART1MCU的来接收数据，因为USART0已经用于与Wi-Fi模块的串行通信。

初始化串口：

void uart_init(void)
{
/* USART interrupt configuration */
nvic_irq_enable(USART0_IRQn, 0, 0);
nvic_irq_enable(USART1_IRQn, 1, 1);
/* enable USART clock */
rcu_periph_clock_enable(RCU_USART0);

/* connect port to USART0_Tx */
gpio_af_set(GPIOA, GPIO_AF_7, GPIO_PIN_9);
/* connect port to USART0_Rx */
gpio_af_set(GPIOA, GPIO_AF_7, GPIO_PIN_10);

/* configure USART Tx as alternate function push-pull */
gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_9);
gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_9);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_10);
gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_10);
/* USART configure */
usart_deinit(USART0);
usart_baudrate_set(USART0,115200U);
usart_receive_config(USART0, USART_RECEIVE_ENABLE);
usart_transmit_config(USART0, USART_TRANSMIT_ENABLE);
usart_enable(USART0);

/* enable USART clock */
rcu_periph_clock_enable(RCU_USART1);

/* connect port to USART0_Tx */
gpio_af_set(GPIOD, GPIO_AF_7, GPIO_PIN_5);
/* connect port to USART0_Rx */
gpio_af_set(GPIOD, GPIO_AF_7, GPIO_PIN_6);

/* configure USART Tx as alternate function push-pull */
gpio_mode_set(GPIOD, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_5);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_5);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(GPIOD, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_6);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_6);
/* USART configure */
usart_deinit(USART1);
usart_baudrate_set(USART1,9600U);
usart_receive_config(USART1, USART_RECEIVE_ENABLE);
usart_transmit_config(USART1, USART_TRANSMIT_ENABLE);
usart_enable(USART1);

/* enable USART0 receive interrupt */
usart_interrupt_enable(USART0, USART_INT_RBNE);

/* enable USART1 receive interrupt */
usart_interrupt_enable(USART1, USART_INT_RBNE);
}

初始化 USART 中断处理函数。GUC300 默认每秒输出$GNRMC, $GNGGA, $GPGSV, 和$GPGGA句子。我们可以解析$GPGGA句子。
$GPGGA句子包含17个字段：标题，位置的UTC时间状态，纬度，纬度方向，经度，经度方向，定位质量，正在使用的卫星数量，精度水平稀释，天线高度高于/低于平均海平面，天线高度单位、起伏（大地水准面和WGS84椭球的关系）、起伏的单位、校正数据的年龄、差分基站ID、校验和、句子终止符。
例子：$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*79
按照上述格式处理串行数据。

#define USART_FH_len 6  // The length of the header.
char USART_FH[USART_FH_len]={'$','G','N','G','G','A'}; // Header
#define USART_FT_len 2  // The length of the trailer.
uint8_t USART_FT[USART_FT_len]={0X0D,0X0A}; // Frame trailer
uint8_t data_buf[128] = {0};
uint8_t rx_buf[128] = {0};
uint8_t buff_len = 0;
void USART1_IRQHandler(void)
{
if((RESET != usart_interrupt_flag_get(USART1, USART_INT_FLAG_RBNE)) &&
(RESET != usart_flag_get(USART1, USART_FLAG_RBNE))){

rx_buf[buff_len++] = (uint8_t)usart_data_receive(USART1);
if(rx_buf[0] != USART_FH[0]) {
rx_buf[0] = 0;
buff_len = 0;
}
if(buff_len >= USART_FH_len) {
if(strncmp((char*)rx_buf,USART_FH,USART_FH_len) == 0) {
if(strncmp(&rx_buf[buff_len-2],(char*)USART_FT,USART_FT_len) == 0) {
memcpy(data_buf,rx_buf,buff_len);
memset(rx_buf,0,buff_len);
buff_len = 0;
}
}else {
memset(rx_buf,0,USART_FH_len);
buff_len = 0;
}
}
}
}
void gps_data_task(void *pvParameters)
{
MAP_POINT_t *p;
p = &map_point[point_1];
uint8_t data_len = 0;
while(1) {
vTaskDelay(100);

data_len = strlen((char*)data_buf);
if(data_len != 0){
gps_data_pick(point_1, data_buf, data_len);
memset(data_buf,0,data_len);
}
}
}