LSA Backup

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