diff --git a/FOExp.v b/FOExp.v index 77b1e53..4945e8c 100644 --- a/FOExp.v +++ b/FOExp.v @@ -467,60 +467,68 @@ else if (CALC_SM_STATE==1) 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 + begin + CALC_SM_COUNTER = CALC_SM_COUNTER + 1; + if (CALC_SM_COUNTER==1) + begin + + // Калибровка тока чоппера для блока UNIT1: + // вычитаем смещение 16585 или берём модуль отклонения относительно этой точки + `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 + + // Калибровка тока чоппера для блока UNIT4: + // аналогично UNIT1, но используется своё смещение 16580 + `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 + + // Для UNIT4 тормозной ток BRAKE_CURRENT_ADC полностью игнорируется: + // принудительно обнуляется перед дальнейшими расчётами + `ifdef UNIT4 + BRAKE_CURRENT_ADC = 0; + // Для всех остальных блоков (UNIT1, UNIT2, UNIT3) тормозной ток калибруется: + // вычитаем смещение 16560 или берём модуль отклонения относительно этой точки + `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; @@ -714,30 +722,34 @@ 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; + 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 + // Настройка ПИ-регулятора для UNIT1: + // REG_PROP — пропорциональный коэффициент, REG_INT — интегральный + `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 + + // Настройка ПИ-регулятора для UNIT4: + // немного другое интегральное усиление (12) под конкретный модуль + `ifdef UNIT4 + REG_PROP = 1; //A/V + REG_INT = 12; // + `endif if (CHOPPER_ENABLE==1 && GlobalError==0)