SM Path Traversal 1.1

module LSA(
    output [7:0]m1,  //motor1                                            PIN_D3
     output [7:0]m1b,
    output [7:0]m2,  //motor2                                            PIN_C3
     output [7:0]m2b,
    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_1,    //50 MHz clock
    input reset,
     input [2:0]Clr,
     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 EM,
     output Clr_en
    );

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 [4:0]i = -5'd1;
reg [4:0]j = -5'd2;
reg flag = 0;

reg led1=0;
reg led2=0;
reg led3=0;

reg [7:0]odc =56;     //optimum duty cycle
reg [7:0]mo1 = 50;    // pwm to motor1, initially set to 50
reg [7:0]mo1r= 0;
reg [7:0]mo2 = 50;    // pwm to motor2, initially set to 50
reg [7:0]mo2r= 0;
reg [7:0]ml1 = 50;    // pwm to motor1 when it is on line
reg [7:0]ml1r=0;
reg [7:0]ml2 = 50;    // pwm to motor2 when it is on line
reg [7:0]ml2r=0;
reg [7:0]mn1 = 50;    // pwm to motor1 when it is on node
reg [7:0]mn1r= 0;
reg [7:0]mn2 = 50;    // pwm to motor2 when it is on node
reg [7:0]mn2r= 0;

reg [2:0] id1 =0;        //to control uart
reg hl=0;
reg n=0;

reg en=0;
reg [1:0]flag1=0; //0: arena, 1: pick, 2:place
reg [2:0]path_no = 0;  //no. of path
reg [3:0]path_len[5:0]; //length of each path


reg [1:0]path[36:0];
initial begin
path[0]<= 0;
path[1]<= 0;
path[2]<= 0;
path[3]<= 0;
path[4]<= 1;
path[5]<= 0;
path[6]<= 2;
path[7]<= 0;
end

reg clr_en = 1;  //disable clr detection
reg [1:0]clrsN=0;
reg [1:0]clrs[2:0];

reg [1:0]rcount=0;     //count no. of red color patches
reg [1:0]gcount=0;     //count no. of green color patches
reg [1:0]bcount=0;     //count no. of blue color patches

reg [1:0]clrPath[8:0][9:0];   //enter 9 S paths in sequence RGB
reg [2:0]clrPathlen[8:0]; //enter 9 S paths length in sequence RGB
initial
begin
    //configuration RRRGGGBNN
    clrPath[0][0] <= 0;
    clrPath[0][1] <= 1;
    clrPath[0][2] <= 3;
    clrPath[0][3] <= 2;
    clrPath[0][4] <= 0;

    clrPath[1][0] <= 0;
    clrPath[1][1] <= 0;
    clrPath[1][2] <= 1;
    clrPath[1][3] <= 3;
    clrPath[1][4] <= 2;
    clrPath[1][5] <= 0;
    clrPath[1][6] <= 0;

    clrPath[2][0] <= 0;
    clrPath[2][1] <= 0;
    clrPath[2][2] <= 0
    clrPath[2][3] <= 1;
    clrPath[2][4] <= 3;
    clrPath[2][5] <= 2;
    clrPath[2][6] <= 0;
    clrPath[2][7] <= 0;
    clrPath[2][8] <= 0;

    clrPath[3][0] <= 0;
    clrPath[3][1] <= 0;
    clrPath[3][2] <= 0;
    clrPath[3][3] <= 2;
    clrPath[3][4] <= 3;
    clrPath[3][5] <= 1;
    clrPath[3][6] <= 0;
    clrPath[3][7] <= 0;
    clrPath[3][8] <= 0;

    clrPath[4][0] <= 0;
    clrPath[4][1] <= 0;
    clrPath[4][2] <= 2;
    clrPath[4][3] <= 3;
    clrPath[4][4] <= 1;
    clrPath[4][5] <= 0;
    clrPath[4][6] <= 0;

    clrPath[5][0] <= 0;
    clrPath[5][1] <= 2;
    clrPath[5][2] <= 3;
    clrPath[5][3] <= 1;
    clrPath[5][4] <= 0;

    clrPath[6][0] <= 2;
    clrPath[6][1] <= 3;
    clrPath[6][2] <= 1;

    clrPathlen[0] <= 4;
    clrPathlen[1] <= 6;
    clrPathlen[2] <= 8;
    clrPathlen[3] <= 8;
    clrPathlen[4] <= 6;
    clrPathlen[5] <= 4;
    clrPathlen[6] <= 2;
    clrPathlen[7] <= 0;
    clrPathlen[8] <= 0;

end

