Dijkstra+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 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
    );

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 [6:0]k, l, adj, near, parnode;

reg [11:0] dist [0:36];
reg [6:0] parent [0:36];
reg visited[0:36];

integer length=0;    // length of path
reg [6:0] sp[0:36];     // Store path
reg [1:0] p_dc[0:36];       //Stores The turns


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

parameter V = 37;

reg [7:0] i,j;

reg [11:0] cost [0:36][0:36];
reg [11:0] cost2 [0:36][0:36];

// Function to find out the shortest distance
function automatic [6:0]minNode;
    input [6:0]a;
    reg [6:0]minValue = 999;
   reg [6:0]minNode1 = 0;
    begin
        for (k = 0; k < V; k=k+1)
        begin
        if (dist[k] < minValue && visited[k] == 0)
        begin
            minValue = dist[k];
            minNode1 = k;
                //$display("Enter min");
        end
        end
    minNode = minNode1;
     //$display("Function Called %6d", minNode1);
    end
endfunction
//


//Storing Values of the cost matrix
initial begin
    for(i=0;i<V;i=i+1)
begin
    for(j=0;j<V;j=j+1) begin

        if(i==j) begin
                cost2[i][j] = 0;
                //cost[i][j] = 0;
          end
        else begin
                cost2[i][j] = 999;
                //cost[i][j] = 999;
          end
    end
end
cost2[0][1] = 3;
cost2[0][2] = 2;
cost2[0][8] = 1;

cost2[1][0] = 4;

cost2[2][0] = 1;
cost2[2][3] = 2;
cost2[2][4] = 3;

cost2[3][2] = 1;
cost2[3][6] = 3;
cost2[3][17] = 2;

cost2[4][2] = 4;
cost2[4][5] = 2;
cost2[4][11] = 3;

cost2[5][4] = 1;

cost2[6][3] = 4;
cost2[6][7] = 2;
cost2[6][14] = 3;

cost2[7][6] = 1;

cost2[8][0] = 4;
cost2[8][10] = 2;
cost2[8][33] = 3;

cost2[9][10] = 3;

cost2[10][8] = 1;
cost2[10][9] = 4;
cost2[10][11] = 2;

cost2[11][4] = 4;
cost2[11][10] = 1;
cost2[11][12] = 2;
cost2[11][18] = 3;

cost2[12][11] = 1;        //M
cost2[12][13] = 4;
cost2[12][14] = 2;
cost2[12][23] = 3;

cost2[13][12] = 3;

cost2[14][6] = 4;        //O
cost2[14][12] = 1;
cost2[14][15] = 2;

cost2[15][14] = 1;
cost2[15][16] = 4;
cost2[15][17] = 2;

cost2[16][15] = 3;       //Q

cost2[17][15] = 1;
cost2[17][3] = 4;
cost2[17][26] = 3;

cost2[18][19] = 2;
cost2[18][11] = 4;
cost2[18][21] = 3;       //S

cost2[19][18] = 1;

cost2[20][21] = 2;       //U

cost2[21][20] = 1;
cost2[21][22] = 2;
cost2[21][28] = 3;
cost2[21][18] = 4;

cost2[22][21] = 1;       //W

cost2[23][24] = 2;
cost2[23][12] = 4;
cost2[23][34] = 3;

cost2[24][22] = 1;

cost2[25][26] = 2;       //Z

cost2[26][25] = 1;
cost2[26][17] = 4;
cost2[26][34] = 3;       //AA

cost2[27][28] = 2;

cost2[28][27] = 1;
cost2[28][21] = 4;
cost2[28][29] = 2;
cost2[28][31] = 3;

cost2[29][28] = 1;       //AD

cost2[30][31] = 2;

cost2[31][30] = 1;       //AF
cost2[31][28] = 4;
cost2[31][32] = 2;
cost2[31][33] = 3;

cost2[32][31] = 1;       //AG

cost2[33][31] = 4;
cost2[33][8] = 1;
cost2[33][34] = 2;

cost2[34][35] = 2;
cost2[34][23] = 4;
cost2[34][33] = 1;

cost2[35][36] = 4;
cost2[35][26] = 2;
cost2[35][34] = 1;

cost[36][35] = 3;
end

initial begin
for(i=0;i<V;i=i+1)
begin
    for(j=0;j<V;j=j+1) begin

        if(i==j) begin
                //cost2[i][j] = 0;
                cost[i][j] = 0;
          end
        else begin
                //cost2[i][j] = 999;
                cost[i][j] = 999;
          end
    end
end
cost[0][1] = 100;               //A
cost[0][2] = 2;
cost[0][8] = 3;

cost[1][0] = 100;               //B

cost[2][0] = 2;                 //C
cost[2][3] = 3;
cost[2][4] = 2;

cost[3][2] = 3;                 //D
cost[3][6] = 2;
cost[3][17] = 4;

cost[4][2] = 2;         //E
cost[4][5] = 100;
cost[4][11] = 2;

cost[5][4] = 100;       //F

cost[6][3] = 2;         //G
cost[6][7] = 100;
cost[6][14] = 3;

cost[7][6] = 100;       //H

cost[8][0] = 3;         //I
cost[8][10] = 2;
cost[8][33] = 4;

cost[9][10] = 100;      //J

cost[10][8] = 2;        //K
cost[10][9] = 100;
cost[10][11] = 1;

