dp_test.vhd

本程序是用VHDL语言编写的

begin
	if (rst ='0' ) then A_servo_stop <='0';
	else
		if	(clk_in'event and clk_in ='1')	then
			if(servo_poisition(0)='0' or servo_poisition(3)='0') then
				A_servo_stop <='1';
			else
				A_servo_stop <=A_servo_stop;
			end if;
		end	if;
	end if;
end	process A_servo_sensor_test;

------------------------伺服电机驱动使能--------------------------------

A_servo_enable:	process(clk_in,rst,Servo_unable)
begin
	--if (rst ='0' or Servo_unable ='1' )	then
	if (Servo_unable ='1'  )	then
		A_servo_on <='1';  A_sv_clr <='0';	-- 禁止驱动
	else
		if	(clk_in'event and clk_in ='1')	then
			if	(Servo_unable ='0' ) then
				A_servo_on <='1';  A_sv_clr <='0';	-- 禁止驱动
			--else 
			--case	A_sv_enable  is
				--when	"10"  => A_servo_on <='1';  A_sv_clr <='0';	-- 禁止驱动
				--when	"01"  => A_servo_on <='0';	A_sv_clr <='1';-- 驱动
				--when	others=> null;
			--end	case;
				--A_servo_on <='0';	A_sv_clr <='1';-- 驱动
			end if;
		end if;
	end	if;
end	process	A_servo_enable;
------------------------------------------------------------------
	
-----------伺服电机A控制---------------------------------------
		
Servor_A: process (clk_in)
begin
	if	(clk_in'event and clk_in ='1')	then
		if	rst ='0' then	A_servo_dir<='0';	A_run_state <= "00"; A_speed_ctr<="0000";
		else
			case	A_servo is
				when	"00000000"	=>			A_servo_dir<='0';	 A_run_state <= "00";A_speed_ctr<="0000";	-- 停止
		
				when	"00000101"	=>			A_servo_dir<='0';	 A_run_state <= "10";A_speed_ctr<="0000";	-- 正向减速
		
				when	"00000011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0000";	-- 正向加速至最慢速度
				when	"00010011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0001";	-- 正向加速至第2档速度
				when	"00100011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0010";	-- 正向加速至第3档速度
				when	"00110011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0011";	-- 正向加速至第4档速度
				when	"01000011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0100";	-- 正向加速至第5档速度
				when	"01010011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0101";	-- 正向加速至第6档速度
				when	"01100011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0110";	-- 正向加速至第7档速度
				when	"01110011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="0111";	-- 正向加速至第8档速度
				when	"10000011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="1000";	-- 正向加速至第9档速度
				when	"10010011"	=>			A_servo_dir<='0';	 A_run_state <= "01";A_speed_ctr<="1001";	-- 正向加速至最快速度
		
		
				when	"00001011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0000";	-- 反向加速至最慢速度
				when	"00011011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0001";	-- 反向加速至第2档速度
				when	"00101011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0010";	-- 反向加速至第3档速度
				when	"00111011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0011";	-- 反向加速至第4档速度
				when	"01001011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0100";	-- 反向加速至第5档速度
				when	"01011011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0101";	-- 反向加速至第6档速度
				when	"01101011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0110";	-- 反向加速至第7档速度
				when	"01111011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="0111";	-- 反向加速至第8档速度
				when	"10001011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="1000";	-- 反向加速至第9档速度
				when	"10011011"	=>			A_servo_dir<='1';	 A_run_state <= "01";A_speed_ctr<="1001";	-- 反向加速至最快速度
		
				when	"00001101"	=>			A_servo_dir<='1';	 A_run_state <= "10";A_speed_ctr<="0000";	-- 反向减速
				when	others		=>		    A_servo_dir<='0';    A_run_state <= "00";A_speed_ctr<="0000";	-- 停止
			end		case;
		end	if;
	end if;
end		process	servor_A;
---------------------------------------------------------------
-- DDS	频率合成产生伺服控制器脉冲
---------------------------------------------------------------
process(clk_in,rst)     --相位控制字产生
begin
	if(clk_in'event and clk_in='1') then
		if rst='0' then
			A_servo_fre <=A_fre_min;
		else
			if (A_phase_ctr='1') 	then
			case 	A_run_state is
			when	"01" => 	if(A_servo_fre >A_frequecy) then  --加速
									A_servo_fre <=A_servo_fre-180;
								else
									A_servo_fre <=A_frequecy;
								end if;
			when	"10" =>		if(A_servo_fre<A_fre_min)  then	 -- 减速
									A_servo_fre <=A_servo_fre+180;
								else
									A_servo_fre <=A_fre_min;	
								end if;				
			when	others =>	A_servo_fre <= A_servo_fre;				
			end 	case;		
		   end if;
		end if;
	end if;	
end process;

speed_ctr:	process(clk_in)
begin
	if (clk_in'event and clk_in='1') then
		case	A_speed_ctr is
			when	"0000" =>	A_frequecy <=A_fre_max0;
			when	"0001" =>	A_frequecy <=A_fre_max1;
			when	"0010" =>	A_frequecy <=A_fre_max2;
			when	"0011" =>	A_frequecy <=A_fre_max3;
			when	"0100" =>	A_frequecy <=A_fre_max4;
			when	"0101" =>	A_frequecy <=A_fre_max5;
			when	"0110" =>	A_frequecy <=A_fre_max6;
			when	"0111" =>	A_frequecy <=A_fre_max7;
			when	"1000" =>	A_frequecy <=A_fre_max8;
			when	"1001" =>	A_frequecy <=A_fre_max9;
			when	others =>	A_frequecy <=A_fre_max0;
		end	case;
	end if;
end	process	speed_ctr;

A_fre_buf  <= A_servo_fre;
Servo_A_on <= A_servo_on;--A_servo_on;	--伺服常开
Servo_A_clr<= A_sv_clr; 
Servo_A_CP <= A_servo_cp;-- 输出
Servo_A_dir<= A_servo_dir;

-----------------------------------------------------------
process(clk_in)      -- 波形生成
begin
	if (clk_in'event and clk_in='1') then
		if(rst='0' or A_servo_on ='1') then
			A_servo_cp<='0';
			A_phase_ctr<='0';
			A_servo_counter<=0;
		else
			if(A_servo_counter<A_fre_buf) then
				A_servo_counter<=A_servo_counter+1;	
				A_phase_ctr<='0';
			else
				A_servo_counter<=0;	
				A_phase_ctr<='1';
			end if;
		
			if(A_servo_counter<A_fre_buf/2) then
				A_servo_cp<='1';
			else
				A_servo_cp<='0';
			end if;
		end if;
	end if;
end	process;
------------------------------------------------------
		
servo_dir_test <=A_ccw_test;		--A_servo_pdir;
---------------------------------------------------------------------
------------------------------ 编码器处理----------------------------
---------------------------------------------------------------------
-------------------------伺服电机编码器清零----------------------------------
A_servo_code_clr_enable:	process(clk_in)
begin
	if	(clk_in'event and clk_in ='1')	then
		case	A_servo_count_clr is
			when	"10"  => A_servo_cp_clr <='1';	-- 清零
			when	"01"  => A_servo_cp_clr <='0';	-- 解除清零
			when	others=> null;
		end		case;
	end	if;
end	process	A_servo_code_clr_enable;
---------------------------------------------------------------------
A_phase_a	  <= Servo_Phase_A;
A_phase_b	  <= Servo_Phase_B;
Not_A_phase_a <= not Servo_Phase_A;	-- A相反向
------------伺服电机转动反方向计数------------------------------------
Code_A_cw	:process (A_phase_a,A_phase_b,A_servo_cp_clr)
	begin	
		if	A_servo_cp_clr = '1'	then	A_servo_cp_cw<="0000000000000000";
		else
			if(A_phase_a'event and A_phase_a ='0')	then
				if	A_phase_b ='1'	then
					A_servo_cp_cw <= A_servo_cp_cw + "0000000000000001";
					A_cw_test	<= '1';
					
				else
					A_servo_cp_cw <= A_servo_cp_cw;
					A_cw_test	<= '0';
				end	if;
			end	if;
		end	if;
end	process	Code_A_cw;
------------伺服电机转动反方向计数------------------------------------
Code_A_ccw	:process (Not_A_phase_a,A_phase_b,A_servo_cp_clr)
	begin	
		if	A_servo_cp_clr = '1'	then	A_servo_cp_ccw<="0000000000000000";
		else
			if(Not_A_phase_a'event and Not_A_phase_a ='0')	then
				if	A_phase_b ='1'	then
					A_servo_cp_ccw <= A_servo_cp_ccw + "0000000000000001";
					A_ccw_test	<= '1';
				else
					A_servo_cp_ccw <= A_servo_cp_ccw;
				    A_ccw_test	<= '0';  
				end	if;
			end	if;
		end	if;
end	process	Code_A_ccw;

------------伺服电机转动方向判别------------------------------------
A_servo_direction : process(A_phase_a,A_phase_b)
	begin	
		if	(A_phase_a'event and A_phase_a ='0')	then		-- 下降沿
			if	A_phase_b ='1'	then	
				A_servo_pdir <='0';
			else
				A_servo_pdir <='1';
			end	if;
		end  if;
end	process	A_servo_direction;
			
-------------------------------------------------------------------------
-----------------伺服电机A编码器计数-------------------------------------

A_servo_number:process(clk_in)
begin
	if	(clk_in'event and clk_in ='1')	then
		if	A_servo_cp_clr='1' then	A_cp_number<="0000000000000000";
		else
			if	(A_servo_cp_cw > A_servo_cp_ccw) then
				A_cp_number <= A_servo_cp_cw -A_servo_cp_ccw;
			else	
				A_cp_number <= A_servo_cp_ccw -A_servo_cp_cw;
			end	if;
		end	if;
	end  if;
end		process	A_servo_number;


		
----中断产生 -----------------------------------------------------

servo_cp_test<= A_phase_a;
--CPLD_MCU_int <= A_cp_number(0);
-------------------------------------------------------------------

end	exam;
-----------------------------------------------------------------