reg [1:0]path[3:0][11:0];
reg [2:0]path_len[3:0];
initial
begin

    path[0][0] <= 0;
    path[0][1] <= 0;
    path[0][2] <= 0;
    path[0][3] <= 0;
    path[0][4] <= 2;
    path[0][5] <= 0;
    path[0][6] <= 1;
    path[0][7] <= 0;
    path[0][8] <= 1;
    path[0][9] <= 0;
    path[0][10] <= 0;

    path[1][0] <= 2;
    path[1][1] <= 0;
    path[1][2] <= 0;
    path[1][3] <= 0;
    path[1][4] <= 1;
    path[1][5] <= 0;
    path[1][6] <= 0;
    path[1][7] <= 1;
    path[1][8] <= 0;
    path[1][9] <= 2;
    path[1][10] <= 2;

    path[2][0] <= 1;
    path[2][1] <= 0;
    path[2][2] <= 1;
    path[2][3] <= 1;
    path[2][4] <= 0;
    path[2][5] <= 1;
    path[2][6] <= 0;
    path[2][7] <= 2;

    path[3][0] <= 1;
    path[3][1] <= 0;
    path[3][2] <= 1;
    path[3][3] <= 0;
    path[3][4] <= 1;
    path[3][5] <= 0;
    path[3][6] <= 1;
    path[3][7] <= 0;
    path[3][8] <= 2;
    path[3][9] <= 2;
    path[3][10] <= 0;
    path[3][11] <= 0;

    path_len[0] <= 10;
    path_len[1] <= 10;
    path_len[2] <= 7;
    path_len[3] <= 11;

end;

function automatic pwm;
    input [1:0]n;
    begin
        case(n)
        0:begin
            mn1 = odc;
            mn1r=0;
            mn2 = odc;
            mn2r=0;
          end
        1:begin
            mn1 = 5;
            mn1r=0;
            mn2 = 56;
            mn2r=0;
          end
        2:begin
            mn1 = 58;
            mn1r=0;
            mn2 = 5;
            mn2r=0;
          end
        3:begin
            mn1=0;
            mn1r=odc;
            mn2=odc;
            mn2r=0;
          end
        endcase
        pwm = 1;
     end
    endfunction

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

    if ((s1==0) && s2 && (s3==0)) //ready for next node, flag resets to zero on line : WBW
    begin
        flag <= 0;
          ml1r <= 0;
          ml2r <= 0;
    end
    /*if ((s1==0) && (s2==0) && (s3==0)&& (flag ==0))
    begin
        flag <= 0;
        n=pwm(3);
        ml1 <=0;
        ml1r<=odc;
        ml2 <=0;
        ml2r<=odc;
    end*/

    if (s1 && s2 && s3 && flag == 0)  //detect node
    begin
            id1 <= 4;
            hl <= 1;
            nodecount <= nodecount + 1;
         flag <= 1;
            i <= i+2;
            j <= j+2;
    end

    if(flag == 1)    //Detection of node and applying pwm accordingly
    begin
    //n<=pwm(path[nodecount]);
    if(flag1==0)
    begin
        n<=pwm(path[path_no][i:j]);
        if(nodecount==path_len[path_no])
        begin
            path_no <= path_no + 1;
            nodecount <= -5'd1;
            if(clrsN > 0)
            begin
                flag1 <= 1;
            end
        end
    end else
    if(flag1 == 1)
    begin
        if(clrs[clrsN] == 1)
        begin
        n<=pwm(clrPath[0+rcount][nodecount]);
            if(nodecount==clrPathlen[0+rcount])
            begin
                flag1 <= 2;
                nodecount <= -5'd1;
                rcount <= rcount + 1;
            end
        end
    end else
    if(


    /*if(path_no == && nodecount == )      //enable clr module only at patches
    begin
        clr_en <= 0;
    end else
    begin
        clr_en <= 1;
    end*/

    if(clr_en == 1 && Clr != 0)
    begin
        clrsN <= clrsN +1;
        clrs[clrsN] <= Clr;
    end

    case (nodecount)
    0: begin
        n<=pwm(0);
        end
    1: begin
        n<=pwm(0);
        end
    2: begin
        n<=pwm(0);
        end
    3: begin
        n<=pwm(0);
        end
    4: begin
        n<=pwm(1);
        end
    5: begin
        n<=pwm(0);
        end
    6: begin
        n<=pwm(2);
        end
    7: begin
        n<=pwm(0);
        end
    8: begin
        n<=pwm(2);
        end
    9: begin
        n<=pwm(2);
        end
    10:begin
        n<=pwm(0);
        end
    11:begin
        n<=pwm(0);
        end
    12:begin
        n<=pwm(0);
        end
    13:begin
        n<=pwm(2);
        end
    14:begin
        en <= 1;
        n<=pwm(3);
        id1 <= 5;
        hl <= 1;
        end
    15:begin
        n<=pwm(1);
        end
    16:begin
        n<=pwm(0);
        end
    17:begin
        n<=pwm(1);
        end
    18:begin
        n<=pwm(0);
        end
    19:begin
        n<=pwm(2);
        end
    20:begin
        n<=pwm(1);
        end
    21:begin
        en <= 0;
        n<=pwm(3);
        id1 <= 6;
        hl <= 1;
        end
    endcase
    end

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

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

assign id = id1;
assign HL1 = hl;
assign m1 = mo1;
assign m1b= mo1r;
assign m2 = mo2;
assign m2b= mo2r;

assign EM = en;
assign Clr_en = flag;
endmodule