From 1fc237d550ba553b78d6f0587d8d1d988f9f4ab1 Mon Sep 17 00:00:00 2001 From: irobo Date: Thu, 11 Jun 2026 16:30:05 +0300 Subject: [PATCH] added files --- FOExp.v | 1037 ++++++++++++++++++++++++++++++++++++++++++++++++ PCON_DATA_IO.v | 461 +++++++++++++++++++++ 2 files changed, 1498 insertions(+) create mode 100644 FOExp.v create mode 100644 PCON_DATA_IO.v diff --git a/FOExp.v b/FOExp.v new file mode 100644 index 0000000..0ee0771 --- /dev/null +++ b/FOExp.v @@ -0,0 +1,1037 @@ +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]; +ERROR_FLAGS[1] = BRAKE_CURR_ERR[0]|BRAKE_CURR_ERR[1]; //NORMAL U1,2,4!!!! +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]; +ERROR_FLAGS[5] = SKIIP_BRAKE_ERR[0]|SKIIP_BRAKE_ERR[1]; +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; +ERROR_FLAGS[10] = (~INP_1V2OK)|(~INP_2V5OK)|INP_3V3OK; //NORMAL U1,2,3!!!! +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] = 0; + SKiiP_CTRL_BUF[4] = 0; + 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] = 0; + SKiiP_CTRL_BUF[2] = 0; + 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]; + + BRAKE_CURRENT_ADC[14:0] <= BRAKE_CURR_IN[14:0]; //NORMAL U1,2,4!!!! + + 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; + + SKIIP_BRAKE_CURRENT_ADC[11:0] <= SKIIP_BRAKE_CURRENT_IN; //NORMAL U3,4!!!! + SKIIP_BRAKE_TEMP_ADC[11:0] <= SKIIP_BRAKE_TEMP_IN; //NORMAL U3,4!!!! + + 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 + + if (CHOPPER_CURRENT_ADC>=16383) begin CHOPPER_CURRENT_ADC = CHOPPER_CURRENT_ADC - 16383; end //U1=16585 //U2 = 16560 //U3 = 16590 //U4 = 16580 + else begin CHOPPER_CURRENT_ADC = 16383 - CHOPPER_CURRENT_ADC; end + + if (BRAKE_CURRENT_ADC>=16383) begin BRAKE_CURRENT_ADC = BRAKE_CURRENT_ADC - 16383; end + else begin BRAKE_CURRENT_ADC = 16383 - BRAKE_CURRENT_ADC; end + + 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>1200) || (SKIIP_CHOPP_CURRENT>1200))) + || (CHOPPER_CURRENT>1800) + || (SKIIP_CHOPP_CURRENT>1800) + ) begin CHOPP_OVERCURRENT_ERR = 1; end +else begin CHOPP_OVERCURRENT_ERR = 0; end + +if ( + (BRAKE_ENABLE==1 && ((BRAKE_CURRENT>2200) || (SKIIP_BRAKE_CURRENT>1400))) + || (BRAKE_CURRENT>2200) + || (SKIIP_BRAKE_CURRENT>2200) + ) 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; + + REG_PROP = 1; //A/V + REG_INT = 9; //~x0.1 10 = 10 * 10000/1024 = 95 A/V/s = 9500A/100V/s = 950A/100ms + +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_OUTPUTSKIIP_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_CURRENT200) + 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_COUNTER100) || (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 \ No newline at end of file diff --git a/PCON_DATA_IO.v b/PCON_DATA_IO.v new file mode 100644 index 0000000..9fa187c --- /dev/null +++ b/PCON_DATA_IO.v @@ -0,0 +1,461 @@ +module PCON_DATA_IO (clockin, FO_INPUT, FO_OUTP, DATA_TO_SEND, DATA_RECIEVED, DATA_CLOCK, IO_ERR); + +input FO_INPUT; +input clockin; +input [155:0] DATA_TO_SEND; + +output FO_OUTP; +output [155:0] DATA_RECIEVED; +output DATA_CLOCK; +output [2:0] IO_ERR; + +reg [7:0] FO_INPUT_CRITICAL_DATA; +reg [95:0] FO_INPUT_REGULAR_DATA; + +reg [15:0] TRANSMITT_DATA_VALUE; +reg [7:0] TX_SM_COUNTER; +reg [4:0] TX_SM_STATE; +reg [4:0] TX_BIT_COUNTER; +reg [4:0] TX_VAL_NUM_COUNTER; +reg [4:0] TX_SLOW_VAL_NUM_COUNTER; + + +reg [155:0] DATA_OUTPUT_VALUE; + +reg FO_OUTPUT; +reg DATA_READY; +reg ERROR_FLAG; +reg CRITICAL_ERROR_FLAG; +reg NOTRANSMITTION_FAULT_FLAG; +reg [2:0] ERROR_OUTPUT; +reg [15:0] NOCHANGE_COUNTER; +reg ERROR_FLAG_COUNTER; +reg ERROR_FLAG_COUNTER_PREV; +reg CRITICAL_ERROR_FLAG_COUNTER; + +reg FO_INPUT_REG; +reg FO_INPUT_PREV; +reg [63:0] RX_DATA_READ_VALUE; +reg [11:0] RX_DATA_VALUE; +reg [11:0] RX_DATA_CHECK_VALUE; +reg [7:0] RX_CRITICAL_DATA_VALUE; +reg [7:0] RX_CRITICAL_DATA_CHECK_VALUE; +reg [3:0] RX_DATA_NUM; +reg [3:0] RX_DATA_NUM_CHECK; +reg [3:0] RX_SLOW_VAL_NUM_COUNTER; + +reg [15:0] RX_SM_STATE; +reg [15:0] RX_SM_COUNTER; +reg [5:0] RX_BIT_COUNTER; +reg [5:0] RX_BIT_COUNTER_OLD; +reg [15:0] RX_POSVAL_COUNTER; +reg [15:0] RX_NEGVAL_COUNTER; +reg [15:0] RX_CHECKVAL_COUNTER; + +//assign FO_OUTP = ~FO_OUTPUT; //NORMAL +assign FO_OUTP = FO_OUTPUT; //TEMP INVERSE!!!!!!! +assign DATA_RECIEVED = DATA_OUTPUT_VALUE; +assign DATA_CLOCK = DATA_READY; +assign IO_ERR = ERROR_OUTPUT; + +initial +begin +TX_SM_COUNTER = 0; +TX_SM_STATE = 0; +TX_BIT_COUNTER=0; +TX_VAL_NUM_COUNTER=0; +end + +always@(posedge clockin) +begin + +//TRANSMITT -------------------------------------------------- + +if (TX_SM_STATE==0) //Generate start signal + begin + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + if (TX_SM_COUNTER>75) + begin + TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[7:0]; + FO_OUTPUT <= 0; + TX_SM_STATE <= 1; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + else + begin + FO_OUTPUT<=0; + end + + end +else if (TX_SM_STATE==1) //Transmitt critical data + begin + + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + if (TX_SM_COUNTER==28) + begin + TX_SM_COUNTER<=0; + TX_BIT_COUNTER <= TX_BIT_COUNTER+1; + if (TX_BIT_COUNTER==7) + begin + FO_OUTPUT <= 0; + TX_SM_STATE <= 2; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + end + if (TX_SM_COUNTER==0) + begin + FO_OUTPUT <= 1; + end + if (TRANSMITT_DATA_VALUE[TX_BIT_COUNTER]==0) + begin + if (TX_SM_COUNTER==8) + begin + FO_OUTPUT<=0; + end + end + else + begin + if (TX_SM_COUNTER==20) + begin + FO_OUTPUT<=0; + end + end + + end +else if (TX_SM_STATE==2) //Repeat critical data transmittion + begin + + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + if (TX_SM_COUNTER==28) + begin + TX_SM_COUNTER<=0; + TX_BIT_COUNTER <= TX_BIT_COUNTER+1; + if (TX_BIT_COUNTER==7) + begin + if (TX_VAL_NUM_COUNTER==0) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[19:8]; end + else if (TX_VAL_NUM_COUNTER==1) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[31:20]; end + else if (TX_VAL_NUM_COUNTER==2) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[43:32]; end + else if (TX_VAL_NUM_COUNTER==3) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[55:44]; end + else if (TX_VAL_NUM_COUNTER==4) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[67:56]; end + else if (TX_VAL_NUM_COUNTER==5) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[79:68]; end + else if (TX_VAL_NUM_COUNTER==6) + begin TRANSMITT_DATA_VALUE[11:0] <= DATA_TO_SEND[91:80]; end + else if (TX_VAL_NUM_COUNTER==7) + begin + TRANSMITT_DATA_VALUE[11:8] = TX_SLOW_VAL_NUM_COUNTER; + if (TX_SLOW_VAL_NUM_COUNTER==0) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[99:92]; end + if (TX_SLOW_VAL_NUM_COUNTER==1) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[107:100]; end + if (TX_SLOW_VAL_NUM_COUNTER==2) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[115:108]; end + if (TX_SLOW_VAL_NUM_COUNTER==3) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[123:116]; end + if (TX_SLOW_VAL_NUM_COUNTER==4) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[131:124]; end + if (TX_SLOW_VAL_NUM_COUNTER==5) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[139:132]; end + if (TX_SLOW_VAL_NUM_COUNTER==6) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[147:140]; end + if (TX_SLOW_VAL_NUM_COUNTER==7) begin TRANSMITT_DATA_VALUE[7:0] <= DATA_TO_SEND[155:148]; end + end + + TRANSMITT_DATA_VALUE[15:12] <= TX_VAL_NUM_COUNTER; + FO_OUTPUT <= 1; + TX_SM_STATE <= 3; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + end + if (TX_SM_COUNTER==0) + begin + FO_OUTPUT <= 1; + end + if (TRANSMITT_DATA_VALUE[TX_BIT_COUNTER]==0) + begin + if (TX_SM_COUNTER==8) + begin + FO_OUTPUT<=0; + end + end + else + begin + if (TX_SM_COUNTER==20) + begin + FO_OUTPUT<=0; + end + end + end +else if (TX_SM_STATE==3) //Transmitt regular data + begin + + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + if (TX_SM_COUNTER==28) + begin + TX_SM_COUNTER<=0; + TX_BIT_COUNTER <= TX_BIT_COUNTER+1; + if (TX_BIT_COUNTER==15) + begin + FO_OUTPUT <= 1; + TX_SM_STATE <= 4; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + end + if (TX_SM_COUNTER==0) + begin + FO_OUTPUT <= 1; + end + if (TRANSMITT_DATA_VALUE[TX_BIT_COUNTER]==0) + begin + if (TX_SM_COUNTER==8) + begin + FO_OUTPUT<=0; + end + end + else + begin + if (TX_SM_COUNTER==20) + begin + FO_OUTPUT<=0; + end + end + end +else if (TX_SM_STATE==4) //Repeat regular data transmittion + begin + + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + if (TX_SM_COUNTER==28) + begin + TX_SM_COUNTER<=0; + TX_BIT_COUNTER <= TX_BIT_COUNTER+1; + if (TX_BIT_COUNTER==15) + begin + FO_OUTPUT <= 1; + TX_SM_STATE <= 5; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + end + if (TX_SM_COUNTER==0) + begin + FO_OUTPUT <= 1; + end + if (TRANSMITT_DATA_VALUE[TX_BIT_COUNTER]==0) + begin + if (TX_SM_COUNTER==8) + begin + FO_OUTPUT<=0; + end + end + else + begin + if (TX_SM_COUNTER==20) + begin + FO_OUTPUT<=0; + end + end + end +else if (TX_SM_STATE==5) //End pulse generation + begin + TX_SM_COUNTER <= TX_SM_COUNTER + 1; + FO_OUTPUT<=1; + if (TX_SM_COUNTER==28) + begin + if (TX_VAL_NUM_COUNTER==7) + begin + if (TX_SLOW_VAL_NUM_COUNTER<7) begin TX_SLOW_VAL_NUM_COUNTER = TX_SLOW_VAL_NUM_COUNTER + 1; end + else begin TX_SLOW_VAL_NUM_COUNTER=0; end + TX_VAL_NUM_COUNTER = 0; + end + else + begin + TX_VAL_NUM_COUNTER = TX_VAL_NUM_COUNTER + 1; + end + FO_OUTPUT <= 0; + TX_SM_STATE <= 0; + TX_SM_COUNTER <= 0; + TX_BIT_COUNTER <= 0; + end + end +//TRANSMITT END----------------------------------------------- + + +//RECIEVE ---------------------------------------------------- +FO_INPUT_REG = FO_INPUT; +RX_SM_COUNTER = RX_SM_COUNTER + 1; + +ERROR_OUTPUT[0] = NOTRANSMITTION_FAULT_FLAG; +ERROR_OUTPUT[1] = CRITICAL_ERROR_FLAG; +ERROR_OUTPUT[2] = ERROR_FLAG; + +if (FO_INPUT_PREV!=FO_INPUT_REG) + begin + NOCHANGE_COUNTER = 0; + NOTRANSMITTION_FAULT_FLAG = 0; + if (FO_INPUT_PREV==0) + begin + if (RX_SM_COUNTER<3) + begin + if (RX_CHECKVAL_COUNTER<14) + begin + RX_DATA_READ_VALUE[RX_BIT_COUNTER] = 1; + end + else + begin + RX_DATA_READ_VALUE[RX_BIT_COUNTER] = 0; + end + + end + if (RX_SM_COUNTER>3) + begin + RX_POSVAL_COUNTER = 0; + RX_NEGVAL_COUNTER = 0; + RX_BIT_COUNTER_OLD = RX_BIT_COUNTER; + end + + if (RX_SM_COUNTER>4) + begin + FO_INPUT_PREV = FO_INPUT_REG; + end + end + else + begin + RX_CRITICAL_DATA_VALUE = RX_DATA_READ_VALUE[8:1]; + RX_CRITICAL_DATA_CHECK_VALUE = RX_DATA_READ_VALUE[16:9]; + RX_DATA_VALUE = RX_DATA_READ_VALUE[28:17]; + RX_DATA_NUM = RX_DATA_READ_VALUE[32:29]; + RX_DATA_CHECK_VALUE = RX_DATA_READ_VALUE[44:33]; + RX_DATA_NUM_CHECK = RX_DATA_READ_VALUE[48:45]; + RX_BIT_COUNTER = RX_BIT_COUNTER_OLD + 1; + RX_SLOW_VAL_NUM_COUNTER = RX_DATA_READ_VALUE[28:25]; + + if (RX_SM_COUNTER>4) + begin + FO_INPUT_PREV = FO_INPUT_REG; + end + end + end +else + begin + RX_SM_COUNTER = 0; + if (NOCHANGE_COUNTER>250) + begin + NOTRANSMITTION_FAULT_FLAG = 1; + end + else + begin + NOCHANGE_COUNTER = NOCHANGE_COUNTER + 1; + end + end + +if (FO_INPUT_REG==1) + begin + RX_POSVAL_COUNTER = RX_POSVAL_COUNTER + 1; + RX_NEGVAL_COUNTER = RX_NEGVAL_COUNTER; + RX_CHECKVAL_COUNTER = RX_CHECKVAL_COUNTER; + end +else + begin + RX_NEGVAL_COUNTER = RX_NEGVAL_COUNTER + 1; + RX_POSVAL_COUNTER = RX_POSVAL_COUNTER; + RX_CHECKVAL_COUNTER = RX_NEGVAL_COUNTER; + end + +if (RX_NEGVAL_COUNTER>50 && RX_NEGVAL_COUNTER<55) + begin + RX_BIT_COUNTER = 0; + + if (RX_CRITICAL_DATA_VALUE==RX_CRITICAL_DATA_CHECK_VALUE) + begin + DATA_OUTPUT_VALUE[7:0] = RX_CRITICAL_DATA_VALUE; + CRITICAL_ERROR_FLAG = 0; + CRITICAL_ERROR_FLAG_COUNTER=0; + end + else + begin + if (CRITICAL_ERROR_FLAG_COUNTER==1) + begin + CRITICAL_ERROR_FLAG = 1; + DATA_OUTPUT_VALUE[7:0] = 0; + end + else + begin + CRITICAL_ERROR_FLAG_COUNTER = 1; + DATA_OUTPUT_VALUE[7:0] = DATA_OUTPUT_VALUE[7:0]; + end + end + + + if ((RX_DATA_VALUE==RX_DATA_CHECK_VALUE) && (RX_DATA_NUM==RX_DATA_NUM_CHECK)) + begin + if (RX_DATA_NUM==0) + begin + DATA_OUTPUT_VALUE[19:8] = RX_DATA_VALUE; + ERROR_FLAG_COUNTER = 0; + end + else if (RX_DATA_NUM==1) + begin DATA_OUTPUT_VALUE[31:20] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==2) + begin DATA_OUTPUT_VALUE[43:32] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==3) + begin DATA_OUTPUT_VALUE[55:44] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==4) + begin DATA_OUTPUT_VALUE[67:56] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==5) + begin DATA_OUTPUT_VALUE[79:68] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==6) + begin DATA_OUTPUT_VALUE[91:80] = RX_DATA_VALUE; end + else if (RX_DATA_NUM==7) + begin + if (RX_SLOW_VAL_NUM_COUNTER==0) begin DATA_OUTPUT_VALUE[99:92] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==1) begin DATA_OUTPUT_VALUE[107:100] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==2) begin DATA_OUTPUT_VALUE[115:108] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==3) begin DATA_OUTPUT_VALUE[123:116] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==4) begin DATA_OUTPUT_VALUE[131:124] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==5) begin DATA_OUTPUT_VALUE[139:132] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==6) begin DATA_OUTPUT_VALUE[147:140] = RX_DATA_VALUE[7:0]; end + if (RX_SLOW_VAL_NUM_COUNTER==7) begin DATA_OUTPUT_VALUE[155:148] = RX_DATA_VALUE[7:0]; end + if (ERROR_FLAG_COUNTER==0 && ERROR_FLAG_COUNTER_PREV==0) + begin + ERROR_FLAG = 0; + end + else + begin + ERROR_FLAG_COUNTER_PREV = ERROR_FLAG_COUNTER; + end + + end + end + else + begin + if (ERROR_FLAG_COUNTER_PREV==1) + begin + ERROR_FLAG = 1; + end + else + begin + ERROR_FLAG_COUNTER = 1; + end + end + + end + +if (RX_NEGVAL_COUNTER>55) + begin + RX_CRITICAL_DATA_VALUE = 0; + RX_CRITICAL_DATA_CHECK_VALUE = 0; + RX_DATA_VALUE = 0; + RX_DATA_NUM = 0; + RX_DATA_CHECK_VALUE = 0; + RX_DATA_NUM_CHECK = 0; + RX_DATA_READ_VALUE = 0; + end +//RECIEVE END------------------------------------------------- + + +end + + +endmodule