quadz.v

quadz.v

module quadencoderz
    #(
         parameter BITS = 32,
         parameter QUAD_TYPE = 0,
         parameter CLK_FREQ = 27_000_000,
         parameter PPR = 400 
     )
     (
         input clk,
         input a,
         input b,
         input z,
         input indexenable,
         output reg indexout = 0,
         output signed [BITS-1:0] position,
         output reg signed [BITS-1:0] velocity_rps
     );
    reg [2:0] quadA_delayed = 0;
    reg [2:0] quadB_delayed = 0;
    reg [2:0] quadZ_delayed = 0;
    always @(posedge clk) quadA_delayed <= {quadA_delayed[1:0], a};
    always @(posedge clk) quadB_delayed <= {quadB_delayed[1:0], b};
    always @(posedge clk) quadZ_delayed <= {quadZ_delayed[1:0], z};

    

    wire count_enable = quadA_delayed[1] ^ quadA_delayed[2] ^ quadB_delayed[1] ^ quadB_delayed[2];
    wire count_direction = quadA_delayed[1] ^ quadB_delayed[2];
    reg signed [BITS-1:0] count = 0;
    reg indexwait = 0;
    assign position = $signed(count>>>QUAD_TYPE);
    reg prev_index;
    always @(posedge clk) begin
        if (indexenable == 1 && indexout == 1 && quadZ_delayed == 1) begin
            indexout <= 0;
            count <= 0;
            indexwait <= 1;
        end else begin
            if (indexenable == 1 && indexwait == 0 && indexout == 0 && quadZ_delayed == 0) begin
                indexout <= 1;
            end else if (indexenable == 0 && indexwait == 1) begin
                indexwait <= 0;
            end
            if (count_enable) begin
                if(count_direction) begin
                    count <= count + 1;
                end else begin
                    count <= count - 1;
                end
            end
        end

    end
    reg [31:0] prev_time; // Use a wider type for accurate timekeeping
    always @(posedge clk) begin
        // increment the timer
        prev_time <= prev_time + 1;
        // Choose RPS calculation method based on presence of index signal
        if (z) begin
          // Use index signal method (more accurate)
         
            // Calculate time elapsed since last revolution
            velocity_rps <= $signed((count << 2) * CLK_FREQ) / (4 * prev_time);

            // reset the timer
            prev_time <= 0;
                      
        end 
        //else begin
          // Use PPR method (less accurate)
          //velocity_rps <= $signed((count << 2) * CLK_FREQ * PPR) / (4 * prev_time);
        //end
        // Reset previous time on state change or overflow
        //if (curr_state !== prev_state || (count == ((1 << BITS) - 1))) begin
          //prev_time <= 0;
        //end
   
  end
endmodule