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: 646874160@qq.com 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 = (cnt<m_vef)?1'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
