LSA Hardcoded

module LSA(
    output [7:0]m1,  //motor1                                            PIN_D3
    output [7:0]m2,  //motor2                                            PIN_C3
    input s1,  //12-bit output of ch. 5 (parallel)
    input s2,  //12-bit output of ch. 6 (parallel)
    input s3,  //12-bit output of ch. 7 (parallel)
    input clk_50,    //50 MHz clock
    input reset,
     output Led1,    //Led used to indicate position of bot i.e. node or line
     output Led2,
     output Led3,
     output HL1,        // to control uart
     output [2:0] id,
     output EM1,     //Output for electromagnet
     output [4:0] NC
    );

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 [4:0]nodecount = -5'd1;     //No. of nodes bot has traversed, initially set to -1
//reg [3:0]flag=0;
reg flag = 0;

reg led1=0;
reg led2=0;
reg led3=0;
reg em;                 //to control electromagnet

reg [7:0]odc =50;     //optimum duty cycle
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 [7:0]ml1 = 50;    // pwm to motor1 when it is on line
reg [7:0]ml2 = 50;    // pwm to motor2 when it is on line
reg [7:0]mn1 = 50;    // pwm to motor1 when it is on node
reg [7:0]mn2 = 50;    // pwm to motor2 when it is on node
//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 [2:0] id1 =0;        //to control uart
reg hl;

/*reg [9:0]s1v1=0;
reg [9:0]s1v2=0;
reg [9:0]s1v3=0;
reg [9:0]s1v4=0;
reg [9:0]s1v5=0;
reg [9:0]s1v6=0;

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

reg [9:0]s3v1=0;
reg [9:0]s3v2=0;
reg [9:0]s3v3=0;
reg [9:0]s3v4=0;
reg [9:0]s3v5=0;
reg [9:0]s3v6=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


    if(reset == 0)
     begin
        nodecount <=-4'd1 ;         //Initially node count set to -1
     end

     led1 <= (s1);              //Led lights up when s1 has a greater value than 2000
     led2 <= (s2);              //Led lights up when s2 has a greater value than 2000
     led3 <= (s3);              //Led lights up when s3 has a greater value than 2000

    error = (s1) - (s3);    //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 > 10)      //Condition to reset the value of cumulative error to 10 if it crosses 10
    begin
        cumulative_error <= 10;
    end

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

     if ((s1==0) && s2 && (s3==0)) //Cumulative error resets to zero when the bot is on line i.e WBW
     begin
            cumulative_error <= 0;
            hl <=0;
            id1 <= 0;
     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


     ml1 <= odc - correction;  // PID tuning for motor 1
     ml2 <= odc + correction;  // PID tuning for motor 2

     if (ml1>70)      //Resetting value of ml1 to 70 if it crosses 70
     begin
         ml1 <= 70;
     end
     if (ml2>70)      //Resetting value of ml2 to 70 if it crosses 70
     begin
         ml2 <= 70;
     end
     if (ml1<30)       //Resetting value of ml1 to 30 if it becomes less than 30
     begin
         ml1 <= 30;
     end
     if (ml2<30)       //Resetting value of ml2 to 30 if it becomes less than 30
     begin
         ml2 <= 30;
     end


    /*  s1v1 <= s1v2;
      s1v2 <= s1v3;
      s1v3 <= s1v4;
      s1v4 <= s1v5;
      s1v5 <= s1v6;
      s1v6 <= s1[11:2];
      s1avg <= s1v1 + s1v2 + s1v3 + s1v4 + s1v5 + s1v6;

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

      s3v1 <= s3v2;
      s3v2 <= s3v3;
      s3v3 <= s3v4;
      s3v4 <= s3v5;
      s3v5 <= s3v6;
      s3v6 <= s3[11:2];
      s3avg <= s3v1 + s3v2 + s3v3 + s3v4 + s3v5 + s3v6; */

    /*if(s1>thr && s2>thr && s3>thr)
    begin
        flag = 1;
        mo1 <= 0;
        mo2 <= 63;
    end*/
    /*if(s1>thr && s2<thr && s3<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 ((s1==0) && (s2==1) && (s3==0)) //ready for next node, flag resets to zero on line : WBW
    begin
        flag <= 0;
    end
     /*else begin
        flag <= 1;
    end*/

    // Only detect node once.
    if (s1==1 && s2==1 && s3==1 && flag == 0)  //detect node
//  begin

/*        if (nodecount == 7)        // Nodecount resets to zero if it crosses 7
        begin
            id1 <= 4;
            hl <= 1;
            nodecount <= 0;
            flag <= 1;            //flag set to 1 when it detects a node
        end else   */

        begin
            id1 <= 4;
            hl <= 1;
            nodecount <= nodecount + 1;
         flag <= 1;
        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)
    1: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    2: begin
        mn1 <= odc;
        mn2 <= odc;
        end
    3: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    4: begin
        mn1 <= odc;
        mn2 <= odc;
        end
    5: begin
        mn1 <= 5;
      mn2 <= 60;
        end
    6: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    7: begin
        mn1 <= 60;
        mn2 <= 5;
        end
    8: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    9: begin
        mn1 <= 60;
        mn2 <= 5;
        end
    10:begin
        mn1 <= 60;
      mn2 <= 5;
        end
    11:begin
        mn1 <= odc;
        mn2 <= odc;
        end
    12:begin
        mn1 <= odc;
      mn2 <= odc;
        end
    13:begin
        mn1 <= odc;
      mn2 <= odc;
        end
    14: begin
        mn1 <= 60;
        mn2 <= 5;
        end
    15:begin
        em <= 1;
        mn1 <= 50;
      mn2 <= 50;
        end
    16:begin
        mn1 <= 5;
        mn2 <= 60;
        end
    17:begin
        mn1 <= odc;
      mn2 <= odc;
        end
    18:begin
        mn1 <= 5;
        mn2 <= 60;
        end
    19:begin
        mn1 <= odc;
      mn2 <= odc;
        end
    20:begin
        mn1 <= 60;
      mn2 <= 5;
        end
    21:begin
        mn1 <= 5;
        mn2 <= 60;
        end
    22:begin
        em <= 0;
        mn1 <= 0;
        mn2 <= 0;
        end
    0: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    endcase
    /*
        mn1 <= 5;
        mn2 <= 60;*/
    end

    if(flag == 0)    //assigning values of ml1 and ml2 (line condition) to mo1 and mo2 respectively
    begin
        mo1 <= ml1;
        mo2 <= ml2;
    end else
    begin            //assigning values of mn1 and mn2 (node condition) to mo1 and mo2 respectively
        mo1 <= mn1;
        mo2 <= mn2;
    end
end


assign Led1 = led1;
assign Led2 = led2;
assign Led3 = led3;

assign id = id1;
assign HL1 = hl;
assign m1 = mo1;
assign m2 = mo2;
assign EM1 = em;
assign NC = nodecount;

endmodule