Documented LSA

module LSA(
    output [7:0]m1,    //motor1                                            PIN_D3
    output [7:0]m2,    //motor2                                            PIN_C3
    input [11:0]s1,    //12-bit output of ch. 5 (parallel)
    input [11:0]s2,    //12-bit output of ch. 6 (parallel)
    input [11:0]s3,    /12-bit output of ch. 7 (parallel)
    input clk_50,      //50 MHz clock
    input reset,
     output HL,
     output [2:0] Id
    );
reg signed[7:0]error = 0;
reg signed[7:0]difference = 0;
reg signed[7:0]correction = 0;
reg signed[7:0]cumulative_error = 0;
reg signed[7:0]preverror = 0;
reg [3:0]nodecount = -4'd1;         //No. of nodes bot has traversed, initially set to -1
//reg [3:0]flag=0;
reg flag = 0;

reg hl;
reg [2:0] id;

reg [7:0]odc =50;           //No. of nodes bot has traversed, initially set to -1
reg [7:0]mo1 = 50;      // pwm to motor1, initially set to 50
reg [7:0]mo2 = 50;      // pwm to motor2, initially set to 50
reg [11:0]thr = 2000;   //Condition to differentiate between white and black surface
reg [7:0]r_COUNT = 0;   //counter
reg clk_1 = 0;              //1 MHz Clock

reg [9:0]s1v1=0;
reg [9:0]s1v2=0;
reg [9:0]s1v3=0;
reg [9:0]s1v4=0;

reg [9:0]s2v1=0;
reg [9:0]s2v2=0;
reg [9:0]s2v3=0;
reg [9:0]s2v4=0;

reg [9:0]s3v1=0;
reg [9:0]s3v2=0;
reg [9:0]s3v3=0;
reg [9:0]s3v4=0;

reg [11:0]s1avg=0;
reg [11:0]s2avg=0;
reg [11:0]s3avg=0;

always @(posedge clk_50) begin          //Scaling down of 50MHz clock to 1 MHz


    if(reset == 0)
     begin
        r_COUNT <= 0;
     end

   if (r_COUNT == 49 )begin            //Check if count reaches 49
      r_COUNT <= 0;                    //Reset counter if it reaches 49
   end else begin
      r_COUNT <= r_COUNT + 1;          //increment counter by 1 till 49
   end
    clk_1 <= (r_COUNT < 25);
end

always @(posedge clk_1) begin

    error = (s1>thr) - (s3>thr);                           //Relative error between sensors s1 and s3: values range from -1 to 1  P

    cumulative_error <= cumulative_error + error;       //Adds up the error to give a cumulative error  I

    if (cumulative_error > 5)                                   //Condition to reset the value of cumulative error to 5 if it crosses 5
    begin
        cumulative_error <= 5;
    end

     if (cumulative_error < -5)                           //Condition to reset the value of cumulative error to -5 if it crosses -5
    begin
        cumulative_error <= -5;
    end
     difference <= error-preverror;                         //forms the differential part D
     correction <= ((10*error) + cumulative_error + (2*difference));        // kp = 10, ki = 1, kd =2

      preverror <= error;                                        // Stores value of current error to previous error so that it can be used in next loop cycle

     mo1 <= odc - correction;                                   // PID tuning for motor 1
     mo2 <= odc + correction;                                   // PID tuning for motor 2

     if (mo1>90)                                                    //Resetting value of ml1 to 90 if it crosses 90
     begin
         mo1 <= 90;
     end
     if (mo2>90)                                                    //Resetting value of ml2 to 90 if it crosses 90
     begin
         mo2 <= 90;
     end
     if (mo1<30 && flag == 1)                                   //Resetting value of ml1 to 30 if it becomes less than 30
     begin
         mo1 <= 30;
     end
     if (mo2<30)                                                    //Resetting value of ml2 to 30 if it becomes less than 30
     begin
         mo2 <= 30;
     end


      s1v1 <= s1v2;
      s1v2 <= s1v3;
      s1v3 <= s1v4;
      s1v4 <= s1[11:2];
      s1avg <= s1v1 + s1v2 + s1v3 + s1v4;

      s2v1 <= s2v2;
      s2v2 <= s2v3;
      s2v3 <= s2v4;
      s2v4 <= s2[11:2];
      s2avg <= s2v1 + s2v2 + s2v3 + s2v4;

      s3v1 <= s3v2;
      s3v2 <= s3v3;
      s3v3 <= s3v4;
      s3v4 <= s3[11:2];
      s3avg <= s3v1 + s3v2 + s3v3 + s3v4;

    /*if(s1>thr && s2>thr && s3>thr)
    begin
        flag = 1;
        mo1 <= 0;
        mo2 <= 63;
    end*/
    if(s1avg>thr && s2avg<thr && s3avg<thr && flag == 1)    //end turning
    begin
        flag <= 0;
        mo1 <= odc - correction;
      mo2 <= odc + correction;
    end
    /*if(s1<thr && s2>thr && s3<thr && flag == 1)
    begin
        flag <= 0;
        mo1 <= odc - correction;
      mo2 <= odc + correction;
    end*/
    if (s1avg < thr && s2avg > thr && s3avg < thr)              //ready for next node
    begin
        flag <= 0;
    end
     /*else begin
        flag <= 1;
    end*/

    // Only detect node once.
    if (s1avg>thr && s2avg>thr && s3avg>thr && flag == 0)    //detect node
    begin
        if (nodecount == 7)
        begin
            nodecount <= 0;
            flag <= 1;
            hl=~hl;
            id=0;
        end else
        begin
            nodecount <= nodecount + 1;
         flag <= 1;
            hl=~hl;
            id=0;
        end
    end

    /*if (s1>thr && s2>thr && s3>thr)
        begin
            if(flag == 3)
            begin
                if (nodecount == 7)
                begin
                    nodecount <= 0;
                    flag <= 0;
                    flag2 <= 1;
                end else
                begin
                    nodecount <= nodecount + 1;
                    flag <=0;
                    flag2 <= 1;
                end
            end else
            begin
                flag <= flag + 1;
            end
        end else
            begin
                flag <= 0;
            end*/

    if(flag == 1)                                               //Detection of node and applying pwm accordingly
    begin
    case (nodecount)
    0: begin
        mo1 <= odc;
      mo2 <= odc;
        end
    1: begin
        mo1 <= 5;
        mo2 <= 60;
        end
    2: begin
        mo1 <= odc;
      mo2 <= odc;
        end
    3: begin
        mo1 <= 5;
        mo2 <= 60;
        end
    4: begin
        mo1 <= odc;
      mo2 <= odc;
        end
    5: begin
        mo1 <= odc;
      mo2 <= odc;
        end
    6: begin
        mo1 <= 5;
        mo2 <= 60;
        end
    7: begin
        mo1 <= odc;
      mo2 <= odc;
        end
    endcase
    end
end




assign m1 = mo1;
assign m2 = mo2;

assign HL = hl;
assign Id = id;

endmodule