基于FPGA的手柄遥控排爆机器人

FPGA开发之 游戏手柄遥控排爆机器人

• 使用对象：PS2游戏手柄，L298N电机驱动模块，履带底盘，6自由度机械臂
• 使用环境：ISE14.7和BASYS2开发板

---

1.排爆机器人展示

整体原理简介
该排爆机器人硬件部分为一个履带底盘和一个6自由度的机械臂，通过PS2游戏手柄遥控，fpga开发板接收遥控信号并控制机器人的运动，输出6路PWM波控制6自由度机械臂，还有2路pwm波控制底盘电机。

2.Verilog代码实现

所用PS2游戏手柄的SPI协议解析和输出PWM波的原理可以参考代码注释和我的前两篇博客

`timescale 1ns / 1ps
module spi_control_pwm(
    input clk,              //板载50Mhz时钟信号
    input reset,的
    output [7:0] ledout,       //板上八个LED灯亮，指示正常工作状态以及第四个接收到byte
    output spi_clk,         //SPI通讯时钟线
    output spi_cs,          //SPI通讯片选信号线
    output spi_mosi,        //SPI通讯写端口
    input spi_miso,         //SPI通讯读端口
    inout [7:0] pwm_out,    //八路PWM输出
    output [3:0] motor_io     //控制电机正反转
    );
reg [1:0] moto1=0;    //控制电机正反转，原理参考L298N电机驱动模块原理图
reg [1:0] moto2=0;
reg [7:0] data_out; 
reg [7:0] data_in1;    //接收到的数据，八位的字节
reg [7:0] data_in2;
reg [7:0] data_in3;
reg [7:0] data_in4;
reg [7:0] data_in5;
reg [7:0] data_in6;
reg sclk=1;          //SPI通讯时钟线  
reg smosi=1;           //SPI通讯写
reg smiso=0;           //SPI通讯读
reg scs;                //SPI通讯片选信号
reg [9:0] cnt_clk_6us=0;
reg clk_6us=1;
reg [15:0] cnt_1020us=0;
reg clk_1020us=0;
reg [7:0] led_set=0;
reg trig=1;
reg [3:0] count_for_trig=0;
reg [7:0] count_trig=0;
reg [3:0] motor_set=0;
reg [7:0] pwm_set=0;
reg [19:0] count_for_pwmclk=0;
reg pwm_clk=0;
always @(posedge clk)
begin
        if(cnt_clk_6us == 10'b00_1001_0110-1) begin     //产生6us信号
            cnt_clk_6us <= 0;
            clk_6us <= ~clk_6us;   //按位取反
        end
        else
            cnt_clk_6us <= cnt_clk_6us + 1;


            if(cnt_1020us == 16'b0110_0011_1001_1100-1) begin    //周期1020us
            cnt_1020us <= 0;
            clk_1020us <= ~clk_1020us;  
        end
        else
            cnt_1020us <= cnt_1020us + 1;

            if(count_for_pwmclk == 20'b0000_0000_0000_1111_1010-1) begin    // 0.01ms触发一次，故pwm波形精度为0.01ms
            count_for_pwmclk <= 0;
            pwm_clk <= ~pwm_clk;   //按位取反
        end
        else
            count_for_pwmclk <= count_for_pwmclk + 1;
end


reg [11:0] count_pwm=0;
reg [11:0] pwm_compare1=12'b0000_1001_0110;  //初值定在150，即1.5ms，是舵机的中位
reg [11:0] pwm_compare2=12'b0000_1001_0110;
reg [11:0] pwm_compare3=12'b0000_1001_0100;   //初值148，控制360度舵机  
reg [11:0] pwm_compare4=12'b0000_1001_0110;
reg [11:0] pwm_compare5=12'b0000_1001_0110;
reg [11:0] pwm_compare6=12'b0000_1001_0110;
reg [11:0] pwm_compare7=12'b0000_1001_0110;
reg [11:0] pwm_compare8=12'b0000_1001_0110;
reg [11:0] speed_temp;

reg pwm_flag1=0;
reg pwm_flag2=0;
reg pwm_flag3=0;
reg pwm_flag4=0;
reg pwm_flag5=0;
reg pwm_flag6=0;
reg pwm_flag7=0;
reg pwm_flag8=0;

always @( posedge clk_1020us)
begin
led_set<=data_in1;
count_for_trig<=count_for_trig+1;
    if (count_for_trig==4'b0001)
    trig<=0;
    else if (count_for_trig==4'b0010)
    trig<=1;
    else    if (count_for_trig==4'b1010)
    count_for_trig<=4'b0000;
end
    控制pwm信号/
always @(posedge scs)    //每10ms跟新一次PWM输出和电机驱动状态
begin
    if ((data_in1[4]==0)&(pwm_compare1<250))    //control pwm1
    pwm_compare1<=pwm_compare1+1;
    else if((data_in1[6]==0)&(pwm_compare1>50))
    pwm_compare1<=pwm_compare1-1;

    if ((data_in1[7]==0)&(pwm_compare2<250))    //control pwm2
    pwm_compare2<=pwm_compare2+1; 
    else if((data_in1[5]==0)&(pwm_compare2>50))
    pwm_compare2<=pwm_compare2-1;

    if ((data_in2[2]==0)&(pwm_compare3<250))    //control pwm3  1.48ms控制360度舵机
    pwm_compare3<=pwm_compare3+1;
    else if((data_in2[3]==0)&(pwm_compare3>50))
    pwm_compare3<=pwm_compare3-1;

    if ((data_in2[6]==0)&(pwm_compare4<250))    //control pwm4
    pwm_compare4<=pwm_compare4+1;
    else if((data_in2[4]==0)&(pwm_compare4>50))
    pwm_compare4<=pwm_compare4-1;

    if ((data_in2[0]==0)&(pwm_compare5<250))        //control pwm5
    pwm_compare5<=pwm_compare5+1;
    else if((data_in2[1]==0)&(pwm_compare5>50))
    pwm_compare5<=pwm_compare5-1;

    if ((data_in2[7]==0)&(pwm_compare6<250))        //control pwm6
    pwm_compare6<=pwm_compare6+1;
    else if((data_in2[5]==0)&(pwm_compare6>50))
    pwm_compare6<=pwm_compare6-1;





    if ((data_in4<127)|(data_in4>132))      //检测左摇杆Y方向是否拨动
    begin
        if (data_in4<123)  //前进
            begin
            speed_temp<=((~data_in4)&7'b01111111)<<4;
            if ((data_in5>126)&(data_in5<135)) //偏向拨杆没有拨动  
            begin
            pwm_compare7<=speed_temp;
            pwm_compare8<=speed_temp;
            end
            else if (data_in5<127)
                begin
                pwm_compare8<=speed_temp-(((~data_in5)&7'b01111111)<<4);   //左右偏航
                pwm_compare7<=speed_temp;
                end
            else if (data_in5>132)
                    begin
                    pwm_compare7<=speed_temp-((data_in5-128)<<4);
                    pwm_compare8<=speed_temp;
                    end

            moto1<=2'b10;
            moto2<=2'b10;
            end
           else if(data_in4>132) //后退
                    begin
                    moto1<=2'b01;
                    moto2<=2'b01;
                    pwm_compare7<=(data_in4-128)<<3;
                    pwm_compare8<=pwm_compare7;
                    end
    end
    else if((data_in4>126)&(data_in4<132))
            begin
                moto1<=2'b00;
                moto2<=2'b00;
                if((data_in3<123)|(data_in3>132))    //原地左右转
                begin
                    if (data_in3<123)
                    begin
                    pwm_compare7<=((~data_in3)&7'b01111111)<<3;
                    pwm_compare8<=((~data_in3)&7'b01111111)<<3;
                    moto1<=2'b10;
                    moto2<=2'b01;
                    end

                   else if (data_in3>132)
                            begin
                            pwm_compare7<=(data_in3-128)<<3;
                            pwm_compare8<=(data_in3-128)<<3;
                            moto1<=2'b01;
                            moto2<=2'b10;
                            end
                end
         end



end


always @(posedge pwm_clk)    //控制八路pwm信号输出
begin
    count_pwm<=count_pwm+1;

    if (count_pwm < pwm_compare1)   //pwm1
        pwm_flag1<=1;
        else
        pwm_flag1<=0;

    if (count_pwm < pwm_compare2)   //pwm2
        pwm_flag2<=1;
        else
        pwm_flag2<=0;

    if (count_pwm < pwm_compare3)   //pwm3
        pwm_flag3<=1;
        else
        pwm_flag3<=0;

    if (count_pwm < pwm_compare4)   //pwm4
        pwm_flag4<=1;
        else
        pwm_flag4<=0;   

    if (count_pwm < pwm_compare5)   //pwm5
        pwm_flag5<=1;
        else
        pwm_flag5<=0;

    if (count_pwm < pwm_compare6)   //pwm6
        pwm_flag6<=1;
        else
        pwm_flag6<=0;

    if (count_pwm < pwm_compare7)   //pwm7
        pwm_flag7<=1;
        else
        pwm_flag7<=0;

    if (count_pwm < pwm_compare8)   //pwm8
        pwm_flag8<=1;
        else
        pwm_flag8<=0;   


    if (count_pwm ==12'b0111_1101_0000-1)
    count_pwm<=0;

end

always @(negedge clk_6us)
begin
    if (trig==0)
    begin
    scs<=0;
    count_trig<=count_trig+1;
    //spi_clk波形产生
            if ((0<count_trig)&(count_trig<17))   //byte1
            sclk<=~sclk;
            else if ((19<count_trig)&(count_trig<36))  //byte2
            sclk<=~sclk;
            else if ((38<count_trig)&(count_trig<55))  //byte3
            sclk<=~sclk;
            else if ((57<count_trig)&(count_trig<74))  //byte4
            sclk<=~sclk;
            else if ((76<count_trig)&(count_trig<93))  //byte5
            sclk<=~sclk;
            else if ((95<count_trig)&(count_trig<112))  //byte6
            sclk<=~sclk;
            else if ((114<count_trig)&(count_trig<131))  //byte7
            sclk<=~sclk;
            else if ((133<count_trig)&(count_trig<150))  //byte8
            sclk<=~sclk;
            else if ((152<count_trig)&(count_trig<169))  //byte9
            sclk<=~sclk;

    ///mosi波形产生///
    if (count_trig<2)
        smosi<=1;
    else if ((20<count_trig)&(count_trig<23))
        smosi<=1;
    else if ((30<count_trig)&(count_trig<33))
        smosi<=1;
    else 
        smosi<=0;


    //读取miso



        if (count_trig==58)    //读byte4
        data_in1[0]<=spi_miso;
    else if (count_trig==60)
        data_in1[1]<=spi_miso;
    else if (count_trig==62)
        data_in1[2]<=spi_miso;
    else if (count_trig==64)
        data_in1[3]<=spi_miso;
    else if (count_trig==66)
        data_in1[4]<=spi_miso;
    else if (count_trig==68)
        data_in1[5]<=spi_miso;
    else if (count_trig==70)
        data_in1[6]<=spi_miso;
    else if (count_trig==72)
        data_in1[7]<=spi_miso;  


    if (count_trig==77)    //读byte5
        data_in2[0]<=spi_miso;
    else if (count_trig==79)
        data_in2[1]<=spi_miso;
    else if (count_trig==81)
        data_in2[2]<=spi_miso;
    else if (count_trig==83)
        data_in2[3]<=spi_miso;
    else if (count_trig==85)
        data_in2[4]<=spi_miso;
    else if (count_trig==87)
        data_in2[5]<=spi_miso;
    else if (count_trig==89)
        data_in2[6]<=spi_miso;
    else if (count_trig==91)
        data_in2[7]<=spi_miso;  

    if (count_trig==96)    //读byte6
        data_in3[0]<=spi_miso;
    else if (count_trig==98)
        data_in3[1]<=spi_miso;
    else if (count_trig==100)
        data_in3[2]<=spi_miso;
    else if (count_trig==102)
        data_in3[3]<=spi_miso;
    else if (count_trig==104)
        data_in3[4]<=spi_miso;
    else if (count_trig==106)
        data_in3[5]<=spi_miso;
    else if (count_trig==108)
        data_in3[6]<=spi_miso;
    else if (count_trig==110)
        data_in3[7]<=spi_miso;  

        if (count_trig==115)    //读byte7
        data_in4[0]<=spi_miso;
    else if (count_trig==117)
        data_in4[1]<=spi_miso;
    else if (count_trig==119)
        data_in4[2]<=spi_miso;
    else if (count_trig==121)
        data_in4[3]<=spi_miso;
    else if (count_trig==123)
        data_in4[4]<=spi_miso;
    else if (count_trig==125)
        data_in4[5]<=spi_miso;
    else if (count_trig==127)
        data_in4[6]<=spi_miso;
    else if (count_trig==129)
        data_in4[7]<=spi_miso;

                if (count_trig==134)    //读byte8
        data_in5[0]<=spi_miso;
    else if (count_trig==136)
        data_in5[1]<=spi_miso;
    else if (count_trig==138)
        data_in5[2]<=spi_miso;
    else if (count_trig==140)
        data_in5[3]<=spi_miso;
    else if (count_trig==142)
        data_in5[4]<=spi_miso;
    else if (count_trig==144)
        data_in5[5]<=spi_miso;
    else if (count_trig==146)
        data_in5[6]<=spi_miso;
    else if (count_trig==148)
        data_in5[7]<=spi_miso;


    if (count_trig==153)    //读byte8
        data_in6[0]<=spi_miso;
    else if (count_trig==155)
        data_in6[1]<=spi_miso;
    else if (count_trig==157)
        data_in6[2]<=spi_miso;
    else if (count_trig==159)
        data_in6[3]<=spi_miso;
    else if (count_trig==161)
        data_in6[4]<=spi_miso;
    else if (count_trig==163)
        data_in6[5]<=spi_miso;
    else if (count_trig==165)
        data_in6[6]<=spi_miso;
    else if (count_trig==167)
        data_in6[7]<=spi_miso;  
    end

    else if(trig==1)
        begin
        scs<=1;
        sclk<=1;
        count_trig<=0;
        end


end


assign pwm_out[0]=pwm_flag1;
assign pwm_out[1]=pwm_flag2;
assign pwm_out[2]=pwm_flag3;
assign pwm_out[3]=pwm_flag4;
assign pwm_out[4]=pwm_flag5;
assign pwm_out[5]=pwm_flag6;
assign pwm_out[6]=pwm_flag7;
assign pwm_out[7]=pwm_flag8;
assign motor_io[0]=moto1[1];
assign motor_io[1]=moto1[0];
assign motor_io[2]=moto2[1];
assign motor_io[3]=moto2[0];
assign ledout = led_set;
assign spi_clk=sclk;
assign spi_cs=scs;
assign spi_cs=scs;
assign spi_mosi=smosi;

endmodule

3.供电系统接法以及负载注意事项

本机器人上一共有五个电压档次
- 电池是22.4V,30C,6s的锂电池
- 电机是9V的直流电机
- 舵机驱动电压是6~7.4V
- FPGA开发板是5V的直流电压输出，通过板载电源芯片3.3V电压输出

下面分块验证负载情况

因为电池是2600mah,30C的航模锂电池，输出电流可以达到22V、7.8A，故采用两个稳压模块将22.4V电池电压分别降到和7V。每个稳压源最大输出电流5A。

对于电机驱动而言，我们采用的是L298N舵机驱动模块，电机的最大电流200mA，L298N的最大输出电流为2A，电机电流400mA小于L298N输出电流2A小于降压模块输出电流5A，故此路电流没有问题。

对于舵机而言，由于店家给不出工作电流

所以我实测了最大负载臂的驱动舵机峰值电流为1.1A

这样的六个舵机并联在一起，当舵机联动时负载电流可能超过稳压模块输出的5A，导致供电不足，系统重启。

以上供电系统任然有缺陷存在，写这些是为了提醒大家在做实物的时候需要考虑到供电问题以及一些模电知识，才能做出稳定可靠的系统。