【PYNQ-Z2试用体验】基于PYNQ的神经网络自动驾驶小车-多路电机控制（结项）

单片机

`在上一篇帖子中，我们完成了“基于PYNQ的神经网络自动驾驶小车”的硬件搭建。本文我们将继续完善小车的软件框架，为小车的底盘编写软件。
小车底盘有两种控制方案：
1、在Zynq7020的PL中创建硬件控制逻辑，由PYNQ-Z2直接控制，作品具有更高的集成度；
2、通过单片机专门对其进行控制，将PYNQ从底层IO控制中解放，便于后续对底盘单独通过单片机单独实现为ROS节点。
在这篇帖子中，我们实现第一种。

我使用Verilog对底盘控制进行实现，实现4路PWM输出与8路方向控制输出，并将整个部分打包为IP，由PS部分进行控制。
首先打开Vivado环境，创建一个新的RTL工程，命名为“chassis_control”，设置芯片为xc7z020clg400-1。
接下来选择“Tools -> Create and Package New IP”。

选择“Create a new AXI4 peripheral”，并填写IP名等基本信息。
我们的设计中只需要通过填写6个寄存器对底盘进行控制，所以在第三步中，选择AXI总线类型为Lite、从模式、32位总线、6个寄存器。

在最后一步中，“Next Steps”选择“Edit IP”，并点击完成，就完成了AXI-Lite总线代码的生成，Vivado将自动为IP创建一个新的工程。

在新的工程中，首先实现并添加PWM生成器与方向控制的Verilog代码。
PWM模块如下：

module pwm
#(
parameter N = 32 //pwm bit width
)(
input clk,
input rst,
input [N-1:0]period,
input [N-1:0]duty,
output pwm_out
);
reg [31:0]fre_cnt;
always @(posedge clk)begin
if(rst==1'b0)begin
fre_cnt <=32'd0;
end
else begin
if(fre_cnt
fre_cnt <= fre_cnt+1'b1;
end
else begin
fre_cnt<=32'd0;
end
end
end
assign pwm_out = (duty>fre_cnt);
endmodule

复制代码

方向控制模块代码如下：

module chassis_control
#(parameter DIRECtiON_WIDTH=3) (
input wire clk, rst_n,
input wire [DIRECTION_WIDTH-1:0] direction,
output reg [7:0] DIR_output
);
always @ (posedge clk)
if(rst_n == 1'b0)begin
DIR_output[0] <=1'b0;
DIR_output[1] <=1'b0;
DIR_output[2] <=1'b0;
DIR_output[3] <=1'b0;
DIR_output[4] <=1'b0;
DIR_output[5] <=1'b0;
DIR_output[6] <=1'b0;
DIR_output[7] <=1'b0;
end
else
begin
case (direction)
3'd0 ://Stop
begin
DIR_output[0] <=1'b0;
DIR_output[1] <=1'b0;
DIR_output[2] <=1'b0;
DIR_output[3] <=1'b0;
DIR_output[4] <=1'b0;
DIR_output[5] <=1'b0;
DIR_output[6] <=1'b0;
DIR_output[7] <=1'b0;
end
3'd1 : //Forward
begin
DIR_output[0] <=1'b1;
DIR_output[1] <=1'b0;
DIR_output[2] <=1'b1;
DIR_output[3] <=1'b0;
DIR_output[4] <=1'b1;
DIR_output[5] <=1'b0;
DIR_output[6] <=1'b1;
DIR_output[7] <=1'b0;
end
3'd2 : //Backword
begin
DIR_output[0] <=1'b0;
DIR_output[1] <=1'b1;
DIR_output[2] <=1'b0;
DIR_output[3] <=1'b1;
DIR_output[4] <=1'b0;
DIR_output[5] <=1'b1;
DIR_output[6] <=1'b0;
DIR_output[7] <=1'b1;
end
3'd3 : //Left
begin
DIR_output[0] <=1'b0;
DIR_output[1] <=1'b1;
DIR_output[2] <=1'b0;
DIR_output[3] <=1'b1;
DIR_output[4] <=1'b1;
DIR_output[5] <=1'b0;
DIR_output[6] <=1'b1;
DIR_output[7] <=1'b0;
end
3'd4: //Right
begin
DIR_output[0] <=1'b1;
DIR_output[1] <=1'b0;
DIR_output[2] <=1'b1;
DIR_output[3] <=1'b0;
DIR_output[4] <=1'b0;
DIR_output[5] <=1'b1;
DIR_output[6] <=1'b0;
DIR_output[7] <=1'b1;
end
default:
begin
DIR_output[0] <=1'b0;
DIR_output[1] <=1'b0;
DIR_output[2] <=1'b0;
DIR_output[3] <=1'b0;
DIR_output[4] <=1'b0;
DIR_output[5] <=1'b0;
DIR_output[6] <=1'b0;
DIR_output[7] <=1'b0;
end
endcase
end
endmodule

复制代码

打开IDE自动生成的“chassis_control_v1_0_S00_AXI.v”文件，为总线通信模块“chassis_control_v1_0_S00_AXI”添加输出端口“PWM_output”与“DIR_output”。

// Users to add ports here
output wire [3:0] PWM_output,
output wire [7:0] DIR_output,
// User ports ends

复制代码

在文件最下方，对pwm模块和方向控制模块进行例化，代码如下：

// Add user logic here
chassis_control #(.DIRECTION_WIDTH(3)) chassis_control_inst
(
.clk(S_AXI_ACLK),
.rst_n(S_AXI_ARESETN),
.direction(slv_reg0),
.DIR_output(DIR_output)
);
pwm #(.N(32)) pwm_instance_0 (
.clk(S_AXI_ACLK),
.rst(S_AXI_ARESETN),
.period(slv_reg1),
.duty(slv_reg2),
.pwm_out(PWM_output[0])
);
pwm #(.N(32)) pwm_instance_1 (
.clk(S_AXI_ACLK),
.rst(S_AXI_ARESETN),
.period(slv_reg1),
.duty(slv_reg3),
.pwm_out(PWM_output[1])
);
pwm #(.N(32)) pwm_instance_2 (
.clk(S_AXI_ACLK),
.rst(S_AXI_ARESETN),
.period(slv_reg1),
.duty(slv_reg4),
.pwm_out(PWM_output[2])
);
pwm #(.N(32)) pwm_instance_3 (
.clk(S_AXI_ACLK),
.rst(S_AXI_ARESETN),
.period(slv_reg1),
.duty(slv_reg5),
.pwm_out(PWM_output[3])
);
// User logic ends
endmodule

复制代码

打开IDE自动生成的“chassis_control_v1_0.v”文件，为顶层模块“chassis_control_v1_0”添加输出端口“PWM_output”与“DIR_output”：

output wire [3:0] PWM_output,
output wire [7:0] DIR_output,

复制代码

将其连接至“chassis_control_v1_0_S00_AXI”：

// Instantiation of Axi Bus Interface S00_AXI
chassis_control_v1_0_S00_AXI # (
.C_S_AXI_DATA_WIDTH(C_S00_AXI_DATA_WIDTH),
.C_S_AXI_ADDR_WIDTH(C_S00_AXI_ADDR_WIDTH)
) chassis_control_v1_0_S00_AXI_inst (
.PWM_output(PWM_output),
.DIR_output(DIR_output),
.S_AXI_ACLK(s00_axi_aclk),
...

复制代码

代码修改就全部完成了，点击“Run Synthesis”对其进行综合，综合结果确认代码无误后，再次点击“Tools -> Create and Package New IP”，这一次选择“Package your current project”，对上一次创建的IP进行overwrite，完成IP的最终封装。

关闭原工程，创建一个新的Vivado工程，命名为“chassis_control_test”，这一次在芯片选择界面，在“Boards”选项卡下选择“pynq-z2”板卡，这样会省去很多后续配置。

在新创建的工程中，点击“IP Catalog”，在出现的选项卡中，右击添加我们刚刚封装的IP。

随后，在左侧的“IP INTEGRATIOR”中通过“Create Block Design”创建新设计，在设计中通过添加IP，完成框图如下：

接下来，为新框图创建“Output Product”与“HDL Wrapper”。
为便于观察与测量控制效果，我们将四路PWM输出绑定至四颗板载LED，将8路方向输出绑定至Arduino接口中的AR0~AR7。

## LEDs
set_property -dict {PACKAGE_PIN R14 IOSTANDARD LVCMOS33} [get_ports {PWM_output_0[0]}]
set_property -dict {PACKAGE_PIN P14 IOSTANDARD LVCMOS33} [get_ports {PWM_output_0[1]}]
set_property -dict {PACKAGE_PIN N16 IOSTANDARD LVCMOS33} [get_ports {PWM_output_0[2]}]
set_property -dict {PACKAGE_PIN M14 IOSTANDARD LVCMOS33} [get_ports {PWM_output_0[3]}]
## Arduino GPIO
set_property -dict {PACKAGE_PIN T14 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[0]}]
set_property -dict {PACKAGE_PIN U12 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[1]}]
set_property -dict {PACKAGE_PIN U13 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[2]}]
set_property -dict {PACKAGE_PIN V13 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[3]}]
set_property -dict {PACKAGE_PIN V15 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[4]}]
set_property -dict {PACKAGE_PIN T15 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[5]}]
set_property -dict {PACKAGE_PIN R16 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[6]}]
set_property -dict {PACKAGE_PIN U17 IOSTANDARD LVCMOS33} [get_ports {DIR_output_0[7]}]

复制代码

综合布局生成“Bitstream”二进制流文件，并使用“File -> Export -> Export Hardware”与“File -> Export -> Export Block Design”导出硬件设计信息。
为了快速验证程序功能，我们先用“Xilinx SDK”对其进行验证。
打开“File -> Launch SDK”，在新生成的平台信息中，可见到chassis_control_0已包含在PS地址空间中。

我们创建一个新的“Application Project”，使用“Hello World”模板即可。
在“helloworld.c”中编写PWM测试程序，根据经验，在PWM占空比为0~30%时亮度变化明显。若硬件设计正确，四颗板载LED将交替呼吸亮灭。

#include
#include "platform.h"
#include "xil_printf.h"
#include "chassis_control.h"
#include "xil_io.h"
#include "xparameters.h"
#include "sleep.h"
int main()
{
unsigned int i;
init_platform();
print("PWM Testing
");
CHASSIS_CONTROL_mWriteReg(XPAR_CHASSIS_CONTROL_0_S00_AXI_BASEADDR, CHASSIS_CONTROL_S00_AXI_SLV_REG1_OFFSET, 10000);
while(1){
for(i = 0; i < 3500; i+=500)
{
CHASSIS_CONTROL_mWriteReg(XPAR_CHASSIS_CONTROL_0_S00_AXI_BASEADDR, CHASSIS_CONTROL_S00_AXI_SLV_REG2_OFFSET, i);
CHASSIS_CONTROL_mWriteReg(XPAR_CHASSIS_CONTROL_0_S00_AXI_BASEADDR, CHASSIS_CONTROL_S00_AXI_SLV_REG3_OFFSET, 3000-i);
CHASSIS_CONTROL_mWriteReg(XPAR_CHASSIS_CONTROL_0_S00_AXI_BASEADDR, CHASSIS_CONTROL_S00_AXI_SLV_REG4_OFFSET, i);
CHASSIS_CONTROL_mWriteReg(XPAR_CHASSIS_CONTROL_0_S00_AXI_BASEADDR, CHASSIS_CONTROL_S00_AXI_SLV_REG5_OFFSET, 3000-i);
//printf("Num= %d
", i);
sleep(2);
}
}
cleanup_platform();
return 0;
}

复制代码

注意在硬件仿真之前，须连接USB线，并将板卡Boot模式设置为JTAG模式。

最后下载运行测试程序，可以看到在PWM波驱动下LED交替呼吸亮灭。

下面将Boot模式调回SD卡，进入Linux系统，在Jupyter Notebook下进行底盘控制。
首先从chassis_control_test工程中提取Overlay相关文件，各命名为“chassis_control.tcl”与“chassis_control.bit”。
在Jupyter Notebook中创建一个新的文件夹，将“chassis_control.tcl”和“chassis_control.bit”上传，在同文件夹下创建Python3 Notebook程序。
Pynq软件框架提供了MMIO，全称为“Memory Mapped I/O”，使用它我们可以通过AXI总线访问PL中的底盘控制相关寄存器。
测试程序如下，