SM Path+Dep 1.2

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 flag = 0;

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

reg [7:0]odc =58;     //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]state = 0; //0: arena, 1: pick, 2:place
reg [1:0]path_no = 0;  //no. of path
reg [1:0]path[3:0][11:0];
reg [3:0]path_len[3:0]; //length of each path
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
reg [1:0]destPath[8:0][12:0];
reg [3:0]destPathlen[8:0];
reg [1:0]DepPath[8:0][10:0];
reg [2:0]DepPath_len[8:0];


reg clr_en = 1;  //disable clr detection
reg [1:0]clrsN=0;
reg [2:0]clrs[0:2];
reg [4:0]dest[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 flag2=0;

reg done = 0;
    //Arena

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

always @(posedge clk_1) begin
if(done == 0)
begin
    state <=0;

    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;



    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][1] <= 2;
    clrPath[6][2] <= 3;
    clrPath[6][3] <= 1;

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


    DepPath [0][0] <= 2;
    DepPath [0][1] <= 0;
    DepPath [0][2] <= 1;
    DepPath [0][3] <= 1;
    DepPath [0][4] <= 3;
    DepPath [0][5] <= 2;
    DepPath [0][6] <= 2;
    DepPath [0][7] <= 0;
    DepPath [0][8] <= 1;
    //for DZP2
    DepPath [1][0] <= 2;
    DepPath [1][1] <= 1;
    DepPath [1][2] <= 3;
    DepPath [1][3] <= 2;
    DepPath [1][4] <= 1;
    //for DZM1
    DepPath [2][0] <= 1;
    DepPath [2][1] <= 1;
    DepPath [2][2] <= 2;
    DepPath [2][3] <= 3;
    DepPath [2][4] <= 1;
    DepPath [2][5] <= 2;
    DepPath [2][6] <= 2;
    //for DZM2
    DepPath [3][0] <= 1;
    DepPath [3][1] <= 1;
    DepPath [3][2] <= 0;
    DepPath [3][3] <= 2;
    DepPath [3][4] <= 2;
    DepPath [3][5] <= 3;
    DepPath [3][6] <= 1;
    DepPath [3][7] <= 0;
    DepPath [3][8] <= 1; //This shall place the bot at start
    //for DZM3
    DepPath [4][0] <= 1;
    DepPath [4][1] <= 0;
    DepPath [4][2] <= 0;
    DepPath [4][3] <= 0;
    DepPath [4][4] <= 1;
    DepPath [4][5] <= 1;
    DepPath [4][6] <= 3;
    DepPath [4][7] <= 1;
    DepPath [4][8] <= 0;
    DepPath [4][9] <= 0;
    DepPath [4][10] <= 1; //This shall place the bot at start
    //for DZV2
    DepPath [5][0] <= 1;
    DepPath [5][1] <= 2;
    DepPath [5][2] <= 3;
    DepPath [5][3] <= 1;
    DepPath [5][4] <= 2;
    //for DZV1
    DepPath [6][0] <= 0;
    DepPath [6][1] <= 1;
    DepPath [6][2] <= 3;
    DepPath [6][3] <= 2;
    DepPath [6][4] <= 0;
    //for DZN1
    DepPath [7][0] <= 1;
    DepPath [7][1] <= 0;
    DepPath [7][2] <= 2;
    DepPath [7][3] <= 1;
    DepPath [7][4] <= 3;
    DepPath [7][5] <= 2;
    DepPath [7][6] <= 1;
    DepPath [7][7] <= 0;
    DepPath [7][8] <= 2;
    //for DZN2
    DepPath [8][0] <= 1;
    DepPath [8][1] <= 0;
    DepPath [8][2] <= 0;
    DepPath [8][3] <= 2;
    DepPath [8][4] <= 3;
    DepPath [8][5] <= 1;
    DepPath [8][6] <= 0;
    DepPath [8][7] <= 0;
    DepPath [8][8] <= 2;

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

     done <= 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 && s2 && s3 && flag == 0)  //detect node
    begin
            id1 <= 4;
            hl <= 1;
            nodecount <= nodecount + 1;
         flag <= 1;   //node
            flag2 <= 1;  //clr lock
            clr_en <= 1; //color enable 1 disable
    end

    if(flag == 1)                                  //if node
    begin
        if(state==0)                                //is arena?
        begin
            n<=pwm(path[path_no][nodecount]);         //follow arena
        end else
        if((state==1)&&(nodecount<10))                                //is pick?
        begin
            if((clrs[clrsN-1][0]==1)&&(clrs[clrsN-1][1]==0))
            begin
                n<=pwm(clrPath[0+rcount][nodecount]);
            end else if((clrs[clrsN-1][0]==0)&&(clrs[clrsN-1][1]==1))
            begin
                n<=pwm(clrPath[3+gcount][nodecount]);
            end else if((clrs[clrsN-1][0]==1)&&(clrs[clrsN-1][1]==1))
            begin
                n<=pwm(clrPath[6+bcount][nodecount]);
            end
        end
     if (state== 2)
     begin
        n<=pwm(DepPath[path_no][nodecount]);
     end

    end

    if(flag == 0)    //assigning values of ml1 and ml2 (line condition) to mo1 and mo2 respectively
    begin
        if(path_no ==0 && nodecount ==5 )        //enable clr module at patch1
            begin
                clr_en <= 0;
                if((Clr != 0)&&(flag2==1))
                begin
                    clrsN <= clrsN +1;
                    clrs[clrsN] <= Clr;
                    dest[clrsN] <= 0;
                    flag2 <= 0;
                end
        end else if(path_no ==0 && nodecount ==7 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==1 && nodecount ==5 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==1 && nodecount ==6 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==1 && nodecount ==7 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==2 && nodecount ==1 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==2 && nodecount ==6 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==3 && nodecount ==5 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else if(path_no ==3 && nodecount ==7 )      //enable clr module only at patches
        begin
            clr_en <= 0;
            if((Clr != 0)&&(flag2==1))
            begin
                clrsN <= clrsN +1;
                clrs[clrsN] <= Clr;
                dest[clrsN] <= 0;
                flag2 <= 0;
            end
        end else
        begin
            clr_en <= 1;
        end
        if((state==0)&&(nodecount == (path_len[path_no]-1)))       //if about to reach end of path
        begin
            if(clrsN != 0)                           //detected color?
            begin
                state <= 1;                             //go to pick
            end else if(clrsN == 0)
            begin
                nodecount <= -5'd1;                      //reset nodecount
                path_no <= path_no + 1;                  //change path
            end
        end
        if(state==1)
        begin
            if(nodecount == path_len[path_no])
            begin
            state <= 2;
            nodecount <= -5'd1;
            end
            nodecount <= nodecount+1;
        end
        if(state==2)
        begin
            if(nodecount == DepPath_len[path_no])
            begin
            state <= 0;
            nodecount <= -5'd1;
            end
            nodecount <= nodecount+1;
        end

        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 = clr_en;
endmodule