module servo_cntr (
    input clk, reset_p,
    input mode_servo, start_servo,
    output servo,
    output [15:0] led
    );

    integer cnt_sysclk;
    always @(posedge clk) cnt_sysclk = cnt_sysclk + 1;
    
    wire cnt_sysclk_pedge;
    edge_detector_pos sysclk_edge (clk, reset_p, cnt_sysclk[21], cnt_sysclk_pedge);

    reg [7:0] step;
    // assign led[0] = mode_servo;
    // assign led[1] = start_servo;
    // assign led[9:2] = step;
    always @(posedge clk or posedge reset_p) begin
        if(reset_p) begin
            step <= 20;
        end
        else begin
            if (cnt_sysclk_pedge) begin
                if (start_servo) begin
                    if (step <= 41) step <= 40;
                    else step <= step - 1;
                end
                else begin
                    if (step >= 149) step <= 150;
                    else step <= step + 1;
                end
            end
        end
        // else if (!mode_servo) begin
        //     if (btn_pedge[1]) begin
        //         step <= step - 1;
        //     end
        //     else if (btn_pedge[2]) begin
        //         step <= step + 1;
        //     end
        // end
    end

    pwm_Nstep #(.pwm_freq(50), .duty_step_N(1440))
        pwm_servo (clk, reset_p, step, servo);
endmodule