diff --git a/01_Matlab/99_RecycleBin/BLDCmotorDerating_R2017b.slx b/01_Matlab/99_RecycleBin/BLDCmotorDerating_R2017b.slx new file mode 100644 index 0000000..bf9de56 Binary files /dev/null and b/01_Matlab/99_RecycleBin/BLDCmotorDerating_R2017b.slx differ diff --git a/01_Matlab/99_RecycleBin/bldc_motor_derating.c b/01_Matlab/99_RecycleBin/bldc_motor_derating.c new file mode 100644 index 0000000..bee1d1e --- /dev/null +++ b/01_Matlab/99_RecycleBin/bldc_motor_derating.c @@ -0,0 +1,258 @@ +/* +* This file has been re-implemented with 4 new selectable motor control methods. +* Recommended control method: 3 = Sinusoidal 3rd order. This control method offers superior performanace +* compared to previous method. The new method features: +* ► reduced noise and vibrations +* ► smooth torque output +* ► improved motor efficiency -> lower energy consumption +* +* Copyright (C) 2019 Emanuel FERU +* +* This program is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* This program is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this program. If not, see . +*/ + +#include "stm32f1xx_hal.h" +#include "defines.h" +#include "setup.h" +#include "config.h" + +// Matlab includes and defines - from auto-code generation +// ############################################################################### +#include "BLDC_controller.h" /* Model's header file */ +#include "rtwtypes.h" + +extern RT_MODEL *const rtM_Left; +extern RT_MODEL *const rtM_Right; + +extern DW rtDW_Left; /* Observable states */ +extern ExtU rtU_Left; /* External inputs */ +extern ExtY rtY_Left; /* External outputs */ + +extern DW rtDW_Right; /* Observable states */ +extern ExtU rtU_Right; /* External inputs */ +extern ExtY rtY_Right; /* External outputs */ +// ############################################################################### + + +volatile int pwml = 0; +volatile int pwmr = 0; + +int pwm_limiter = 1024; + +extern volatile adc_buf_t adc_buffer; + +extern volatile uint32_t timeout; + +uint8_t buzzerFreq = 0; +uint8_t buzzerPattern = 0; +static uint32_t buzzerTimer = 0; + +uint8_t enable = 0; + +static const uint16_t pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000 +static float pwml_lim = 0; +static float curl_filt = 0; +float CUR; + +static int offsetcount = 0; +static int offsetrl1 = 2000; +static int offsetrl2 = 2000; +static int offsetrr1 = 2000; +static int offsetrr2 = 2000; +static int offsetdcl = 2000; +static int offsetdcr = 2000; + +float batteryVoltage = BAT_NUMBER_OF_CELLS * 4.0; + +//scan 8 channels with 2ADCs @ 20 clk cycles per sample +//meaning ~80 ADC clock cycles @ 8MHz until new DMA interrupt =~ 100KHz +//=640 cpu cycles +void DMA1_Channel1_IRQHandler(void) { + + DMA1->IFCR = DMA_IFCR_CTCIF1; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); + // HAL_GPIO_TogglePin(LED_PORT, LED_PIN); + + if(offsetcount < 1000) { // calibrate ADC offsets + offsetcount++; + offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2; + offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2; + offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2; + offsetrr2 = (adc_buffer.rr2 + offsetrr2) / 2; + offsetdcl = (adc_buffer.dcl + offsetdcl) / 2; + offsetdcr = (adc_buffer.dcr + offsetdcr) / 2; + return; + } + + if (buzzerTimer % 1000 == 0) { // because you get float rounding errors if it would run every time + batteryVoltage = batteryVoltage * 0.99f + ((float)adc_buffer.batt1 * ((float)BAT_CALIB_REAL_VOLTAGE / (float)BAT_CALIB_ADC)) * 0.01f; + } + + // //disable PWM when current limit is reached (current chopping) + // if(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) { + // LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; + // //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); + // } else { + // LEFT_TIM->BDTR |= TIM_BDTR_MOE; + // //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); + // } + + // if(ABS((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT || timeout > TIMEOUT || enable == 0) { + // RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; + // } else { + // RIGHT_TIM->BDTR |= TIM_BDTR_MOE; + // } + + //disable PWM when current limit is reached (current chopping) + if(timeout > TIMEOUT || enable == 0) { + LEFT_TIM->BDTR &= ~TIM_BDTR_MOE; + //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); + } else { + LEFT_TIM->BDTR |= TIM_BDTR_MOE; + //HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); + } + + if(timeout > TIMEOUT || enable == 0) { + RIGHT_TIM->BDTR &= ~TIM_BDTR_MOE; + } else { + RIGHT_TIM->BDTR |= TIM_BDTR_MOE; + } + + // pwml_lim = pwml_lim + 0.001f * (ABS((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP) - DC_CUR_LIMIT); + // pwml_lim = CLAMP(pwml_lim, 0, ABS(pwmr)); + // pwmr = (int) (pwmr - SIGN(pwmr) * pwml_lim); + + // if (ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT) + // pwml_lim = pwml_lim + 0.00001f; + // else + // pwml_lim = pwml_lim - 0.00001f; (pwml_lim * (1.0f - FILTER) + cmd2 * FILTER) + + float CUR_FILTER = 0.1f; // 0.00001f; + curl_filt = ((100.0f - CUR_FILTER) * curl_filt + CUR_FILTER * ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP)) / 100; + // curl_filt = ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP); + // pwml_lim = pwml_lim + 1 * (curl_filt - DC_CUR_LIMIT); + pwml_lim = 2 * (curl_filt - DC_CUR_LIMIT); + pwml_lim = CLAMP(pwml_lim, 0, ABS(pwml)); + pwml = (int) (pwml - SIGN(pwml) * pwml_lim); + + // if(curl_filt > DC_CUR_LIMIT) { + // pwml_lim+=0.1f; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); // Derating active + // } else { + // pwml_lim-=0.1f; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); // Derating deactive + // } + // pwml_lim = CLAMP(pwml_lim/1000, 0, ABS(pwml)); + // pwml = (int) (pwml - SIGN(pwml) * pwml_lim); + + + // DC_CUR = MAX(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP), ABS((adc_buffer.dcr - offsetdcr) * MOTOR_AMP_CONV_DC_AMP)); + +// CUR = curl_filt; + CUR = (adc_buffer.rl1 - offsetrl1) * MOTOR_AMP_CONV_DC_AMP; + // if(ABS((adc_buffer.dcl - offsetdcl) * MOTOR_AMP_CONV_DC_AMP) > DC_CUR_LIMIT) { + // pwm_limiter--; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1); // Derating active + // } else { + // pwm_limiter++; + // HAL_GPIO_WritePin(LED_PORT, LED_PIN, 0); // Derating deactive + // } + // pwm_limiter = CLAMP(pwm_limiter, 0, 1024); + // pwml = (pwm_limiter*pwml) >> 10; + // pwmr = CLAMP(pwmr, -pwm_lim, pwm_lim); + + //create square wave for buzzer + buzzerTimer++; + if (buzzerFreq != 0 && (buzzerTimer / 5000) % (buzzerPattern + 1) == 0) { + if (buzzerTimer % buzzerFreq == 0) { + HAL_GPIO_TogglePin(BUZZER_PORT, BUZZER_PIN); + } + } else { + HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0); + } + + // ############################### MOTOR CONTROL ############################### + + static boolean_T OverrunFlag = false; + /* Check for overrun */ + if (OverrunFlag) { + return; + } + OverrunFlag = true; + + int ul, vl, wl; + int ur, vr, wr; + // ========================= LEFT MOTOR ============================ + // Get hall sensors values + uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN); + uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN); + uint8_t hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN); + + /* Set motor inputs here */ + rtU_Left.b_hallA = hall_ul; + rtU_Left.b_hallB = hall_vl; + rtU_Left.b_hallC = hall_wl; + rtU_Left.r_DC = pwml; + + /* Step the controller */ + BLDC_controller_step(rtM_Left); + + /* Get motor outputs here */ + ul = rtY_Left.DC_phaA; + vl = rtY_Left.DC_phaB; + wl = rtY_Left.DC_phaC; + // motSpeedLeft = rtY_Left.n_mot; + // motAngleLeft = rtY_Left.a_elecAngle; + + /* Apply commands */ + LEFT_TIM->LEFT_TIM_U = (uint16_t)CLAMP(ul + pwm_res / 2, 10, pwm_res-10); + LEFT_TIM->LEFT_TIM_V = (uint16_t)CLAMP(vl + pwm_res / 2, 10, pwm_res-10); + LEFT_TIM->LEFT_TIM_W = (uint16_t)CLAMP(wl + pwm_res / 2, 10, pwm_res-10); + // ================================================================= + + + // ========================= RIGHT MOTOR =========================== + // Get hall sensors values + uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN); + uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN); + uint8_t hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN); + + /* Set motor inputs here */ + rtU_Right.b_hallA = hall_ur; + rtU_Right.b_hallB = hall_vr; + rtU_Right.b_hallC = hall_wr; + rtU_Right.r_DC = pwmr; + + /* Step the controller */ + BLDC_controller_step(rtM_Right); + + /* Get motor outputs here */ + ur = rtY_Right.DC_phaA; + vr = rtY_Right.DC_phaB; + wr = rtY_Right.DC_phaC; + // motSpeedRight = rtY_Right.n_mot; + // motAngleRight = rtY_Right.a_elecAngle; + + /* Apply commands */ + RIGHT_TIM->RIGHT_TIM_U = (uint16_t)CLAMP(ur + pwm_res / 2, 10, pwm_res-10); + RIGHT_TIM->RIGHT_TIM_V = (uint16_t)CLAMP(vr + pwm_res / 2, 10, pwm_res-10); + RIGHT_TIM->RIGHT_TIM_W = (uint16_t)CLAMP(wr + pwm_res / 2, 10, pwm_res-10); + // ================================================================= + + /* Indicate task complete */ + OverrunFlag = false; + + // ############################################################################### + +} diff --git a/01_Matlab/99_RecycleBin/init_model_derating.m b/01_Matlab/99_RecycleBin/init_model_derating.m new file mode 100644 index 0000000..a31c456 --- /dev/null +++ b/01_Matlab/99_RecycleBin/init_model_derating.m @@ -0,0 +1,15 @@ + + +Ts_ctrl = 6e-5; % [s] Controller samplid time (~16 kHz) +% Ts_ctrl = 12e-5; % [s] Controller samplid time (~8 kHz) +f_ctrl = 1/Ts_ctrl; % [Hz] Controller frequency = 1/Ts_ctrl + +%% Motor Current Derating Parameters +PWM_LIM_MAX = 1000; +t_DC_CUR_DER = 2000; % [ms] +t_DC_CUR_REC = 2000; +DC_CUR_LIM_CONT = 15; +DC_CUR_LIM_PEAK = 20; + +dt_curLimDer = PWM_LIM_MAX / t_DC_CUR_DER / (f_ctrl / 1000); +dt_curLimRec = - PWM_LIM_MAX / t_DC_CUR_REC / (f_ctrl / 1000); \ No newline at end of file