cost[11][4] = 2;        //L
cost[11][10] = 1;
cost[11][12] = 3;
cost[11][18] = 1;

cost[12][11] = 3;           //M
cost[12][13] = 100;
cost[12][14] = 1;
cost[12][23] = 2;

cost[13][12] = 100;     //N

cost[14][6] = 3;            //O
cost[14][12] = 1;
cost[14][15] = 2;

cost[15][14] = 2;       //P
cost[15][16] = 100;
cost[15][17] = 1;

cost[16][15] = 100;         //Q

cost[17][15] = 1;       //R
cost[17][3] = 4;
cost[17][26] = 2;

cost[18][19] = 100;     //S
cost[18][11] = 1;
cost[18][21] = 1;

cost[19][18] = 100;     //T

cost[20][21] = 100;         //U

cost[21][20] = 100;     //V
cost[21][22] = 100;
cost[21][28] = 1;
cost[21][18] = 1;

cost[22][21] = 100;             //W

cost[23][24] = 100;     //X
cost[23][12] = 2;
cost[23][34] = 3;

cost[24][23] = 100;     //Y

cost[25][26] = 100;             //Z

cost[26][25] = 100;     //AA
cost[26][17] = 2;
cost[26][34] = 4;

cost[27][28] = 100;     //AB

cost[28][27] = 100;     //AC
cost[28][21] = 1;
cost[28][29] = 100;
cost[28][31] = 1;

cost[29][28] = 100;         //AD

cost[30][31] = 100;     //AE

cost[31][30] = 100;             //AF
cost[31][28] = 1;
cost[31][32] = 100;
cost[31][33] = 1;

cost[32][31] = 100;             //AG

cost[33][31] = 1;       //AH
cost[33][8] = 4;
cost[33][34] = 3;

cost[34][35] = 2;       //AI
cost[34][23] = 3;
cost[34][33] = 3;

cost[35][36] = 100;     //AJ
cost[35][26] = 4;
cost[35][34] = 2;

cost[36][35] = 100;     //AK

end

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;


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


//dijkstra function
function automatic dijkstra;
input [6:0] start;
input [6:0] enda;
begin
     $display("Enter dijkstra");
    for (i = 0; i < V; i=i+1)
    begin
        dist[i] = 999;
        parent[i] = i;
          visited[i] = 0;
          sp[i] = 0;
    end
    dist[start] = 0;
        for (i = 0; i < V; i=i+1)
        begin
        near = minNode(1);
        visited[near] = 1;

        for (adj = 0; adj < V; adj=adj+1)
        begin
            if (cost[near][adj] != 0 && dist[adj] > dist[near] + cost[near][adj])
            begin
                dist[adj] = dist[near] + cost[near][adj];
                parent[adj] = near;
            end
        end
        end
      parnode = parent[enda];
      sp[length] = enda;
      for(i=0;i<V;i=i+1)
           begin
                if(parnode != start) begin
               //$display(" <- %6d || %6d",parnode,parent[parnode]);
                length = length + 1;
                sp[length] = parnode;
                parnode = parent[parnode];
                    end
            end
        length= length + 1;
        sp[length] = start;
      //$display("%6d", length);
        //direction
        i=0;
        for(j=35;j>0;j=j-1) begin
            //$display("%6d", sp[j]);

            if(j<length && j>0) begin
                $display("%6d", sp[j]);
                if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 1)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) &&(cost2[sp[j]][sp[j-1]] == 4)))
                    begin //$display("Straight");
                        p_dc[i]=0;
                        i=i+1;
                    end
                if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 4)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 1)))
                    begin //$display("Left");
                        p_dc[i]=1;
                        i=i+1;
                    end
                if(((cost2[sp[j]][sp[j+1]]==1) && (cost2[sp[j]][sp[j-1]] == 4)) || ((cost2[sp[j]][sp[j+1]]==4) && (cost2[sp[j]][sp[j-1]] == 2)) || ((cost2[sp[j]][sp[j+1]]==2) && (cost2[sp[j]][sp[j-1]] == 3)) || ((cost2[sp[j]][sp[j+1]]==3) && (cost2[sp[j]][sp[j-1]] == 1)))
                    begin //$display("Right");
                        p_dc[i]=2;
                        i=i+1;
                    end

            end


        end

    dijkstra = 1;
end
endfunction
//

reg flag_dijkstra = 0;
always @(posedge clk_1) begin

//Call dijkstra
    if(flag_dijkstra == 0) begin
        flag_dijkstra = dijkstra(33, 7);
        i=-1;
        end

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

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

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

          hl = 1;
          id1 = 4;
          flag <= 1;

          //nodecount = p_dc[i];
          i=i+1;
    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

    if(p_dc[i] == 0)
    begin
        mn1 <= odc;
      mn2 <= odc;
    end
    if(p_dc[i] == 1)
    begin
        mn1 <= 5;
      mn2 <= 60;
    end
    if(p_dc[i] == 2)
    begin
        mn1 <= 60;
      mn2 <= 5;
    end
    /*case (nodecount)
    0: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    1: begin
        mn1 <= 5;
        mn2 <= 60;
        end
    2: begin
        mn1 <= 60;
      mn2 <= 5;
        end
    4: begin
        mn1 <= 5;
        mn2 <= 60;
        end
    5: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    6: begin
        mn1 <= odc;
      mn2 <= odc;
        end
    7: begin
        mn1 <= 5;
        mn2 <= 60;
        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;

endmodule