forked from lucysrausch/hoverboard-firmware-hack
Added some back-up files
This commit is contained in:
EmanuelFeru
BIN
01_Matlab/99_RecycleBin/BLDCmotorDerating_R2017b.slx
Normal file
BIN
01_Matlab/99_RecycleBin/BLDCmotorDerating_R2017b.slx
Normal file
Binary file not shown.
258
01_Matlab/99_RecycleBin/bldc_motor_derating.c
Normal file
258
01_Matlab/99_RecycleBin/bldc_motor_derating.c
Normal file
@ -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 <aerdronix@gmail.com>
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
|
||||
// ###############################################################################
|
||||
|
||||
}
|
||||
15
01_Matlab/99_RecycleBin/init_model_derating.m
Normal file
15
01_Matlab/99_RecycleBin/init_model_derating.m
Normal file
@ -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);
|
||||
Reference in New Issue
Block a user