【红色飓风Nano二代测评】小车动起来+超声波

发表于 4/6/2014 5:45:08 PM 阅读（4004）

给自己放了一天假，又投入到学习中了，之前一直在调试基于NANO2的机械臂控制，然而小车已经到了，迫不及待，果断先来动动小车来试试。

意外发现：在ise中，似乎在顶层模块中例化其他的子模块的时候，如果两个子模块的信号都连接到某个wire信号上，那么在顶层文件中必须要例化前申明这个wire信号，而在quartus中式不需要的，很诧异的一件事情。

小车：

开发环境：ise14.6，硬件平台：NANO2

实现思路：这次的实验仅仅是对小车运动速度和超声波的一个联用，一开始小车以固定速度行驶，当超声波检测到障碍物时（20-40cm会引起速度的改变，同时会使led亮），这里简单的将一个电机的速度将到很低，而一个电机的速度基本不变，这个小车就能避开障碍物了，当然，这仅仅是测试而已。

电机驱动器：这里采用TI的DRV8412来做直流电机的驱动器，因为是实验室现成的东西，用起来很方便。

FPGA设计：

顶层文件

`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date:    20:41:24 04/04/2014 
// Design Name: 
// Module Name:    motor_sonic 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.01 - File Created
// Additional Comments: 
//
//////////////////////////////////////////////////////////////////////////////////
module motor_sonic(
    input clk,
    input rst_n,
    input echo,
    output trig,
    output pwm_a,
    output pwm_b,
	 output pwm_c,
    output pwm_d,
    output led
    );
	 wire [9:0]speed_motor1,speed_motor2;
wire [23:0]distance;
ultrasonic		u1(
						.clk(clk),
						.rst_n(rst_n),
						.echo(echo),
						.trig(trig),
						.led(led),
						.distance(distance)
						);
						
pwm				u2(
						.clk(clk),
						.rst_n(rst_n),
						.speed(speed_motor1),
						.pwm_1(pwm_a),
						.pwm_2(pwm_b)
						);
						
pwm				u3(
						.clk(clk),
						.rst_n(rst_n),
						.speed(speed_motor2),
						.pwm_1(pwm_c),
						.pwm_2(pwm_d)
						);
//reg [9:0]speed_temp1,speed_temp2;
assign speed_motor1=((distance>=24'd20)&(distance<=24'd40))?250:300;
assign speed_motor2=((distance>=24'd20)&(distance<=24'd40))?150:300;

endmodule

超声波检测：

`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date:    18:52:06 04/02/2014 
// Design Name: 
// Module Name:    ultrasonic 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.01 - File Created
// Additional Comments: 
//
//////////////////////////////////////////////////////////////////////////////////
module ultrasonic(
    input clk,
    input rst_n,
    input echo,
    output trig,
    output led,
	 output [23 : 0] distance
    );

//产生周期60ms，10us脉冲的触发信号trig，clk为50M
reg [21:0]cnt;
always @(posedge clk)
begin
	if(!rst_n)
		cnt<=0;
	else	if(cnt==22'd2999999)
				cnt<=0;
			else
				cnt<=cnt+1'b1;
end
assign trig=((cnt>=22'd100) & (cnt<=22'd599)) ? 1:0;

//测量echo的长度
reg echo_reg1,echo_reg2;
wire start,finish;
always @(posedge clk)
begin
	if(!rst_n)
		begin
			echo_reg1<=0;
			echo_reg2<=0;
		end
	else
		begin
			echo_reg1<=echo;
			echo_reg2<=echo_reg1;
		end
end
//这里还不完善，外一有扰动就悲剧了
assign start=echo_reg1 & (~echo_reg2);   //捕捉上升沿
assign finish=(~echo_reg1) & echo_reg2;   //捕捉下降沿
//测量从start到finish这段时间
reg [23:0]distance_cnt;
reg [23:0]distance_temp;
parameter idle=2'b00;
parameter state1=2'b01;
parameter state2=2'b10;
reg [1:0]state;
always @(posedge clk)
begin
	if(!rst_n)
		begin	
			state<=idle;
			distance_cnt<=0;
			distance_temp<=0;
		end
	else	
		begin
			case(state)
			idle: 	begin
							if(start)
								state<=state1;
							else
								state<=idle;
						end
			state1:	begin
							if(finish)
								state<=state2;
							else
								begin
									distance_cnt<=distance_cnt+1'b1;
									state<=state1;
								end
						end
			state2:  begin
							distance_cnt<=0;
							distance_temp<=distance_cnt;
							state<=idle;
						end
			default: state<=idle;
			endcase
		end
end

//根据公式 us： 20*distance_temp/1000，分母为58，总的来说就是 s=distance_temp/2900，调用ip核
wire [11:0] divisor;  //除数
assign divisor=12'd2900;
wire [11 : 0] fractional; //省去
wire rfd;
//除法器例化
div div (
	.clk(clk), // input clk
	.rfd(rfd), // output rfd ready for data和ip核设置有关，这里没什么关系
	.dividend(distance_temp), // input [23 : 0] dividend
	.divisor(divisor), // input [11 : 0] divisor
	.quotient(distance), // output [23 : 0] quotient
	.fractional(fractional)); // output [11 : 0] fractional
	
//led指示状态，20-40cm认为是有障碍物
reg led_state;
always @(posedge clk)
begin
	if(!rst_n)
		led_state<=0;
	else	if((distance>=24'd20)&(distance<=24'd40))
				led_state<=1;
			else
				led_state<=0;
end
assign led=led_state;
endmodule

pwm模块

`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date:    20:51:36 04/04/2014 
// Design Name: 
// Module Name:    pwm 
// Project Name: 
// Target Devices: 
// Tool versions: 
// Description: 
//
// Dependencies: 
//
// Revision: 
// Revision 0.01 - File Created
// Additional Comments: 
//
//////////////////////////////////////////////////////////////////////////////////
module pwm(
    input clk,
    input rst_n,
    input [9:0] speed,
    output pwm_1,
    output pwm_2
    );


always @(posedge clk)
begin
	if(!rst_n)
		cnt<=0;
	else
		cnt<=cnt+1'b1;
end
reg [9:0]cnt;
assign pwm_1=(cnt<=speed)?1:0;
assign pwm_2=0;

endmodule

视频链接：

http://v.youku.com/v_show/id_XNjk1MjQwOTY4.html

说明：因为在FPGA中取好了障碍物在20-40的时候才会引起电机的速度变化。

做到这里，觉得自己的某些能力是比较差的，比如是无法把东西弄的漂漂亮亮，还有没有电池，都不敢把电机放在地上跑，只能用砖来垫起来，还是就是机械手到时候放哪里呢？这些都是一个头疼的问题，不知道到时候小车项目能不能做完，实现既定的功能。不管怎么样，现阶段还是把机械手控制调好，上位机写好，还有一段路要走啊。

« 上一篇：【红色飓风Nano二代测评】超声波检测

» 下一篇：STM32 Printf函数实现方法（转载）