// Unit name to specify here - UNIT1, UNIT2, UNIT3 or UNIT4
`define UNIT2

module FOExp (SKiiP_EN, SKiiP_CTRL, SKiiP_ERR, PCON_IN, PCON_OUT, PCON_OUT_EN , SENS_IN, clock, DINPUT, DOUTPUT, INP_1V2OK, INP_2V5OK, INP_3V3OK);

output [4:1] SKiiP_EN;
output [4:1] SKiiP_CTRL; 
input [2:1] SKiiP_ERR;

input PCON_IN;
output PCON_OUT;
output PCON_OUT_EN;

input [6:1] SENS_IN;

input clock;

input [4:1] DINPUT;
output [2:1] DOUTPUT;

input INP_1V2OK;
input INP_2V5OK;
input INP_3V3OK;

reg [2:1] SKiiP_ERR_DATA;
reg PCON_IN_DATA;
reg [6:1] SENS_IN_DATA;
reg [4:1] DINPUT_DATA;
reg INP_1V2OK_DATA;
reg INP_2V5OK_DATA;
reg INP_3V3OK_DATA;

reg [4:1] SKiiP_ENABLE;
reg [4:1] SKiiP_CONTROL;
reg [2:1] DIGITAL_OUTPUT;

reg [4:1] SKiiP_EN_BUF;
reg [4:1] SKiiP_CTRL_BUF;
reg [2:1] DOUTPUT_BUF;
reg PCON_OUT_BUF;
reg PCON_OUT_EN_BUF;


reg [25:0] clock_div;
reg [5:0] clock_div2;

wire [15:0] CHOPP_CURR_IN;
reg [11:0] CHOPPER_CURRENT /*synthesis preserve */;
reg [31:0] CHOPPER_CURRENT_ADC;
wire CHOPP_CURR_DATA_READY;
wire [1:0] CHOPPER_CURR_ERR;

wire [15:0] BRAKE_CURR_IN;
reg [11:0] BRAKE_CURRENT /*synthesis preserve */;
reg [31:0] BRAKE_CURRENT_ADC;
wire BRAKE_CURR_DATA_READY;
wire [1:0] BRAKE_CURR_ERR;

wire [15:0] INPUT_VOLTAGE_IN;
reg [11:0] INPUT_VOLTAGE /*synthesis preserve */;
reg [31:0] INPUT_VOLTAGE_ADC;
wire INPUT_VOLTAGE_DATA_READY;
wire [1:0] INPUT_VOLTAGE_ERR;

wire [15:0] OUTPUT_VOLTAGE_IN;
reg [11:0] OUTPUT_VOLTAGE /*synthesis preserve */;
reg [31:0] OUTPUT_VOLTAGE_ADC;
wire OUTPUT_VOLTAGE_DATA_READY;
wire [1:0] OUTPUT_VOLTAGE_ERR;

wire [11:0] SKIIP_CHOPP_CURRENT_IN;
reg [11:0] SKIIP_CHOPP_CURRENT /*synthesis preserve */;
reg [31:0] SKIIP_CHOPP_CURRENT_ADC;
wire [11:0] SKIIP_CHOPP_TEMP_IN;
reg [11:0] SKIIP_CHOPP_TEMP  /*synthesis preserve */;
reg [31:0] SKIIP_CHOPP_TEMP_ADC;
wire SKIIP_CHOPP_DATA_READY;
wire [1:0] SKIIP_CHOPP_ERR;

wire [11:0] SKIIP_BRAKE_CURRENT_IN;
reg [11:0] SKIIP_BRAKE_CURRENT /*synthesis preserve */;
reg [31:0] SKIIP_BRAKE_CURRENT_ADC;
wire [11:0] SKIIP_BRAKE_TEMP_IN;
reg [11:0] SKIIP_BRAKE_TEMP  /*synthesis preserve */;
reg [31:0] SKIIP_BRAKE_TEMP_ADC;
wire SKIIP_BRAKE_DATA_READY;
wire [1:0] SKIIP_BRAKE_ERR;

reg [155:0] DATA_TO_PCON;
wire [155:0] DATA_FROM_PCON_wire;
reg [155:0] DATA_FROM_PCON;
wire PCON_DATA_READY;
wire [2:0] PCON_IO_ERR;

reg [15:0] CALC_SM_COUNTER;
reg [4:0] CALC_SM_STATE;
reg CALC_ERROR_FLAG;

reg [7:0] CRITICAL_DATA_INPUT;
reg [11:0] FO_INPUT_VAL1;
reg [11:0] FO_INPUT_VAL2;
reg [11:0] FO_INPUT_VAL3;
reg [11:0] FO_INPUT_VAL4;
reg [11:0] FO_INPUT_VAL5;
reg [11:0] FO_INPUT_VAL6;
reg [11:0] FO_INPUT_VAL7;
reg [7:0] FO_SLOW_INPUT_VAL1;
reg [7:0] FO_SLOW_INPUT_VAL2;
reg [7:0] FO_SLOW_INPUT_VAL3;
reg [7:0] FO_SLOW_INPUT_VAL4;
reg [7:0] FO_SLOW_INPUT_VAL5;
reg [7:0] FO_SLOW_INPUT_VAL6;
reg [7:0] FO_SLOW_INPUT_VAL7;
reg [7:0] FO_SLOW_INPUT_VAL8;

reg [23:0] ERROR_FLAGS /*synthesis preserve */;
reg [23:0] ERROR_LATCH /*synthesis preserve */;
reg GlobalError /*synthesis preserve */;
reg [15:0] Error_Reset_Counter;

reg [63:0] BRAKE_ENERGY_STOCK = 12000000;
reg [63:0] BRAKE_ENERGY_APPLIED;
reg [63:0] BRAKE_ENERGY_DISSIPATED;
reg [15:0] BRAKE_ENERGY_ON;
reg [15:0] BRAKE_ENERGY_OFF;
reg [15:0] BRAKE_ENERGY_COUNTER;
reg [15:0] BREAK_OUTPUT_VOLTAGE;

//reg REACTOR_OVERHEAT_ERROR1;
//reg REACTOR_OVERHEAT_ERROR2;
//reg BRAKE_RESISTANCE_OVERHEAT;

reg INP_OVERVOLTAGE_ERR;
reg OUT_OVERVOLTAGE_ERR;
reg CHOPP_OVERCURRENT_ERR;
reg BRAKE_OVERCURRENT_ERR;
reg OVERTEMP_ERR;
reg BRAKE_OVERLOAD_ERR=1;
reg PCON_ERROR_FLAG;
reg ERROR_RESET_FLAG;

reg [32:0] FAN_START_COUNTER;

reg BRAKE_ENABLE;
reg [15:0] BRAKE_PWM_COUNTER;
reg [15:0] BRAKE_PWM_WIDTH;

reg CHOPPER_ENABLE;
reg [15:0] CHOPPER_PWM_COUNTER;
reg [15:0] CHOPPER_PWM_WIDTH;

reg CHOPP_TOP_IGBT;
reg CHOPP_BOT_IGBT;

reg BRAKE_TOP_IGBT;
reg BRAKE_BOT_IGBT;

reg [4:1] DINPUT_FILTERED;
reg [15:0] DFILTER_COUNTER;
reg [15:0] DINP_FILTER1;
reg [15:0] DINP_FILTER2;
reg [15:0] DINP_FILTER3;
reg [15:0] DINP_FILTER4;

reg signed [31:0] REG_INTEGRATOR_LIMIT = 0;
reg signed [31:0]  CHOPPER_VOLTAGE_SETPOINT=0/*synthesis preserve */;
reg signed [31:0]  REG_VOLTAGE_INPUT=0;
reg signed [31:0]  REG_INPUT=0;
reg signed [31:0]  REG_INTEGRATOR=0;
reg signed [31:0]  REG_OUTPUT=0/*synthesis preserve */;
reg signed [31:0]  REG_ERROR = 1;
reg signed [31:0]  REG_PROP_OUTPUT = 0;
reg signed [31:0]  REG_INTEGRATOR_OUTPUT = 0;
reg signed [31:0]  REG_PROP = 5; // A/V
reg signed [31:0]  REG_INT = 15; //~x0.1 40 = 40 * 10000/1024 = 390 A/V/s
reg signed [31:0]  REG_INT_T = 4; //~x0.1 40 = 40 * 10000/1024 = 390 A/V/s
reg signed [31:0]  REG_RESULT = 1/*synthesis preserve */;
reg [15:0] REG_INTEGRATOR_COUNTER = 0;
reg signed [31:0]  	CHOPPER_CURRENT_LIMIT = 0/*synthesis preserve */;
reg signed [31:0] 	REG_CURRENT_LIMIT = 0/*synthesis preserve */; 
reg  [31:0] REG_DC_POWER = 0/*synthesis preserve */; 

/*reg signed	[12:0]	INPV_FILTER_INPUT;
reg INPV_FILTER_DATA_INP_READY;
reg [1:0] INPV_FILTER_DATA_INP_ERROR;	
wire [26:0] INPV_FILTER_OUTPUT;					
wire INPV_FILTER_DATA_OUTP_READY;
wire [1:0] INPV_FILTER_DATA_OUTP_ERROR;
reg signed [26:0] INPUT_VOLTAGE_FILTERED;
reg [15:0] FILTERS_COUNTER;
reg [1:0] FILTER_ERROR;
reg FILTER_DATA_READY;
reg filter_reset;
*/

reg [11:0] VIN_FILTER_INPUT /*synthesis preserve */;
reg [11:0] VIN_FILTER_OUTPUT /*synthesis preserve */;
wire [11:0] VIN_FILTER_OUTPUT_wire;

reg [11:0] VOUT_FILTER_INPUT /*synthesis preserve */;
reg [11:0] VOUT_FILTER_OUTPUT /*synthesis preserve */;
wire [11:0] VOUT_FILTER_OUTPUT_wire;

reg [11:0] CURRENT_FILTER_INPUT /*synthesis preserve */;
reg [11:0] CURRENT_FILTER_OUTPUT /*synthesis preserve */;
wire [11:0] CURRENT_FILTER_OUTPUT_wire;
reg [15:0]  MEAN_CHOPPER_CURRENT;

reg signed [31:0] INPUT_VOLTAGE_MAXPWM;
reg signed [31:0] OUTPUT_VOLTAGE_MAXPWM;
reg signed [31:0] CHOPPER_PWM_WIDTH_MAX;
reg signed [31:0] INPUT_VOLTAGE_PWM_WIDTH_MAX;
reg signed [31:0] CHOPPER_PWM_WIDTH_MAX_N_MULT;
reg signed [31:0] CHOPPER_PWM_WIDTH_MAX_N_DIV;
reg [15:0] CHOPPER_PWM_WIDTH_LIMIT;
reg [15:0] CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER=0;


reg [31:0] TEST_COUNTER=0;
reg TEST_DIRECTION=0;
reg [11:0] TEST_OUTPUT=0;

reg [15:0] REG_CALC_COUNTER=0;

reg BRAKE_ERROR; 
reg [31:0] BRAKE_ENABLE_COUNTER = 0;

reg [11:0] BRAKE_CURRENT_MAX;
reg [11:0] BRAKE_CURRENT_MAX_CYCLE;

reg [11:0] SKIIP_BRAKE_CURRENT_MAX;
reg [11:0] SKIIP_BRAKE_CURRENT_MAX_CYCLE;

reg [15:0] MAXCURR_COUNTER=0;

PCON_DATA_IO MAIN_FO_IO(.clockin(clock), .FO_INPUT(PCON_IN), .FO_OUTP(PCON_OUT),
								.DATA_TO_SEND(DATA_TO_PCON), 
								.DATA_RECIEVED(DATA_FROM_PCON_wire), 
								.DATA_CLOCK(PCON_DATA_READY), .IO_ERR(PCON_IO_ERR));

SKIIPInput CHOPPER_SKIIP_INPUT(.clockin(clock), .FO_INPUT(SENS_IN[1]), 
				.CURRENT_OUTPUT(SKIIP_CHOPP_CURRENT_IN), 
				.TEMP_OUTPUT(SKIIP_CHOPP_TEMP_IN), 
				.DATA_CLOCK(SKIIP_CHOPP_DATA_READY), 
				.SENSOR_ERR(SKIIP_CHOPP_ERR));
				
SKIIPInput BRAKE_SKIIP_INPUT(.clockin(clock), .FO_INPUT(SENS_IN[2]), 
				.CURRENT_OUTPUT(SKIIP_BRAKE_CURRENT_IN), 
				.TEMP_OUTPUT(SKIIP_BRAKE_TEMP_IN), 
				.DATA_CLOCK(SKIIP_BRAKE_DATA_READY), 
				.SENSOR_ERR(SKIIP_BRAKE_ERR));

SensorInput CHOPPER_CURRENT_SENSOR_INP(.clockin(clock), .FO_INPUT(SENS_IN[3]),
								.DATA_OUTPUT(CHOPP_CURR_IN), .DATA_CLOCK(CHOPP_CURR_DATA_READY), .SENSOR_ERR(CHOPPER_CURR_ERR));
SensorInput BRAKE_CURRENT_SENSOR_INP(.clockin(clock), .FO_INPUT(SENS_IN[4]), 
								.DATA_OUTPUT(BRAKE_CURR_IN), .DATA_CLOCK(BRAKE_CURR_DATA_READY), .SENSOR_ERR(BRAKE_CURR_ERR));
SensorInput INP_VOLTAGE_SENSOR_INP(.clockin(clock), .FO_INPUT(SENS_IN[5]), 
								.DATA_OUTPUT(INPUT_VOLTAGE_IN), .DATA_CLOCK(INPUT_VOLTAGE_DATA_READY), .SENSOR_ERR(INPUT_VOLTAGE_ERR));
SensorInput OUTP_VOLTAGE_SENSOR_INP(.clockin(clock), .FO_INPUT(SENS_IN[6]), 
								.DATA_OUTPUT(OUTPUT_VOLTAGE_IN), .DATA_CLOCK(OUTPUT_VOLTAGE_DATA_READY), .SENSOR_ERR(OUTPUT_VOLTAGE_ERR));


MeanFilter VIN_FILTER(.clockin(clock), .DATA_INPUT(VIN_FILTER_INPUT), .DATA_OUTPUT(VIN_FILTER_OUTPUT_wire));	
MeanFilter VOUT_FILTER(.clockin(clock), .DATA_INPUT(VOUT_FILTER_INPUT), .DATA_OUTPUT(VOUT_FILTER_OUTPUT_wire));
MeanFilter CHOPPER_CURRENT_FILTER(.clockin(clock), .DATA_INPUT(CURRENT_FILTER_INPUT), .DATA_OUTPUT(CURRENT_FILTER_OUTPUT_wire));
								
assign SKiiP_EN = SKiiP_EN_BUF;
assign SKiiP_CTRL = SKiiP_CTRL_BUF;
assign PCON_OUT_EN = PCON_OUT_EN_BUF;
assign DOUTPUT = DOUTPUT_BUF;

initial
begin
SKiiP_EN_BUF = 0;
SKiiP_CTRL_BUF = 0;
PCON_OUT_EN_BUF = 0;
DOUTPUT_BUF = 0;
CALC_ERROR_FLAG=0;
end

always@*
begin

SKiiP_CONTROL[1] = CHOPP_TOP_IGBT;
SKiiP_CONTROL[2] = CHOPP_BOT_IGBT;

SKiiP_CONTROL[3] = BRAKE_TOP_IGBT;
SKiiP_CONTROL[4] = BRAKE_BOT_IGBT;


ERROR_FLAGS[0] = CHOPPER_CURR_ERR[0]|CHOPPER_CURR_ERR[1];

`ifdef UNIT3
ERROR_FLAGS[1] = 0;//BRAKE_CURR_ERR[0]|BRAKE_CURR_ERR[1]; 		//TEMPLOCK U3!!!!
`else
ERROR_FLAGS[1] = BRAKE_CURR_ERR[0]|BRAKE_CURR_ERR[1];				//NORMAL U1,2,4!!!!
`endif

