diff --git a/Makefile b/Makefile index 474d54c..64d59df 100755 --- a/Makefile +++ b/Makefile @@ -45,6 +45,7 @@ RAPCOREFILES := boards/$(BOARD)/$(BOARD).v \ pwm.v \ quad_enc.v \ spi.v \ + crc4.v \ dda_fsm.v \ dual_hbridge.v \ dda_timer.v \ diff --git a/src/crc4.v b/src/crc4.v new file mode 100644 index 0000000..6e6fc37 --- /dev/null +++ b/src/crc4.v @@ -0,0 +1,42 @@ +//----------------------------------------------------------------------------- +// Copyright (C) 2009 OutputLogic.com +// This source file may be used and distributed without restriction +// provided that this copyright statement is not removed from the file +// and that any derivative work contains the original copyright notice +// and the associated disclaimer. +// +// THIS SOURCE FILE IS PROVIDED "AS IS" AND WITHOUT ANY EXPRESS +// OR IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED +// WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE. +//----------------------------------------------------------------------------- +// CRC module for data[63:0] , crc[4:0]=1+x^2+x^3+x^4+x^5; +//----------------------------------------------------------------------------- +module crc4( + input [63:0] data_in, + input crc_en, + output [4:0] crc_out, + input rst, + input clk); + + reg [4:0] lfsr_q,lfsr_c; + + assign crc_out = lfsr_q; + + always @(*) begin + lfsr_c[0] = lfsr_q[3] ^ lfsr_q[4] ^ data_in[0] ^ data_in[1] ^ data_in[4] ^ data_in[7] ^ data_in[8] ^ data_in[9] ^ data_in[10] ^ data_in[11] ^ data_in[13] ^ data_in[14] ^ data_in[15] ^ data_in[19] ^ data_in[21] ^ data_in[23] ^ data_in[24] ^ data_in[26] ^ data_in[31] ^ data_in[32] ^ data_in[35] ^ data_in[38] ^ data_in[39] ^ data_in[40] ^ data_in[41] ^ data_in[42] ^ data_in[44] ^ data_in[45] ^ data_in[46] ^ data_in[50] ^ data_in[52] ^ data_in[54] ^ data_in[55] ^ data_in[57] ^ data_in[62] ^ data_in[63]; + lfsr_c[1] = lfsr_q[4] ^ data_in[1] ^ data_in[2] ^ data_in[5] ^ data_in[8] ^ data_in[9] ^ data_in[10] ^ data_in[11] ^ data_in[12] ^ data_in[14] ^ data_in[15] ^ data_in[16] ^ data_in[20] ^ data_in[22] ^ data_in[24] ^ data_in[25] ^ data_in[27] ^ data_in[32] ^ data_in[33] ^ data_in[36] ^ data_in[39] ^ data_in[40] ^ data_in[41] ^ data_in[42] ^ data_in[43] ^ data_in[45] ^ data_in[46] ^ data_in[47] ^ data_in[51] ^ data_in[53] ^ data_in[55] ^ data_in[56] ^ data_in[58] ^ data_in[63]; + lfsr_c[2] = lfsr_q[0] ^ lfsr_q[3] ^ lfsr_q[4] ^ data_in[0] ^ data_in[1] ^ data_in[2] ^ data_in[3] ^ data_in[4] ^ data_in[6] ^ data_in[7] ^ data_in[8] ^ data_in[12] ^ data_in[14] ^ data_in[16] ^ data_in[17] ^ data_in[19] ^ data_in[24] ^ data_in[25] ^ data_in[28] ^ data_in[31] ^ data_in[32] ^ data_in[33] ^ data_in[34] ^ data_in[35] ^ data_in[37] ^ data_in[38] ^ data_in[39] ^ data_in[43] ^ data_in[45] ^ data_in[47] ^ data_in[48] ^ data_in[50] ^ data_in[55] ^ data_in[56] ^ data_in[59] ^ data_in[62] ^ data_in[63]; + lfsr_c[3] = lfsr_q[1] ^ lfsr_q[3] ^ data_in[0] ^ data_in[2] ^ data_in[3] ^ data_in[5] ^ data_in[10] ^ data_in[11] ^ data_in[14] ^ data_in[17] ^ data_in[18] ^ data_in[19] ^ data_in[20] ^ data_in[21] ^ data_in[23] ^ data_in[24] ^ data_in[25] ^ data_in[29] ^ data_in[31] ^ data_in[33] ^ data_in[34] ^ data_in[36] ^ data_in[41] ^ data_in[42] ^ data_in[45] ^ data_in[48] ^ data_in[49] ^ data_in[50] ^ data_in[51] ^ data_in[52] ^ data_in[54] ^ data_in[55] ^ data_in[56] ^ data_in[60] ^ data_in[62]; + lfsr_c[4] = lfsr_q[2] ^ lfsr_q[3] ^ data_in[0] ^ data_in[3] ^ data_in[6] ^ data_in[7] ^ data_in[8] ^ data_in[9] ^ data_in[10] ^ data_in[12] ^ data_in[13] ^ data_in[14] ^ data_in[18] ^ data_in[20] ^ data_in[22] ^ data_in[23] ^ data_in[25] ^ data_in[30] ^ data_in[31] ^ data_in[34] ^ data_in[37] ^ data_in[38] ^ data_in[39] ^ data_in[40] ^ data_in[41] ^ data_in[43] ^ data_in[44] ^ data_in[45] ^ data_in[49] ^ data_in[51] ^ data_in[53] ^ data_in[54] ^ data_in[56] ^ data_in[61] ^ data_in[62]; + + end // always + + always @(posedge clk) begin + if(~rst) begin + lfsr_q <= {5{1'b1}}; + end + else begin + lfsr_q <= crc_en ? lfsr_c : lfsr_q; + end + end // always +endmodule // crc4 \ No newline at end of file diff --git a/src/quad_enc.v b/src/quad_enc.v index 72d3b2e..a74d734 100755 --- a/src/quad_enc.v +++ b/src/quad_enc.v @@ -2,14 +2,17 @@ `default_nettype none module quad_enc #( - parameter encbits = 64 + parameter encbits = 64, + parameter enable_velocity = 1, + parameter velocity_bits = 32 )( input wire resetn, input wire clk, input wire a, input wire b, output reg faultn, - output reg signed [encbits-1:0] count + output wire signed [encbits-1:0] count, + output wire signed [velocity_bits-1:0] velocity //input [7:0] multiplier ); @@ -21,11 +24,17 @@ module quad_enc #( wire step_b = b_stable[1] ^ b_stable[2]; //Step if b changed wire step = step_a ^ step_b; //Step if a xor b stepped wire direction = a_stable[1] ^ b_stable[2]; //Direction determined by comparing current sample to last - wire signed increment = (direction) ? 1 : -1; + + reg signed [encbits-1:0] count_r; + assign count = count_r; + + reg signed [velocity_bits-1:0] velocity_counter; + reg signed [velocity_bits-1:0] velocity_r; + assign velocity = velocity_r; always @(posedge clk) begin if (!resetn) begin - count <= 0; //reset count + count_r <= 0; //reset count faultn <= 1'b1; //reset faultn a_stable <= 3'b0; b_stable <= 3'b0; @@ -33,12 +42,21 @@ module quad_enc #( else begin a_stable <= {a_stable[1:0], a}; //Shift new a in. Last 2 samples shift to bits 2 and 1 b_stable <= {b_stable[1:0], b}; //Shift new b in + velocity_counter <= velocity_counter + 1'b1; if (step_a & step_b) //We do not know direction if both inputs triggered on single clock faultn <= 0; - /* verilator lint_off WIDTH */ - if (step) count <= count + increment; - /* verilator lint_on WIDTH */ + if (step) begin + if (direction) begin + count_r <= count_r + 1'b1; + velocity_r <= velocity_counter; + end + else begin + count_r <= count_r - 1'b1; + velocity_r <= -velocity_counter; + end + velocity_counter <= 0; + end end end endmodule diff --git a/src/spi_state_machine.v b/src/spi_state_machine.v index cb846e0..204c7c2 100644 --- a/src/spi_state_machine.v +++ b/src/spi_state_machine.v @@ -8,6 +8,7 @@ module spi_state_machine #( parameter dda_bits = 64, parameter move_duration_bits = 32, parameter encoder_bits = 32, + parameter encoder_velocity_bits = 32, parameter default_microsteps = 1, parameter default_current = 140, parameter BUFFER_SIZE = 2, @@ -77,17 +78,19 @@ module spi_state_machine #( ); localparam CMD_COORDINATED_STEP = 8'h01; + localparam CMD_READ_ENCODER = 8'h03; localparam CMD_MOTOR_ENABLE = 8'h0a; localparam CMD_MOTOR_BRAKE = 8'h0b; localparam CMD_MOTORCONFIG = 8'h10; localparam CMD_CLK_DIVISOR = 8'h20; localparam CMD_MICROSTEPPER_CONFIG = 8'h30; localparam CMD_COSINE_CONFIG = 8'h40; - localparam CMD_API_VERSION = 8'hfe; localparam CMD_CHARGEPUMP = 8'h31; localparam CMD_BRIDGEINVERT = 8'h32; localparam CMD_STEPPERFAULT = 8'h50; localparam CMD_ENCODERFAULT = 8'h51; + localparam CMD_CHANNEL_INFO = 8'hfd; + localparam CMD_API_VERSION = 8'hfe; localparam MOVE_BUFFER_SIZE = BUFFER_SIZE - 1; //This is the zero-indexed end index localparam MOVE_BUFFER_BITS = $clog2(BUFFER_SIZE) - 1; // number of bits to index given size @@ -253,18 +256,21 @@ module spi_state_machine #( // wire signed [encoder_bits-1:0] encoder_count [num_encoders-1:0]; - wire [num_encoders-1:0] encoder_fault; + wire [num_encoders-1:0] encoder_faultn; + wire [31:0] encoder_velocity [num_encoders-1:0]; if(num_encoders > 0) begin for (i=0; i