module cordic_core(
input wire clk,
input wire reset_n,
output wire ready_out,
input wire [19:0] PosX_in, // 当前位置
input wire [19:0] PosY_in,
output wire [19:0] PosX_out, // 输出像素的位置
output wire [19:0] PosY_out,
input wire ready_in,
input wire [23:0] Zhi_PosX, // 当前位置
input wire [23:0] Zhi_PosY,
output wire [23:0] Ji_Len, // 极坐标的长 *16
output wire [29:0] Ji_Ang // 极坐标的角度 *1024
);
reg [29:0] reg_Ang_list[12:0];
always @(posedge clk or negedge reset_n)
if(!reset_n) begin
reg_Ang_list[0] = 30'd46080; //45.0000000000;
reg_Ang_list[1] = 30'd27203; //26.5650511771;
reg_Ang_list[2] = 30'd14373; //14.0362434679;
reg_Ang_list[3] = 30'd7296; // 7.1250163489;
reg_Ang_list[4] = 30'd3662; // 3.5763343750;
reg_Ang_list[5] = 30'd1833; // 1.7899106082;
reg_Ang_list[6] = 30'd917; // 0.8951737102;
reg_Ang_list[7] = 30'd458; // 0.4476141709;
reg_Ang_list[8] = 30'd229; // 0.2238105004;
reg_Ang_list[9] = 30'd115; // 0.1119056771;
reg_Ang_list[10] = 30'd57; // 0.0559528919;
reg_Ang_list[11] = 30'd29; // 0.0279764526;
reg_Ang_list[12] = 30'd14; // 0.0139882271;
end
///////////////////////////////////////
// 进行13次转角
wire [23:0] X_old; // 当前位置
wire [23:0] Y_old;
reg [29:0] A_old = 30'd0;
assign X_old = Zhi_PosX;
assign Y_old = Zhi_PosY;
// 1. 转45度..............................
reg [23:0] regX0_new;
reg [23:0] regY0_new;
reg [29:0] regA0_new;
reg [19:0] regPosX0; // 输出像素的位置
reg [19:0] regPosY0;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX0_new <= 23'd0;
regY0_new <= 23'd0;
regA0_new <= 30'd0;
regPosX0 <= 20'd0;
regPosY0 <= 20'd0;
end else if(ready_in) begin
if(Y_old[23]) begin // 负数
regX0_new <= X_old - Y_old;
regY0_new <= Y_old + X_old;
regA0_new <= A_old - reg_Ang_list[0];
end else begin // 正数
regX0_new <= X_old + Y_old;
regY0_new <= Y_old - X_old;
regA0_new <= A_old + reg_Ang_list[0];
end
regPosX0 <= PosX_in;
regPosY0 <= PosY_in;
end
end
// 2. 转 度..............................
reg [23:0] regX1_new;
reg [23:0] regY1_new;
reg [29:0] regA1_new;
reg [19:0] regPosX1; // 输出像素的位置
reg [19:0] regPosY1;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX1_new <= 24'd0;
regY1_new <= 24'd0;
regA1_new <= 30'd0;
regPosX1 <= 20'd0;
regPosY1 <= 20'd0;
end else if(ready_in) begin
if(regY0_new[23]) begin // 负数
regX1_new <= regX0_new - {regY0_new[23],regY0_new[23:1]};
regY1_new <= regY0_new + {regX0_new[23],regX0_new[23:1]};
regA1_new <= regA0_new - reg_Ang_list[1];
end else begin // 正数
regX1_new <= regX0_new + {regY0_new[23],regY0_new[23:1]};
regY1_new <= regY0_new - {regX0_new[23],regX0_new[23:1]};
regA1_new <= regA0_new + reg_Ang_list[1];
end
regPosX1 <= regPosX0;
regPosY1 <= regPosY0;
end
end
// 3. 转 度..............................
reg [23:0] regX2_new;
reg [23:0] regY2_new;
reg [29:0] regA2_new;
reg [19:0] regPosX2; // 输出像素的位置
reg [19:0] regPosY2;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX2_new <= 24'd0;
regY2_new <= 24'd0;
regA2_new <= 30'd0;
regPosX2 <= 20'd0;
regPosY2 <= 20'd0;
end else if(ready_in) begin
if(regY1_new[23]) begin // 负数
regX2_new <= regX1_new - { {2{regY1_new[23]}},regY1_new[23:2]};
regY2_new <= regY1_new + { {2{regX1_new[23]}},regX1_new[23:2]};
regA2_new <= regA1_new - reg_Ang_list[2];
end else begin // 正数
regX2_new <= regX1_new + { {2{regY1_new[23]}},regY1_new[23:2]};
regY2_new <= regY1_new - { {2{regX1_new[23]}},regX1_new[23:2]};
regA2_new <= regA1_new + reg_Ang_list[2];
end
regPosX2 <= regPosX1;
regPosY2 <= regPosY1;
end
end
// 4. 转 度..............................
reg [23:0] regX3_new;
reg [23:0] regY3_new;
reg [29:0] regA3_new;
reg [19:0] regPosX3; // 输出像素的位置
reg [19:0] regPosY3;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX3_new <= 24'd0;
regY3_new <= 24'd0;
regA3_new <= 30'd0;
regPosX3 <= 20'd0;
regPosY3 <= 20'd0;
end else if(ready_in) begin
if(regY2_new[23]) begin // 负数
regX3_new <= regX2_new - {{3{regY2_new[23]}},regY2_new[23:3]};
regY3_new <= regY2_new + {{3{regX2_new[23]}},regX2_new[23:3]};
regA3_new <= regA2_new - reg_Ang_list[3];
end else begin // 正数
regX3_new <= regX2_new + {{3{regY2_new[23]}},regY2_new[23:3]};
regY3_new <= regY2_new - {{3{regX2_new[23]}},regX2_new[23:3]};
regA3_new <= regA2_new + reg_Ang_list[3];
end
regPosX3 <= regPosX2;
regPosY3 <= regPosY2;
end
end
// 5. 转 度..............................
reg [23:0] regX4_new;
reg [23:0] regY4_new;
reg [29:0] regA4_new;
reg [19:0] regPosX4; // 输出像素的位置
reg [19:0] regPosY4;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX4_new <= 24'd0;
regY4_new <= 24'd0;
regA4_new <= 30'd0;
regPosX4 <= 20'd0;
regPosY4 <= 20'd0;
end else if(ready_in) begin
if(regY3_new[23]) begin // 负数
regX4_new <= regX3_new - {{4{regY3_new[23]}},regY3_new[23:4]};
regY4_new <= regY3_new + {{4{regX3_new[23]}},regX3_new[23:4]};
regA4_new <= regA3_new - reg_Ang_list[4];
end else begin // 正数
regX4_new <= regX3_new + {{4{regY3_new[23]}},regY3_new[23:4]};
regY4_new <= regY3_new - {{4{regX3_new[23]}},regX3_new[23:4]};
regA4_new <= regA3_new + reg_Ang_list[4];
end
regPosX4 <= regPosX3;
regPosY4 <= regPosY3;
end
end
// 6. 转 度..............................
reg [23:0] regX5_new;
reg [23:0] regY5_new;
reg [29:0] regA5_new;
reg [19:0] regPosX5; // 输出像素的位置
reg [19:0] regPosY5;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX5_new <= 24'd0;
regY5_new <= 24'd0;
regA5_new <= 30'd0;
regPosX5 <= 20'd0;
regPosY5 <= 20'd0;
end else if(ready_in) begin
if(regY4_new[23]) begin // 负数
regX5_new <= regX4_new - {{5{regY4_new[23]}},regY4_new[23:5]};
regY5_new <= regY4_new + {{5{regX4_new[23]}},regX4_new[23:5]};
regA5_new <= regA4_new - reg_Ang_list[5];
end else begin // 正数
regX5_new <= regX4_new + {{5{regY4_new[23]}},regY4_new[23:5]};
regY5_new <= regY4_new - {{5{regX4_new[23]}},regX4_new[23:5]};
regA5_new <= regA4_new + reg_Ang_list[5];
end
regPosX5 <= regPosX4;
regPosY5 <= regPosY4;
end
end
// 7. 转 度..............................
reg [23:0] regX6_new;
reg [23:0] regY6_new;
reg [29:0] regA6_new;
reg [19:0] regPosX6; // 输出像素的位置
reg [19:0] regPosY6;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX6_new <= 24'd0;
regY6_new <= 24'd0;
regA6_new <= 30'd0;
regPosX6 <= 20'd0;
regPosY6 <= 20'd0;
end else if(ready_in) begin
if(regY5_new[23]) begin // 负数
regX6_new <= regX5_new - {{6{regY5_new[23]}},regY5_new[23:6]};
regY6_new <= regY5_new + {{6{regX5_new[23]}},regX5_new[23:6]};
regA6_new <= regA5_new - reg_Ang_list[6];
end else begin // 正数
regX6_new <= regX5_new + {{6{regY5_new[23]}},regY5_new[23:6]};
regY6_new <= regY5_new - {{6{regX5_new[23]}},regX5_new[23:6]};
regA6_new <= regA5_new + reg_Ang_list[6];
end
regPosX6 <= regPosX5;
regPosY6 <= regPosY5;
end
end
// 8. 转 度..............................
reg [23:0] regX7_new;
reg [23:0] regY7_new;
reg [29:0] regA7_new;
reg [19:0] regPosX7; // 输出像素的位置
reg [19:0] regPosY7;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX7_new <= 24'd0;
regY7_new <= 24'd0;
regA7_new <= 30'd0;
regPosX7 <= 20'd0;
regPosY7 <= 20'd0;
end else if(ready_in) begin
if(regY6_new[23]) begin // 负数
regX7_new <= regX6_new - {{7{regY6_new[23]}},regY6_new[23:7]};
regY7_new <= regY6_new + {{7{regX6_new[23]}},regX6_new[23:7]};
regA7_new <= regA6_new - reg_Ang_list[7];
end else begin // 正数
regX7_new <= regX6_new + {{7{regY6_new[23]}},regY6_new[23:7]};
regY7_new <= regY6_new - {{7{regX6_new[23]}},regX6_new[23:7]};
regA7_new <= regA6_new + reg_Ang_list[7];
end
regPosX7 <= regPosX6;
regPosY7 <= regPosY6;
end
end
// 9
reg [23:0] regX8_new;
reg [23:0] regY8_new;
reg [29:0] regA8_new;
reg [19:0] regPosX8; // 输出像素的位置
reg [19:0] regPosY8;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX8_new <= 24'd0;
regY8_new <= 24'd0;
regA8_new <= 30'd0;
regPosX8 <= 20'd0;
regPosY8 <= 20'd0;
end else if(ready_in) begin
if(regY7_new[23]) begin // 负数
regX8_new <= regX7_new - {{8{regY7_new[23]}},regY7_new[23:8]};
regY8_new <= regY7_new + {{8{regX7_new[23]}},regX7_new[23:8]};
regA8_new <= regA7_new - reg_Ang_list[8];
end else begin // 正数
regX8_new <= regX7_new + {{8{regY7_new[23]}},regY7_new[23:8]};
regY8_new <= regY7_new - {{8{regX7_new[23]}},regX7_new[23:8]};
regA8_new <= regA7_new + reg_Ang_list[8];
end
regPosX8 <= regPosX7;
regPosY8 <= regPosY7;
end
end
// 10
reg [23:0] regX9_new;
reg [23:0] regY9_new;
reg [29:0] regA9_new;
reg [19:0] regPosX9; // 输出像素的位置
reg [19:0] regPosY9;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX9_new <= 24'd0;
regY9_new <= 24'd0;
regA9_new <= 30'd0;
regPosX9 <= 20'd0;
regPosY9 <= 20'd0;
end else if(ready_in) begin
if(regY8_new[23]) begin // 负数
regX9_new <= regX8_new - {{9{regY8_new[23]}},regY8_new[23:9]};
regY9_new <= regY8_new + {{9{regX8_new[23]}},regX8_new[23:9]};
regA9_new <= regA8_new - reg_Ang_list[9];
end else begin // 正数
regX9_new <= regX8_new + {{9{regY8_new[23]}},regY8_new[23:9]};
regY9_new <= regY8_new - {{9{regX8_new[23]}},regX8_new[23:9]};
regA9_new <= regA8_new + reg_Ang_list[9];
end
regPosX9 <= regPosX8;
regPosY9 <= regPosY8;
end
end
// 11
reg [23:0] regX10_new;
reg [23:0] regY10_new;
reg [29:0] regA10_new;
reg [19:0] regPosX10; // 输出像素的位置
reg [19:0] regPosY10;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX10_new <= 24'd0;
regY10_new <= 24'd0;
regA10_new <= 30'd0;
regPosX10 <= 20'd0;
regPosY10 <= 20'd0;
end else if(ready_in) begin
if(regY9_new[23]) begin // 负数
regX10_new <= regX9_new - {{10{regY9_new[23]}},regY9_new[23:10]};
regY10_new <= regY9_new + {{10{regY9_new[23]}},regX9_new[23:10]};
regA10_new <= regA9_new - reg_Ang_list[10];
end else begin // 正数
regX10_new <= regX9_new + {{10{regY9_new[23]}},regY9_new[23:10]};
regY10_new <= regY9_new - {{10{regX9_new[23]}},regX9_new[23:10]};
regA10_new <= regA9_new + reg_Ang_list[10];
end
regPosX10 <= regPosX9;
regPosY10 <= regPosY9;
end
end
// 12
reg [23:0] regX11_new;
reg [23:0] regY11_new;
reg [29:0] regA11_new;
reg [19:0] regPosX11; // 输出像素的位置
reg [19:0] regPosY11;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX11_new <= 24'd0;
regY11_new <= 24'd0;
regA11_new <= 30'd0;
regPosX11 <= 20'd0;
regPosY11 <= 20'd0;
end else if(ready_in) begin
if(regY10_new[23]) begin // 负数
regX11_new <= regX10_new - {{11{regY10_new[23]}},regY10_new[23:11]};
regY11_new <= regY10_new + {{11{regX10_new[23]}},regX10_new[23:11]};
regA11_new <= regA10_new - reg_Ang_list[11];
end else begin // 正数
regX11_new <= regX10_new + {{11{regY10_new[23]}},regY10_new[23:11]};
regY11_new <= regY10_new - {{11{regX10_new[23]}},regX10_new[23:11]};
regA11_new <= regA10_new + reg_Ang_list[11];
end
regPosX11 <= regPosX10;
regPosY11 <= regPosY10;
end
end
// 13
reg [23:0] regX12_new;
reg [23:0] regY12_new;
reg [29:0] regA12_new;
reg [19:0] regPosX12; // 输出像素的位置
reg [19:0] regPosY12;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
regX12_new <= 24'd0;
regY12_new <= 24'd0;
regA12_new <= 30'd0;
regPosX12 <= 20'd0;
regPosY12 <= 20'd0;
end else if(ready_in) begin
if(regY11_new[23]) begin // 负数
regX12_new <= regX11_new - {{12{regY11_new[23]}},regY11_new[23:12]};
regY12_new <= regY11_new + {{12{regX11_new[23]}},regX11_new[23:12]};
regA12_new <= regA11_new - reg_Ang_list[12];
end else begin // 正数
regX12_new <= regX11_new + {{12{regY11_new[23]}},regY11_new[23:12]};
regY12_new <= regY11_new - {{12{regX11_new[23]}},regX11_new[23:12]};
regA12_new <= regA11_new + reg_Ang_list[12];
end
regPosX12 <= regPosX11;
regPosY12 <= regPosY11;
end
end
wire [33:0] tmp_len;
assign tmp_len = regX12_new * 10'd622;
// 近似计算...
assign Ji_Len = tmp_len[33:10];
assign Ji_Ang = regA12_new;
assign PosX_out = regPosX12;
assign PosY_out = regPosY12;
endmodule
module cordic(
input wire clk,
input wire reset_n,
input wire [19:0] PosX_in, // 当前位置
input wire [19:0] PosY_in,
input wire ready_in,
output wire [19:0] PosX_out, // 输出像素的位置
output wire [19:0] PosY_out,
output wire [31:0] usPosX, // 输出像素的位置
output wire [31:0] usPosY,
output wire [23:0] Ji_Len_test, // 输出像素的位置
output wire [29:0] Ji_Ang_test
,output wire [23:0] test_data0
,output wire [23:0] test_data1
,output wire [23:0] test_data2
,output wire [23:0] test_data3
,output wire [23:0] test_data4
,output wire [23:0] test_data5
,output wire test_wire0
,output wire test_wire1
,output wire test_wire2
);
wire [19:0] PosX0; // 当前位置
wire [19:0] PosY0;
// assign PosX0 = PosX_in - 20'd256;
// assign PosY0 = PosY_in + 20'd86;
assign PosX0 = PosY_in + 20'd86;
assign PosY0 = PosX_in - 20'd256;
assign test_data0 = PosX_in;
assign test_data1 = PosY_in;
assign test_data2 = PosX_out;
assign test_data3 = PosY_out;
wire [23:0] PosX1; // 当前位置
wire [23:0] PosY1;
assign PosX1 = {PosX0,4'b0000};
assign PosY1 = {PosY0,4'b0000};
wire [23:0] Ji_Len; // 极坐标的长 *16
wire [29:0] Ji_Ang; // 极坐标的角度 *1024
cordic_core c1(.clk(clk),
.reset_n(reset_n),
.ready_in(ready_in),
.PosX_in(PosX_in),
.PosY_in(PosY_in),
.PosX_out(PosX_out),
.PosY_out(PosY_out),
.Zhi_PosX(PosX1),
.Zhi_PosY(PosY1),
.Ji_Len(Ji_Len),
.Ji_Ang(Ji_Ang)
);
assign Ji_Len_test = Ji_Len;
assign Ji_Ang_test = Ji_Ang;
wire [31:0] tmp_Len; // 极坐标的长 *16
wire [31:0] tmp_Ang; // 极坐标的角度 *1024
//assign tmp_Len = {Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len[23],Ji_Len};
assign tmp_Len = {{8{Ji_Len[23]}},Ji_Len};
// assign tmp_Ang = {Ji_Ang[29],Ji_Ang[29],Ji_Ang};
assign tmp_Ang = {{2{Ji_Ang[29]}},Ji_Ang};
// assign usPosX0 = tmp_Ang * 1365;
// assign usPosY0 = tmp_Len * 534;
reg [31:0] usPosX0;
reg [31:0] usPosY0;
always @(posedge clk or negedge reset_n)
begin
if(!reset_n) begin
usPosX0 <= 32'd0;
usPosY0 <= 32'd0;
end else if(ready_in) begin
// 0 2 4 6 8 10
//usPosX0 <= tmp_Ang * 1365;
usPosX0 <= tmp_Ang + {{2{tmp_Ang[31]}},tmp_Ang[31:2]}
+ {{4{tmp_Ang[31]}},tmp_Ang[31:4]}
+ {{6{tmp_Ang[31]}},tmp_Ang[31:6]}
+ {{8{tmp_Ang[31]}},tmp_Ang[31:8]}
+ {{10{tmp_Ang[31]}},tmp_Ang[31:10]};
//1 6 8 10 11 12 13 16 18 19
// usPosY0 <= tmp_Len * 534;
usPosY0 <= {{1{tmp_Len[31]}},tmp_Len[31:1]}
+ {{6{tmp_Len[31]}},tmp_Len[31:6]}
+ {{8{tmp_Len[31]}},tmp_Len[31:8]}
+ {{10{tmp_Len[31]}},tmp_Len[31:10]}/*
+ {{11{tmp_Len[31]}},tmp_Len[31:11]}
+ {{12{tmp_Len[31]}},tmp_Len[31:12]}
+ {{13{tmp_Len[31]}},tmp_Len[31:13]}*/;
end
end
wire [31:0] usPosX1;
wire [31:0] usPosY1;
assign usPosX1 = {{10{usPosX0[31]}},usPosX0[31:10]};
assign usPosY1 = {{ 4{usPosY0[31]}},usPosY0[31: 4]};
assign usPosX = usPosX1;
assign usPosY = usPosY1;
assign test_data4 = usPosX1;
assign test_data5 = usPosY1;
endmodule