ERROR_FLAGS[2] = INPUT_VOLTAGE_ERR[0]|INPUT_VOLTAGE_ERR[1];
ERROR_FLAGS[3] = OUTPUT_VOLTAGE_ERR[0]|OUTPUT_VOLTAGE_ERR[1];
ERROR_FLAGS[4] = SKIIP_CHOPP_ERR[0]|SKIIP_CHOPP_ERR[1];

`ifdef UNIT2
ERROR_FLAGS[5] = SKIIP_BRAKE_ERR[0]|SKIIP_BRAKE_ERR[1];			//NORMAL U2
`else
ERROR_FLAGS[5] = SKIIP_BRAKE_ERR[0]|SKIIP_BRAKE_ERR[1];			//NORMAL U1,3,4!!!!
`endif

ERROR_FLAGS[6] = ~SKiiP_ERR[1];
ERROR_FLAGS[7] = ~SKiiP_ERR[2];
ERROR_FLAGS[8] = PCON_IO_ERR[0]|PCON_IO_ERR[1]|PCON_IO_ERR[2];
ERROR_FLAGS[9] = CALC_ERROR_FLAG;

`ifdef UNIT4
ERROR_FLAGS[10] = (~INP_1V2OK)|(~INP_2V5OK);  //TEMPLOCK U4!!!!
`else
ERROR_FLAGS[10] = (~INP_1V2OK)|(~INP_2V5OK)|INP_3V3OK;  //NORMAL U1,2,3!!!!
`endif

