pavel/FOEXP_T
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity
0fb6587633
FOEXP_T/FOExp.v
irobo 0fb6587633 added test for T-1531Z
2026-06-12 20:55:23 +03:00

1110 lines
33 KiB
Verilog
Raw Blame History

// Unit name to specify here - UNIT1, UNIT2, UNIT3 or UNIT4
`define UNIT4
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_EN = 4'b0000;
//assign SKiiP_CTRL = SKiiP_CTRL_BUF;
assign SKiiP_CTRL = 4'b0000;
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] = 0; //TEMPLOCK 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] = 1707; //TEMPLOCK U2!!!!
SKIIP_BRAKE_TEMP_ADC[11:0] = 1707; //TEMPLOCK 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
Reference in New Issue View Git Blame Copy Permalink