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

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 led1=0;
reg led2=0;
reg led3=0;

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 && (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 && s2 && s3 && 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 <= 5;
		mn2 <= 60;
		end
	3: begin 
		mn1 <= odc;
      mn2 <= odc;
		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