ERROR_FLAGS[11] = 0;

ERROR_FLAGS[12] = ~DINPUT_FILTERED[1]; // 
ERROR_FLAGS[13] = ~DINPUT_FILTERED[2]; //
ERROR_FLAGS[14] = ~DINPUT_FILTERED[3]; //
ERROR_FLAGS[15] = DINPUT_FILTERED[4];
ERROR_FLAGS[16] = INP_OVERVOLTAGE_ERR;
ERROR_FLAGS[17] = OUT_OVERVOLTAGE_ERR;
ERROR_FLAGS[18] = CHOPP_OVERCURRENT_ERR;
ERROR_FLAGS[19] = BRAKE_OVERCURRENT_ERR;
ERROR_FLAGS[20] = OVERTEMP_ERR;
ERROR_FLAGS[21] = BRAKE_OVERLOAD_ERR;
ERROR_FLAGS[22] = PCON_ERROR_FLAG;
ERROR_FLAGS[23] = GlobalError;

if ((ERROR_LATCH & (24'b001111111111111111111111)) == 0) begin BRAKE_ERROR = 0; end
else begin BRAKE_ERROR = 1; end

//BRAKE_ERROR = 	ERROR_LATCH[1]|ERROR_LATCH[3]|ERROR_LATCH[5]|ERROR_LATCH[6]|ERROR_LATCH[7]|
//					ERROR_LATCH[8]|ERROR_LATCH[9]|ERROR_LATCH[10]|ERROR_LATCH[14]|
//					ERROR_LATCH[19]|ERROR_LATCH[20]|ERROR_LATCH[21];

if (Error_Reset_Counter == 0) 
	begin
	ERROR_LATCH = ERROR_LATCH | ERROR_FLAGS;
	end
else 
	begin
	ERROR_LATCH = ERROR_FLAGS & (24'b001111111111111111111111);
	end
	
if ((ERROR_LATCH == 0))
		begin
		GlobalError=0;
		SKiiP_EN_BUF[1] = 0;
		SKiiP_EN_BUF[2] = 0;
		SKiiP_EN_BUF[3] = 1;
		SKiiP_EN_BUF[4] = 1;
		SKiiP_CTRL_BUF[1] = ~SKiiP_CONTROL[1];
		SKiiP_CTRL_BUF[2] = ~SKiiP_CONTROL[2];
		SKiiP_CTRL_BUF[3] = 1;
		SKiiP_CTRL_BUF[4] = 1;
		DOUTPUT_BUF = ~DIGITAL_OUTPUT;
		end
else
		begin
		SKiiP_EN_BUF[1] = 1;
		SKiiP_EN_BUF[2] = 1;
		SKiiP_EN_BUF[3] = 0;
		SKiiP_EN_BUF[4] = 0;
		SKiiP_CTRL_BUF[1] = 1;
		SKiiP_CTRL_BUF[2] = 1;
		SKiiP_CTRL_BUF[3] = ~SKiiP_CONTROL[3];
		SKiiP_CTRL_BUF[4] = ~SKiiP_CONTROL[4];
		DOUTPUT_BUF = ~DIGITAL_OUTPUT;
		GlobalError=1;
		end

end


always@(posedge clock)
begin



DATA_FROM_PCON = DATA_FROM_PCON_wire;
PCON_OUT_EN_BUF = 0;

//DINP FILTERS -----------------------------------

if (DFILTER_COUNTER>5000)
	begin
	DFILTER_COUNTER = 0;
	DINP_FILTER1 = 0;
	DINP_FILTER2 = 0;
	DINP_FILTER3 = 0;
	DINP_FILTER4 = 0;
	end
else if (DFILTER_COUNTER>4950)
	begin
	if (DINP_FILTER1>2500) begin DINPUT_FILTERED[1]=1; end else  begin DINPUT_FILTERED[1]=0; end
	if (DINP_FILTER2>2500) begin DINPUT_FILTERED[2]=1; end else  begin DINPUT_FILTERED[2]=0; end
	if (DINP_FILTER3>2500) begin DINPUT_FILTERED[3]=1; end else  begin DINPUT_FILTERED[3]=0; end
	if (DINP_FILTER4>2500) begin DINPUT_FILTERED[4]=1; end else  begin DINPUT_FILTERED[4]=0; end
	DFILTER_COUNTER = DFILTER_COUNTER + 1;
	end
else 
	begin
	if (DINPUT[1]==1) begin DINP_FILTER1 = DINP_FILTER1 + 1; end
	if (DINPUT[2]==1) begin DINP_FILTER2 = DINP_FILTER2 + 1; end
	if (DINPUT[3]==1) begin DINP_FILTER3 = DINP_FILTER3 + 1; end
	if (DINPUT[4]==1) begin DINP_FILTER4 = DINP_FILTER4 + 1; end
	DFILTER_COUNTER = DFILTER_COUNTER + 1;
	end


//----------SENSOR VALUES CALCULATIONS ----------------------------

if (CALC_SM_STATE==0)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	CHOPPER_CURRENT_ADC=0;
	BRAKE_CURRENT_ADC=0;
	INPUT_VOLTAGE_ADC=0;
	OUTPUT_VOLTAGE_ADC=0;
	SKIIP_CHOPP_CURRENT_ADC=0;
	SKIIP_CHOPP_TEMP_ADC=0;
	SKIIP_BRAKE_CURRENT_ADC=0;
	SKIIP_BRAKE_TEMP_ADC=0;
	
	if (CALC_SM_COUNTER>5) begin CALC_SM_STATE=1; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==1)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	
	CHOPPER_CURRENT_ADC[14:0] = CHOPP_CURR_IN[14:0];
	
	`ifdef UNIT3
	BRAKE_CURRENT_ADC[14:0] = 16560; 						//TEMPLOCK U3!!!!
	`else
	BRAKE_CURRENT_ADC[14:0] = BRAKE_CURR_IN[14:0];	//NORMAL U1,2,4!!!!
	`endif

	INPUT_VOLTAGE_ADC[14:0] = INPUT_VOLTAGE_IN[14:0];
	OUTPUT_VOLTAGE_ADC[14:0] = OUTPUT_VOLTAGE_IN[14:0];
	
	SKIIP_CHOPP_CURRENT_ADC[11:0] = SKIIP_CHOPP_CURRENT_IN;
	SKIIP_CHOPP_TEMP_ADC[11:0] = SKIIP_CHOPP_TEMP_IN;
	
	`ifdef UNIT2
	SKIIP_BRAKE_CURRENT_ADC[11:0] = SKIIP_BRAKE_CURRENT_IN;	//NORMAL U2
	SKIIP_BRAKE_TEMP_ADC[11:0] = SKIIP_BRAKE_TEMP_IN;			//NORMAL U2
	`else
	SKIIP_BRAKE_CURRENT_ADC[11:0] = SKIIP_BRAKE_CURRENT_IN;	//NORMAL U1,3,4!!!!
	SKIIP_BRAKE_TEMP_ADC[11:0] = SKIIP_BRAKE_TEMP_IN;			//NORMAL U1,3,4!!!!
	`endif
	
	CHOPPER_CURRENT_ADC[31:15] = 0;
	BRAKE_CURRENT_ADC[31:15] = 0;
	INPUT_VOLTAGE_ADC[31:15] = 0;
	OUTPUT_VOLTAGE_ADC[31:15] = 0;
	SKIIP_CHOPP_CURRENT_ADC[31:12] = 0;
	SKIIP_CHOPP_TEMP_ADC[31:12] = 0;
	SKIIP_BRAKE_CURRENT_ADC[31:12] = 0;
	SKIIP_BRAKE_TEMP_ADC[31:12] = 0;
	if (CALC_SM_COUNTER>5) begin CALC_SM_STATE=2; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==2)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	if (CALC_SM_COUNTER==1)
		begin
		
		`ifdef UNIT1
		if (CHOPPER_CURRENT_ADC>=16585) begin CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC - 16585; end //U1=16585 //U2 = 16560 //U3 = 16590 //U4 = 16580 
		else begin CHOPPER_CURRENT_ADC = 16585 - CHOPPER_CURRENT_ADC; end
		`endif
		
		`ifdef UNIT2
		if (CHOPPER_CURRENT_ADC>=16560) begin CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC - 16560; end 
		else begin CHOPPER_CURRENT_ADC = 16560 - CHOPPER_CURRENT_ADC; end
		`endif
		
		`ifdef UNIT3
		if (CHOPPER_CURRENT_ADC>=16590) begin CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC - 16590; end
		else begin CHOPPER_CURRENT_ADC = 16590 - CHOPPER_CURRENT_ADC; end
		`endif
		
		`ifdef UNIT4
		if (CHOPPER_CURRENT_ADC>=16580) begin CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC - 16580; end  
		else begin CHOPPER_CURRENT_ADC = 16580 - CHOPPER_CURRENT_ADC; end
		`endif
		
		`ifdef UNIT4
		BRAKE_CURRENT_ADC = 0;
		`else
		if (BRAKE_CURRENT_ADC>=16560) begin BRAKE_CURRENT_ADC = BRAKE_CURRENT_ADC - 16560; end
		else begin BRAKE_CURRENT_ADC = 16560 - BRAKE_CURRENT_ADC; end
		`endif
		
		
		if (INPUT_VOLTAGE_ADC>=16384) begin INPUT_VOLTAGE_ADC = INPUT_VOLTAGE_ADC - 16384; end
		else begin INPUT_VOLTAGE_ADC = 16384 - INPUT_VOLTAGE_ADC; end
		
		if (OUTPUT_VOLTAGE_ADC>=16384) begin OUTPUT_VOLTAGE_ADC = OUTPUT_VOLTAGE_ADC - 16384; end
		else begin OUTPUT_VOLTAGE_ADC = 16384 - OUTPUT_VOLTAGE_ADC; end
		
		if (SKIIP_CHOPP_CURRENT_ADC>=1707) begin SKIIP_CHOPP_CURRENT_ADC = SKIIP_CHOPP_CURRENT_ADC - 1707; end
		else begin SKIIP_CHOPP_CURRENT_ADC = 1707 - SKIIP_CHOPP_CURRENT_ADC; end
		
		if (SKIIP_CHOPP_TEMP_ADC>=1707) begin SKIIP_CHOPP_TEMP_ADC = SKIIP_CHOPP_TEMP_ADC - 1707; end
		else begin SKIIP_CHOPP_TEMP_ADC = 1707 - SKIIP_CHOPP_TEMP_ADC; end
		
		if (SKIIP_BRAKE_CURRENT_ADC>=1707) begin SKIIP_BRAKE_CURRENT_ADC = SKIIP_BRAKE_CURRENT_ADC - 1707; end
		else begin SKIIP_BRAKE_CURRENT_ADC = 1707 - SKIIP_BRAKE_CURRENT_ADC; end
		
		if (SKIIP_BRAKE_TEMP_ADC>=1707) begin SKIIP_BRAKE_TEMP_ADC = SKIIP_BRAKE_TEMP_ADC - 1707; end
		else begin SKIIP_BRAKE_TEMP_ADC = 1707 - SKIIP_BRAKE_TEMP_ADC; end
		end
		
	if (CALC_SM_COUNTER>5) begin CALC_SM_STATE=3; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==3)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	if (CALC_SM_COUNTER==1)
		begin
		CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC*256;
		BRAKE_CURRENT_ADC=BRAKE_CURRENT_ADC*256;
		INPUT_VOLTAGE_ADC=INPUT_VOLTAGE_ADC*256;
		OUTPUT_VOLTAGE_ADC=OUTPUT_VOLTAGE_ADC*256;
		SKIIP_CHOPP_CURRENT_ADC=SKIIP_CHOPP_CURRENT_ADC*256;
		SKIIP_CHOPP_TEMP_ADC=SKIIP_CHOPP_TEMP_ADC*256;
		SKIIP_BRAKE_CURRENT_ADC=SKIIP_BRAKE_CURRENT_ADC*256;
		SKIIP_BRAKE_TEMP_ADC=SKIIP_BRAKE_TEMP_ADC*256;
		end
	if (CALC_SM_COUNTER>20) begin CALC_SM_STATE=4; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==4)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	if (CALC_SM_COUNTER==1)
		begin
		CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC/1006;
		BRAKE_CURRENT_ADC=BRAKE_CURRENT_ADC/1006;
		INPUT_VOLTAGE_ADC=INPUT_VOLTAGE_ADC/1610;
		OUTPUT_VOLTAGE_ADC=OUTPUT_VOLTAGE_ADC/1610;
		SKIIP_CHOPP_CURRENT_ADC=SKIIP_CHOPP_CURRENT_ADC/175;
		SKIIP_CHOPP_TEMP_ADC= 20 + SKIIP_CHOPP_TEMP_ADC/4369;
		SKIIP_BRAKE_CURRENT_ADC=SKIIP_BRAKE_CURRENT_ADC/175;
		SKIIP_BRAKE_TEMP_ADC= 20 + SKIIP_BRAKE_TEMP_ADC/4369;
		end
	if (CALC_SM_COUNTER>20) begin CALC_SM_STATE=5; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==5)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	if (CHOPPER_CURRENT_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (BRAKE_CURRENT_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (INPUT_VOLTAGE_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (OUTPUT_VOLTAGE_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (SKIIP_CHOPP_CURRENT_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (SKIIP_CHOPP_TEMP_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (SKIIP_BRAKE_CURRENT_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else if (SKIIP_BRAKE_TEMP_ADC>4095) begin CALC_ERROR_FLAG=1; end
	else begin CALC_ERROR_FLAG=0; end
	
	if (CALC_SM_COUNTER>5) begin CALC_SM_STATE=6; CALC_SM_COUNTER=0; end
	end
else if (CALC_SM_STATE==6)
	begin
	CALC_SM_COUNTER = CALC_SM_COUNTER + 1;
	//if (CALC_ERROR_FLAG==0)
	//	begin
		CHOPPER_CURRENT<=CHOPPER_CURRENT_ADC[11:0];
		BRAKE_CURRENT<=BRAKE_CURRENT_ADC[11:0];
		INPUT_VOLTAGE<=INPUT_VOLTAGE_ADC[11:0];
		OUTPUT_VOLTAGE<=OUTPUT_VOLTAGE_ADC[11:0];
		SKIIP_CHOPP_CURRENT<=SKIIP_CHOPP_CURRENT_ADC[11:0];
		SKIIP_CHOPP_TEMP<=SKIIP_CHOPP_TEMP_ADC[11:0];
		SKIIP_BRAKE_CURRENT<=SKIIP_BRAKE_CURRENT_ADC[11:0];
		SKIIP_BRAKE_TEMP<=SKIIP_BRAKE_TEMP_ADC[11:0];
	//	end
	if (CALC_SM_COUNTER>5) begin CALC_SM_STATE=0; CALC_SM_COUNTER=0; end
	end
	
//----------SENSOR VALUES CALCULATIONS ----------------------------	

//-----------------FILTERS-------------------------------

VIN_FILTER_INPUT = INPUT_VOLTAGE;
VIN_FILTER_OUTPUT = VIN_FILTER_OUTPUT_wire;  

VOUT_FILTER_INPUT = OUTPUT_VOLTAGE;
VOUT_FILTER_OUTPUT = VOUT_FILTER_OUTPUT_wire;

MEAN_CHOPPER_CURRENT = (CHOPPER_CURRENT+SKIIP_CHOPP_CURRENT)/2;
CURRENT_FILTER_INPUT = MEAN_CHOPPER_CURRENT;
CURRENT_FILTER_OUTPUT = CURRENT_FILTER_OUTPUT_wire;
REG_DC_POWER = CURRENT_FILTER_OUTPUT*VIN_FILTER_INPUT/1000;
//----------PROTECTIONS -------------------------------------------


if (
	(CHOPPER_ENABLE==1 && ((CHOPPER_CURRENT>1000) || (SKIIP_CHOPP_CURRENT>1200)))
	|| (CHOPPER_CURRENT>1500)
	|| (SKIIP_CHOPP_CURRENT>1800)
	) begin CHOPP_OVERCURRENT_ERR = 1; end
else begin CHOPP_OVERCURRENT_ERR = 0; end

if (
	(BRAKE_ENABLE==1 && ((BRAKE_CURRENT>1500) || (SKIIP_BRAKE_CURRENT>1200)))
	|| (BRAKE_CURRENT>1500)
	|| (SKIIP_BRAKE_CURRENT>2000)
	) begin BRAKE_OVERCURRENT_ERR = 1; end
else begin BRAKE_OVERCURRENT_ERR = 0; end


if (INPUT_VOLTAGE>1100 || VIN_FILTER_OUTPUT>1000) begin INP_OVERVOLTAGE_ERR=1; end 
else begin INP_OVERVOLTAGE_ERR=0; end

if (OUTPUT_VOLTAGE>1100 || VOUT_FILTER_OUTPUT>1000) begin OUT_OVERVOLTAGE_ERR=1; end 
else begin OUT_OVERVOLTAGE_ERR=0; end

if ((SKIIP_BRAKE_TEMP>70) || (SKIIP_CHOPP_TEMP>70)) begin OVERTEMP_ERR = 1; end
else begin OVERTEMP_ERR = 0; end

if (BRAKE_ENERGY_COUNTER>50000)
	begin
	BRAKE_ENERGY_COUNTER = 0;
	BRAKE_ENERGY_ON = 0;
	BRAKE_ENERGY_OFF = 0;
	BREAK_OUTPUT_VOLTAGE = VOUT_FILTER_OUTPUT;
	//BREAK_OUTPUT_VOLTAGE = 700;
	end
if (BRAKE_ENERGY_COUNTER==49990)
	begin
	 if (BRAKE_ENERGY_STOCK<20000000)  begin BRAKE_ENERGY_STOCK = BRAKE_ENERGY_STOCK + BRAKE_ENERGY_APPLIED; end
	 else begin BRAKE_ENERGY_STOCK = 20000000; end
	end
if (BRAKE_ENERGY_COUNTER==49995)
	begin
	if (BRAKE_ENERGY_STOCK>BRAKE_ENERGY_DISSIPATED)
		begin
		BRAKE_ENERGY_STOCK = BRAKE_ENERGY_STOCK - BRAKE_ENERGY_DISSIPATED;
		end
	else
		begin
		BRAKE_ENERGY_STOCK = 0;
		end
	end

BRAKE_ENERGY_APPLIED = (BRAKE_ENERGY_ON*BREAK_OUTPUT_VOLTAGE*BREAK_OUTPUT_VOLTAGE)/50000000;
BRAKE_ENERGY_DISSIPATED = (BRAKE_ENERGY_STOCK/200000)+1; // 75000kW/15000000MJ/1000ms = 1/200/1000 = 1/200000
BRAKE_ENERGY_COUNTER = BRAKE_ENERGY_COUNTER + 1;

if (BRAKE_TOP_IGBT==1)
	begin
	BRAKE_ENERGY_ON = BRAKE_ENERGY_ON + 1;
	end
else
	begin
	BRAKE_ENERGY_OFF = BRAKE_ENERGY_OFF + 1;
	end
	
if (BRAKE_ENERGY_STOCK>12000000) //12MJ limit = 12000000
	begin
	BRAKE_OVERLOAD_ERR = 1;
	end
else
	begin
	BRAKE_OVERLOAD_ERR = 0;
	end

	PCON_ERROR_FLAG = ~CRITICAL_DATA_INPUT[0];

	
	if ((ERROR_RESET_FLAG == 0) && (CRITICAL_DATA_INPUT[1]==1)) //ERROR RESET
		begin
		Error_Reset_Counter = 15000; 
		ERROR_RESET_FLAG = 1;
		end
	if ((ERROR_RESET_FLAG == 1) && (CRITICAL_DATA_INPUT[1]==0))
		begin
		ERROR_RESET_FLAG = 0;
		end
	if (Error_Reset_Counter>0)
		begin
		Error_Reset_Counter = Error_Reset_Counter - 1;
		end
	
//----------PROTECTIONS -------------------------------------------
	
//----------CONTROL AND PWM GENERATION-----------------------------
//CHOPPER CONTROL
CHOPPER_ENABLE = CRITICAL_DATA_INPUT[2];
CHOPP_TOP_IGBT=0;

//TEST--------------------------------------------
/*
if (TEST_COUNTER>50000) begin TEST_COUNTER=0; end
else begin TEST_COUNTER = TEST_COUNTER + 1; end
if (TEST_COUNTER==0)	
begin
if (CURRENT_FILTER_OUTPUT > 500) begin TEST_DIRECTION=1; CURRENT_FILTER_OUTPUT=500; end
if (CURRENT_FILTER_OUTPUT == 0) begin TEST_DIRECTION=0; CURRENT_FILTER_OUTPUT = 1; end
else 
	begin 
		if (TEST_DIRECTION==0) begin CURRENT_FILTER_OUTPUT = CURRENT_FILTER_OUTPUT + 1;  end
		else begin CURRENT_FILTER_OUTPUT = CURRENT_FILTER_OUTPUT - 1; end
	end
end
*/
//TEST--------------------------------------------

CHOPPER_VOLTAGE_SETPOINT = FO_INPUT_VAL1;
CHOPPER_CURRENT_LIMIT = FO_INPUT_VAL2;
REG_INTEGRATOR_LIMIT = CHOPPER_CURRENT_LIMIT*1024;
REG_VOLTAGE_INPUT = VOUT_FILTER_OUTPUT;

	`ifdef UNIT1
	 REG_PROP = 1; //A/V
	 REG_INT = 9; //~x0.1 10 = 10 * 10000/1024 = 95 A/V/s	= 9500A/100V/s = 950A/100ms
	 `endif
	
	 `ifdef UNIT2
	 REG_PROP = 1; //A/V
	 REG_INT = 10; //
	 `endif
	
	`ifdef UNIT3
	 REG_PROP = 1; //A/V
	 REG_INT = 11; //
	`endif 
	 
	`ifdef UNIT4
	 REG_PROP = 1; //A/V
	 REG_INT = 12; //
	`endif


if (CHOPPER_ENABLE==1 && GlobalError==0)
	begin
	//TEST--------------------------------------------
	//if (TEST_COUNTER>2000000) begin TEST_COUNTER=0; end
	//else begin TEST_COUNTER = TEST_COUNTER + 1; end
	//if (TEST_COUNTER<1000000) begin REG_INPUT = 5; end
	//else begin REG_INPUT = -5; end
	//TEST--------------------------------------------
	
	/*if (REG_CALC_COUNTER>50) begin REG_CALC_COUNTER=0; end
	else begin REG_CALC_COUNTER = REG_CALC_COUNTER + 1; end

	if (REG_CALC_COUNTER<20)
	begin
	REG_PROP = REG_PROP;
	REG_INT = REG_INT;
	if (REG_DC_POWER>50)
		begin
			if (REG_DC_POWER>100)
				begin
				REG_PROP_T = 1; //A/V
				REG_INT_T = 20; //~x0.1 20 = 20 * 10000/1024 = 195 A/V/s	
				end
			else
				begin
					if (REG_DC_POWER<75)
						begin
						REG_PROP_T = 2; //A/V
						REG_INT_T = 30; //~x0.1 30 = 30 * 10000/1024 = 293 A/V/s
						end
					else 
						begin
						REG_PROP_T = REG_PROP_T; 
						REG_INT_T = REG_INT_T; 
						end
				end
		end
	else
		begin
			if (REG_DC_POWER<25) 
				begin 
					REG_PROP_T = 5; //A/V
					REG_INT_T = 40; //~x0.1 40 = 40 * 10000/1024 = 390 A/V/s
				end
			else 
				begin 
					REG_PROP_T = REG_PROP_T; 
					REG_INT_T = REG_INT_T; 
				end	
		end
	end
	else if (REG_CALC_COUNTER>30)
	begin
	REG_PROP = REG_PROP_T;
	REG_INT = REG_INT_T;
	end
	else
	begin
	REG_PROP = REG_PROP;
	REG_INT = REG_INT;
	end*/
	
	REG_INPUT = CHOPPER_VOLTAGE_SETPOINT - REG_VOLTAGE_INPUT;

	if (REG_INPUT>100) begin REG_ERROR = 100; end
	else if (REG_INPUT<-100)  begin REG_ERROR = -100; end
	else begin REG_ERROR = REG_INPUT;  end

	REG_PROP_OUTPUT = REG_ERROR*REG_PROP;

	if (REG_INTEGRATOR_COUNTER==5000)
		begin
		REG_INTEGRATOR = REG_INTEGRATOR + REG_ERROR*REG_INT_T; //Integrator x1024
		REG_INTEGRATOR_COUNTER = 0;
		REG_INT_T = REG_INT_T;
		end
	else if (REG_INTEGRATOR_COUNTER>5000)
		begin
		REG_INTEGRATOR_COUNTER = 0;
		REG_INTEGRATOR = REG_INTEGRATOR;
		REG_INT_T = REG_INT_T;
		end
	else
		begin
		REG_INTEGRATOR_COUNTER = REG_INTEGRATOR_COUNTER + 1;
		if (REG_INTEGRATOR>REG_INTEGRATOR_LIMIT) begin REG_INTEGRATOR = REG_INTEGRATOR_LIMIT; end //Integrator x1024
		else if (REG_INTEGRATOR<0) begin REG_INTEGRATOR = 0; end
		else begin REG_INTEGRATOR = REG_INTEGRATOR; end
		
		if (CURRENT_FILTER_OUTPUT>200) begin REG_INT_T = REG_INT; end
		else if (CURRENT_FILTER_OUTPUT<50) begin REG_INT_T = 4;  end
		else begin REG_INT_T = REG_INT_T; end
		
		end
		
		
	REG_INTEGRATOR_OUTPUT = REG_INTEGRATOR/1024;
	REG_RESULT = REG_PROP_OUTPUT + REG_INTEGRATOR_OUTPUT;
	if (REG_RESULT>850) begin REG_OUTPUT = 850; end
	else if (REG_RESULT<0) begin REG_OUTPUT = 0; end
	else begin REG_OUTPUT = REG_RESULT; end
	if (REG_OUTPUT<CHOPPER_CURRENT_LIMIT)		begin			REG_CURRENT_LIMIT = REG_OUTPUT;		end
	else		begin			REG_CURRENT_LIMIT = CHOPPER_CURRENT_LIMIT;		end
	
	end
else
	begin
	if (CHOPPER_CURRENT>SKIIP_CHOPP_CURRENT)
		begin
		REG_INTEGRATOR=CHOPPER_CURRENT*1024;
		end
	else 
		begin
		REG_INTEGRATOR=SKIIP_CHOPP_CURRENT*1024;
		end
	REG_INT_T = 4;	
	REG_INPUT = 0;
	REG_ERROR=0;
	REG_PROP_OUTPUT=0;
	REG_INTEGRATOR_COUNTER=0;
	REG_RESULT=0;
	REG_OUTPUT=0;
	REG_CURRENT_LIMIT = 0;
	end

//if (FO_INPUT_VAL1<50) begin CHOPPER_PWM_WIDTH = 0; end
//else begin CHOPPER_PWM_WIDTH = FO_INPUT_VAL1*4;  end
//if (VIN_FILTER_OUTPUT>10) begin INPUT_VOLTAGE_MAXPWM = VIN_FILTER_OUTPUT; end
//else begin INPUT_VOLTAGE_MAXPWM = 10; end
//if (VOUT_FILTER_OUTPUT>10) 
//	begin
//	if (VOUT_FILTER_OUTPUT<800) begin OUTPUT_VOLTAGE_MAXPWM = VOUT_FILTER_OUTPUT+100; end
//	else begin OUTPUT_VOLTAGE_MAXPWM = 900; end
//	end
//else begin OUTPUT_VOLTAGE_MAXPWM = 110; end
//CHOPPER_PWM_WIDTH_MAX = 15000 - (15000*INPUT_VOLTAGE_MAXPWM)/OUTPUT_VOLTAGE_MAXPWM;

//TEST--------------------------------------------
/*
if (TEST_COUNTER>50000) begin TEST_COUNTER=0; end
else begin TEST_COUNTER = TEST_COUNTER + 1; end
if (TEST_COUNTER==0)	
begin
if (TEST_OUTPUT > 800) begin TEST_DIRECTION=1; TEST_OUTPUT=800; end
if (TEST_OUTPUT == 0) begin TEST_DIRECTION=0; TEST_OUTPUT = 1; end
else 
	begin 
		if (TEST_DIRECTION==0) begin TEST_OUTPUT = TEST_OUTPUT + 1;  end
		else begin TEST_OUTPUT = TEST_OUTPUT - 1; end
	end
end
*/
//TEST--------------------------------------------



if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER>250) begin CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER=0; end
else begin CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER = CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER+1; end

//if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER==0) begin INPUT_VOLTAGE_PWM_WIDTH_MAX = TEST_OUTPUT; end 	//TEST!!
if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER==0) begin INPUT_VOLTAGE_PWM_WIDTH_MAX = VIN_FILTER_OUTPUT; end
else begin INPUT_VOLTAGE_PWM_WIDTH_MAX = INPUT_VOLTAGE_PWM_WIDTH_MAX; end

if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER==50) begin CHOPPER_PWM_WIDTH_MAX_N_MULT = INPUT_VOLTAGE_PWM_WIDTH_MAX*15000; end
else begin CHOPPER_PWM_WIDTH_MAX_N_MULT = CHOPPER_PWM_WIDTH_MAX_N_MULT; end

if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER==100) begin CHOPPER_PWM_WIDTH_MAX_N_DIV = CHOPPER_PWM_WIDTH_MAX_N_MULT/800; end
else begin CHOPPER_PWM_WIDTH_MAX_N_DIV = CHOPPER_PWM_WIDTH_MAX_N_DIV; end

if (CHOPPER_PWM_WIDTH_MAX_CALC_COUNTER==200) begin CHOPPER_PWM_WIDTH_MAX = 15000 - CHOPPER_PWM_WIDTH_MAX_N_DIV; end
else begin CHOPPER_PWM_WIDTH_MAX = CHOPPER_PWM_WIDTH_MAX; end

if (CHOPPER_PWM_WIDTH_MAX>12000) begin CHOPPER_PWM_WIDTH_LIMIT = 12000; end
else if (CHOPPER_PWM_WIDTH_MAX<200) begin CHOPPER_PWM_WIDTH_LIMIT = 0; end
else begin CHOPPER_PWM_WIDTH_LIMIT = CHOPPER_PWM_WIDTH_MAX;  end

//CHOPPER_PWM_WIDTH_LIMIT = 12000;

if (GlobalError==1 || CHOPPER_ENABLE==0) begin CHOPP_BOT_IGBT = 0; CHOPPER_PWM_COUNTER = 0; end
else
	begin
	
		if (CHOPPER_PWM_COUNTER>15000) //300uS cycle, 3333Hz
			begin 
			CHOPPER_PWM_COUNTER = 0;
			CHOPP_BOT_IGBT = 0;	
			end 
		else if (CHOPPER_PWM_COUNTER == 0)
			begin
				if (CHOPPER_CURRENT<REG_CURRENT_LIMIT && SKIIP_CHOPP_CURRENT<REG_CURRENT_LIMIT && CHOPPER_PWM_WIDTH_LIMIT>200)
					begin
					CHOPP_BOT_IGBT = 1;
					CHOPPER_PWM_COUNTER = 1;
					end
				else
					begin
					CHOPP_BOT_IGBT = 0;
					CHOPPER_PWM_COUNTER = 0;
					end
			end
		else 
		begin 
		CHOPPER_PWM_COUNTER = CHOPPER_PWM_COUNTER + 1; 
		if (CHOPPER_CURRENT>REG_CURRENT_LIMIT || SKIIP_CHOPP_CURRENT>REG_CURRENT_LIMIT || CHOPPER_PWM_COUNTER>CHOPPER_PWM_WIDTH_LIMIT)
			begin
				if (CHOPPER_PWM_COUNTER>200) begin CHOPP_BOT_IGBT = 0; end
				else begin CHOPP_BOT_IGBT = CHOPP_BOT_IGBT; end
			end
		else
			begin
			CHOPP_BOT_IGBT = CHOPP_BOT_IGBT;
			end
		end

	end

//BRAKE CONTROL
BRAKE_BOT_IGBT=0; 

if (FO_INPUT_VAL3<25) begin BRAKE_PWM_WIDTH = 0; end
else begin BRAKE_PWM_WIDTH = FO_INPUT_VAL3*8;  end

//BRAKE_PWM_WIDTH = 25000; 

if (GlobalError==0) begin BRAKE_ENABLE_COUNTER = 0; BRAKE_ENABLE=0; end 
else	
	begin
	if (BRAKE_ENABLE_COUNTER<3000000000) // 3000000000 = 60s, 
		begin 
			if (BRAKE_ENABLE_COUNTER>5000000) 
				begin 
				BRAKE_ENABLE = CRITICAL_DATA_INPUT[3]; 
				end
			else 
				begin 
				BRAKE_ENABLE = 0; 
				end
		BRAKE_ENABLE_COUNTER = BRAKE_ENABLE_COUNTER + 1;
		end
	else
		begin
		BRAKE_ENABLE = 0;
		BRAKE_ENABLE_COUNTER = 3000000000;
		end
	end

if (MAXCURR_COUNTER>30000) 
	begin 
	MAXCURR_COUNTER = 0;
	BRAKE_CURRENT_MAX = BRAKE_CURRENT_MAX_CYCLE; 
	SKIIP_BRAKE_CURRENT_MAX = SKIIP_BRAKE_CURRENT_MAX_CYCLE;
	end
else if (MAXCURR_COUNTER == 0) 
	begin
	MAXCURR_COUNTER = MAXCURR_COUNTER + 1;
	BRAKE_CURRENT_MAX_CYCLE = 0;
	SKIIP_BRAKE_CURRENT_MAX_CYCLE = 0;
	end
else
	begin
	MAXCURR_COUNTER = MAXCURR_COUNTER + 1;
	if (BRAKE_CURRENT>BRAKE_CURRENT_MAX_CYCLE) begin BRAKE_CURRENT_MAX_CYCLE = BRAKE_CURRENT;  end
	if (SKIIP_BRAKE_CURRENT>SKIIP_BRAKE_CURRENT_MAX_CYCLE) begin SKIIP_BRAKE_CURRENT_MAX_CYCLE = SKIIP_BRAKE_CURRENT; end
	end
	
if (GlobalError==0 || BRAKE_ENABLE==0 || BRAKE_ERROR==1) begin BRAKE_TOP_IGBT = 0; BRAKE_PWM_COUNTER = 0; end
else
	begin
	
	if (BRAKE_PWM_COUNTER>30000) //1000uS cycle, 1000Hz
		begin 
		BRAKE_PWM_COUNTER = 0; 
		end 
	else 
		begin 
		BRAKE_PWM_COUNTER = BRAKE_PWM_COUNTER + 1; 
		end
	
	if (BRAKE_PWM_COUNTER<BRAKE_PWM_WIDTH)		begin	BRAKE_TOP_IGBT = 1;	end
	else begin	BRAKE_TOP_IGBT = 0;	end
	
	end

	if (CHOPPER_ENABLE || BRAKE_ENABLE || (VOUT_FILTER_OUTPUT>100) || (VIN_FILTER_OUTPUT>100))
		begin
		if (FAN_START_COUNTER<50000000) begin FAN_START_COUNTER = FAN_START_COUNTER + 1; end
		else begin DIGITAL_OUTPUT[2] = 1; end //FAN OUTP
		end
	else
		begin
		if (FAN_START_COUNTER>0) begin FAN_START_COUNTER = FAN_START_COUNTER - 1; end
		else 
			begin 
			if ((VIN_FILTER_OUTPUT<50) && (VOUT_FILTER_OUTPUT<50)) begin DIGITAL_OUTPUT[2] = 0; end //FAN OUTP
			end 
		end

	//if (CHOPPER_ENABLE || BRAKE_ENABLE || (VOUT_FILTER_OUTPUT>100) || (VIN_FILTER_OUTPUT>100))
	//begin DIGITAL_OUTPUT[2] = 1;  end //FAN OUTP
	//else begin DIGITAL_OUTPUT[2] = 0; end
	//DIGITAL_OUTPUT[2] = CHOPPER_ENABLE || BRAKE_ENABLE; 

//----------CONTROL AND PWM GENERATION-----------------------------

//-------------------------------DATA IO-------------------------
DATA_TO_PCON[0] = ~GlobalError;
DATA_TO_PCON[1] = BRAKE_ENABLE;
DATA_TO_PCON[2] = CHOPPER_ENABLE;
DATA_TO_PCON[3] = ~BRAKE_ERROR;
DATA_TO_PCON[4] = 0;
DATA_TO_PCON[5] = 0;
DATA_TO_PCON[6] = 0;
DATA_TO_PCON[7] = 0;

DATA_TO_PCON[19:8] = CHOPPER_CURRENT;
DATA_TO_PCON[31:20] = BRAKE_CURRENT_MAX;
DATA_TO_PCON[43:32] = VIN_FILTER_OUTPUT;
DATA_TO_PCON[55:44] = VOUT_FILTER_OUTPUT;
DATA_TO_PCON[67:56] = SKIIP_CHOPP_CURRENT;
DATA_TO_PCON[79:68] = SKIIP_BRAKE_CURRENT_MAX;
DATA_TO_PCON[91:80] = REG_CURRENT_LIMIT;

//DATA_TO_PCON[91:80] = BRAKE_ENERGY_STOCK/10000;

//DATA_TO_PCON[19:8] = CHOPPER_CURRENT;
//DATA_TO_PCON[31:20] = BRAKE_CURRENT;
//DATA_TO_PCON[43:32] = VIN_FILTER_OUTPUT;
//DATA_TO_PCON[55:44] = VOUT_FILTER_OUTPUT;
//DATA_TO_PCON[67:56] = SKIIP_CHOPP_CURRENT;
//DATA_TO_PCON[79:68] = SKIIP_BRAKE_CURRENT;
//DATA_TO_PCON[91:80] = REG_CURRENT_LIMIT;


DATA_TO_PCON[99:92] = SKIIP_CHOPP_TEMP[7:0];
DATA_TO_PCON[107:100] = SKIIP_BRAKE_TEMP[7:0];
DATA_TO_PCON[115:108] = ERROR_LATCH[7:0];
DATA_TO_PCON[123:116] = ERROR_LATCH[15:8];
DATA_TO_PCON[131:124] = ERROR_LATCH[23:16];
DATA_TO_PCON[139:132] = ERROR_FLAGS[7:0];
DATA_TO_PCON[147:140] = ERROR_FLAGS[15:8];
DATA_TO_PCON[155:148] = ERROR_FLAGS[23:16];

CRITICAL_DATA_INPUT = DATA_FROM_PCON[7:0];
FO_INPUT_VAL1 = DATA_FROM_PCON[19:8];
FO_INPUT_VAL2 = DATA_FROM_PCON[31:20];
FO_INPUT_VAL3 = DATA_FROM_PCON[43:32];
FO_INPUT_VAL4 = DATA_FROM_PCON[55:44];
FO_INPUT_VAL5 = DATA_FROM_PCON[67:56];
FO_INPUT_VAL6 = DATA_FROM_PCON[79:68];
FO_INPUT_VAL7 = DATA_FROM_PCON[91:80];


FO_SLOW_INPUT_VAL1 = DATA_FROM_PCON[99:92];
FO_SLOW_INPUT_VAL2 = DATA_FROM_PCON[107:100];
FO_SLOW_INPUT_VAL3 = DATA_FROM_PCON[115:108];
FO_SLOW_INPUT_VAL4 = DATA_FROM_PCON[123:116];
FO_SLOW_INPUT_VAL5 = DATA_FROM_PCON[131:124];
FO_SLOW_INPUT_VAL6 = DATA_FROM_PCON[139:132];
FO_SLOW_INPUT_VAL7 = DATA_FROM_PCON[147:140];
FO_SLOW_INPUT_VAL8 = DATA_FROM_PCON[155:148];

//-------------------------------DATA IO-------------------------

end

endmodule