MG995舵机控制

左右按键，单次旋转15度。锁相环不分频、倍频，只是为了锁定频率。KEY_M键旋转到中间位置。

舵机的控制脉冲是0.5ms~2.5ms，1.5ms时居中，但是会存在一定的偏差。

  1 //=============================================================================
  2 //Module:    MG995
  3 //Author:    YangFei
  4 //Date:        2013-7-7
  5 //E-mail:    [email protected]
  6 //Function:    Steering Engine control
  7 //=============================================================================
  8 `timescale    1ns/1ns
  9 module    MG995_15D(
 10         EXT_CLK,    //external clock = 50MHz = 20us
 11         RST_N,        //low enable
 12         KEY_L,        //trun left
 13         KEY_M,        //trun middle
 14         KEY_R,        //trun right
 15         PWM_OUT_H,    //horizontal
 16 //        PWM_OUT_V    //vertical
 17         );
 18 input        EXT_CLK;
 19 input        RST_N;
 20 input        KEY_L;
 21 input        KEY_M;
 22 input        KEY_R;
 23 output        PWM_OUT_H;
 24 //output        PWM_OUT_V;
 25 wire        clk;
 26 //-----------------------------------------------------------------------------
 27 reg    [19:0]    cnt;
 28 always    @(posedge clk or negedge RST_N)
 29 begin
 30     if(!RST_N)
 31         cnt    <=    0;
 32     else if((!KEY_L)||(!KEY_M)||(!KEY_R))
 33     begin
 34         if(cnt>=20'd100_0000-1)    //100_0000*20us=20ms=a cycle
 35             cnt <= 0;
 36         else
 37             cnt    <=    cnt + 1;
 38     end
 39     else
 40         cnt    <=    0;
 41 end
 42 //-----------------------------------------------------------------------------
 43 //  total 185 degree
 44 //-----------------------------------------------------------------------------
 45 //  H 水平方向舵机
 46 /*
 47 wire        out_h_l;    //左  -92.5 degree
 48 wire        out_h_m;    //中    0
 49 wire        out_h_r;    //右  92.5
 50 //    理论值+-偏差
 51 assign        out_h_l = (!KEY_L)?((cnt<25000+7000)?1:0):0;//25000*20us=0.5ms
 52 assign        out_h_m = (!KEY_M)?((cnt<75000+7000)?1:0):0;//75000*20us=1.5ms
 53 assign        out_h_r = (!KEY_R)?((cnt<125000+10100)?1:0):0;//...=2.5ms
 54 assign        PWM_OUT_H = out_h_l|out_h_m|out_h_r;
 55 */
 56 //-----------------------------------------------------------------------------
 57 reg    [31:0]    m_vef;
 58 reg            FLAG;
 59 always    @(posedge clk or negedge RST_N)
 60 begin
 61     if(!RST_N)
 62     begin
 63         m_vef    <=    75000+7000;
 64         FLAG    <=    0;
 65     end
 66     else if(!KEY_L)
 67     begin
 68         if(!FLAG)
 69         begin
 70             if(m_vef<25000+7000)
 71                 m_vef <= 25000+7000;
 72             else
 73                 m_vef <= m_vef -8333;
 74         end
 75         FLAG    <=    1;
 76     end
 77     else if(!KEY_M)
 78     begin
 79         FLAG    <=    0;
 80         m_vef    <=    75000+7000;
 81     end
 82     else if(!KEY_R)
 83     begin
 84         if(!FLAG)
 85         begin
 86             if(m_vef>125000+10100)
 87                 m_vef    <=    125000+10100;
 88             else
 89                 m_vef    <=    m_vef + 8500;//8850;
 90         end
 91         FLAG     <=    1;
 92     end
 93     else
 94         FLAG    <=    0;
 95 end
 96 assign        PWM_OUT_H = (cnt1'b1:1'b0;
 97 //  V 垂直方向舵机
 98 /*
 99 wire        out_v_l;
100 wire        out_v_m;
101 wire        out_v_r;
102 assign        out_v_l = (!KEY_L)?((cnt<25000+4000)?1:0):0;
103 assign        out_v_m = (!KEY_M)?((cnt<75000-1000)?1:0):0;
104 assign        out_v_r = (!KEY_R)?((cnt<125000-5000)?1:0):0;
105 assign        PWM_OUT_V = out_v_l|out_v_m|out_v_r;
106 */
107 pll i_pll(
108     .inclk0    (EXT_CLK),
109     .c0        (clk)
110     );
111 
112 endmodule

 

 

转载于:https://www.cnblogs.com/phelonYang/p/3182842.html