修正电流限幅
This commit is contained in:
parent
721796e105
commit
58e529da77
16
.cproject
16
.cproject
@ -23,7 +23,7 @@
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.1984140111" name="Floating-point unit" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1267087860" name="Floating-point ABI" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1450558436" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" value="genericBoard" valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1879587253" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G431VBTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../USB_Device/App | ../USB_Device/Target | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include || || || USE_HAL_DRIVER | STM32G431xx || || USB_Device | Drivers | Core/Startup | Middlewares | Core || || || ${workspace_loc:/${ProjName}/STM32G431VBTX_FLASH.ld} || true || NonSecure || || secure_nsclib.o || || None || || || " valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.1879587253" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G431VBTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include || || || USE_HAL_DRIVER | STM32G431xx || || Drivers | Core/Startup | Core || || || ${workspace_loc:/${ProjName}/STM32G431VBTX_FLASH.ld} || true || NonSecure || || secure_nsclib.o || || None || || || " valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.703046927" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" value="168" valueType="string"/>
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1574791951" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
|
||||
<builder buildPath="${workspace_loc:/BoooomFOC_STSPIN32G4_EVB}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1456362012" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
|
||||
@ -44,12 +44,8 @@
|
||||
</option>
|
||||
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.373873462" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" valueType="includePath">
|
||||
<listOptionValue builtIn="false" value="../Core/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../USB_Device/App"/>
|
||||
<listOptionValue builtIn="false" value="../USB_Device/Target"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/STM32G4xx_HAL_Driver/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy"/>
|
||||
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32G4xx/Include"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
|
||||
</option>
|
||||
@ -78,9 +74,7 @@
|
||||
</toolChain>
|
||||
</folderInfo>
|
||||
<sourceEntries>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="USB_Device"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
|
||||
</sourceEntries>
|
||||
</configuration>
|
||||
@ -109,7 +103,7 @@
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.492494204" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.fpu.value.fpv4-sp-d16" valueType="enumerated"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.1425506387" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.floatabi.value.hard" valueType="enumerated"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1498232860" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" value="genericBoard" valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.268333874" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G431VBTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../USB_Device/App | ../USB_Device/Target | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc | ../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include || || || USE_HAL_DRIVER | STM32G431xx || || USB_Device | Drivers | Core/Startup | Middlewares | Core || || || ${workspace_loc:/${ProjName}/STM32G431VBTX_FLASH.ld} || true || NonSecure || || secure_nsclib.o || || None || || || " valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.268333874" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G431VBTx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc | ../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G4xx/Include | ../Drivers/CMSIS/Include || || || USE_HAL_DRIVER | STM32G431xx || || Drivers | Core/Startup | Core || || || ${workspace_loc:/${ProjName}/STM32G431VBTX_FLASH.ld} || true || NonSecure || || secure_nsclib.o || || None || || || " valueType="string"/>
|
||||
<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1724099006" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" value="168" valueType="string"/>
|
||||
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.1284979115" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
|
||||
<builder buildPath="${workspace_loc:/BoooomFOC_STSPIN32G4_EVB}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1943144139" managedBuildOn="true" name="Gnu Make Builder.Release" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
|
||||
@ -126,12 +120,8 @@
|
||||
</option>
|
||||
<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths.2026911592" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.option.includepaths" valueType="includePath">
|
||||
<listOptionValue builtIn="false" value="../Core/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../USB_Device/App"/>
|
||||
<listOptionValue builtIn="false" value="../USB_Device/Target"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/STM32G4xx_HAL_Driver/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/STM32G4xx_HAL_Driver/Inc/Legacy"/>
|
||||
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Core/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/CMSIS/Device/ST/STM32G4xx/Include"/>
|
||||
<listOptionValue builtIn="false" value="../Drivers/CMSIS/Include"/>
|
||||
</option>
|
||||
@ -160,9 +150,7 @@
|
||||
</toolChain>
|
||||
</folderInfo>
|
||||
<sourceEntries>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="USB_Device"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
|
||||
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
|
||||
</sourceEntries>
|
||||
</configuration>
|
||||
|
7
.idea/codeStyles/Project.xml
Normal file
7
.idea/codeStyles/Project.xml
Normal file
@ -0,0 +1,7 @@
|
||||
<component name="ProjectCodeStyleConfiguration">
|
||||
<code_scheme name="Project" version="173">
|
||||
<clangFormatSettings>
|
||||
<option name="ENABLED" value="true" />
|
||||
</clangFormatSettings>
|
||||
</code_scheme>
|
||||
</component>
|
5
.idea/codeStyles/codeStyleConfig.xml
Normal file
5
.idea/codeStyles/codeStyleConfig.xml
Normal file
@ -0,0 +1,5 @@
|
||||
<component name="ProjectCodeStyleConfiguration">
|
||||
<state>
|
||||
<option name="USE_PER_PROJECT_SETTINGS" value="true" />
|
||||
</state>
|
||||
</component>
|
40
.mxproject
40
.mxproject
File diff suppressed because one or more lines are too long
238
APP/APP_Main.c
238
APP/APP_Main.c
@ -3,141 +3,165 @@
|
||||
//
|
||||
|
||||
#include "APP_Main.h"
|
||||
#include "APP_Task.h"
|
||||
#include "Controller/Controller.h"
|
||||
#include "PreDrive.h"
|
||||
#include "controller.h"
|
||||
#include "encoder.h"
|
||||
#include "foc.h"
|
||||
#include "pwm_curr.h"
|
||||
#include "usr_config.h"
|
||||
|
||||
float32_t NowAngle;
|
||||
tData Data;
|
||||
|
||||
void APP_Init()
|
||||
{
|
||||
volatile uint32_t SystickCount = 0;
|
||||
//uint8_t RxBuffer[2];//接收数据
|
||||
//float32_t NowAngle;
|
||||
uint8_t USART1_IRQHandler_Status;
|
||||
|
||||
void APP_Init() {
|
||||
USR_CONFIG_set_default_config();
|
||||
USR_CONFIG_read_config();
|
||||
|
||||
__HAL_SPI_ENABLE(&hspi1);
|
||||
HAL_Delay(10);
|
||||
Data.Angle.EncoderModel = MT6816;
|
||||
Data_Init(&Data);
|
||||
// NowAngle = Data.Angle.getAngle();
|
||||
// Data.Angle.EncoderModel = MT6816;
|
||||
// Data_Init(&Data);
|
||||
// NowAngle = Data.Angle.getAngle();
|
||||
|
||||
|
||||
|
||||
|
||||
//开启内部运放
|
||||
//<2F>???启内部运<E983A8>???
|
||||
HAL_OPAMP_Start(&hopamp1);
|
||||
HAL_OPAMP_Start(&hopamp2);
|
||||
HAL_OPAMP_Start(&hopamp3);
|
||||
HAL_Delay(10);
|
||||
|
||||
while (InteriorADC_Init());
|
||||
// while (InteriorADC_Init())
|
||||
HAL_ADC_Start_DMA(&hadc1, (uint32_t *) adc1_RegularBuf, 2);
|
||||
|
||||
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
|
||||
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
|
||||
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
|
||||
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
|
||||
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
|
||||
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
|
||||
HAL_Delay(10);
|
||||
__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
|
||||
HAL_Delay(10);
|
||||
// HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
|
||||
// HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
|
||||
// HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
|
||||
// HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
|
||||
// HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
|
||||
// HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
|
||||
// HAL_Delay(10);
|
||||
// __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
|
||||
// HAL_Delay(10);
|
||||
|
||||
|
||||
PreDrive_Init();
|
||||
GPIOE->BSRR = 1 << 7;
|
||||
HAL_Delay(10);
|
||||
// PreDrive_Init();
|
||||
// GPIOE->BSRR = 1 << 7;
|
||||
// HAL_Delay(10);
|
||||
|
||||
TIM1->CCR1 = 0;
|
||||
TIM1->CCR2 = 0;
|
||||
TIM1->CCR3 = 0;
|
||||
HAL_Delay(10);
|
||||
|
||||
MCT_init();
|
||||
FOC_init();
|
||||
PWMC_init();
|
||||
ENCODER_init();
|
||||
CONTROLLER_init();
|
||||
|
||||
__HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_JEOS);//关闭ADC1的中断,避免ADC1_2_IRQHandler触发两次
|
||||
__HAL_ADC_DISABLE_IT(&hadc2, ADC_IT_JEOS);//关闭ADC1的中断,避免ADC1_2_IRQHandler触发两次
|
||||
|
||||
for (uint8_t i = 0, j = 0; i < 250; i++) {
|
||||
if (Foc.v_bus_filt > 20) {
|
||||
if (++j > 20) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
HAL_Delay(2);
|
||||
}
|
||||
|
||||
while (PWMC_CurrentReadingPolarization()) {
|
||||
StatuswordNew.errors.adc_selftest_fatal = 1;
|
||||
}
|
||||
|
||||
|
||||
__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
|
||||
__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOS);//关闭ADC1的中断,避免ADC1_2_IRQHandler触发两次
|
||||
|
||||
// MCT_set_state(IDLE);
|
||||
}
|
||||
|
||||
uint16_t ADC_VAL1, ADC_VAL2;
|
||||
|
||||
//MCU内部温度传感器
|
||||
#define TS_CAL1 ((uint16_t*) ((uint32_t) 0x1FFF75A8)) // 30摄氏度时的MCU内部温度传感器校准值
|
||||
#define TS_CAL2 ((uint16_t*) ((uint32_t) 0x1FFF75CA)) //110摄氏度时的MCU内部温度传感器校准值
|
||||
|
||||
/*滑动平均滤波器长度*/
|
||||
#define MVF_LENGTH 128
|
||||
|
||||
float moving_average_filtre(float xn) {
|
||||
static int index = -1;
|
||||
static float buffer[MVF_LENGTH];
|
||||
static float sum = 0;
|
||||
float yn = 0;
|
||||
int i = 0;
|
||||
if (index == -1) {
|
||||
//初始化
|
||||
for (i = 0; i < MVF_LENGTH; i++) {
|
||||
buffer[i] = xn;
|
||||
}
|
||||
sum = xn * MVF_LENGTH;
|
||||
index = 0;
|
||||
} else {
|
||||
sum -= buffer[index];
|
||||
buffer[index] = xn;
|
||||
sum += xn;
|
||||
index++;
|
||||
if (index >= MVF_LENGTH) {
|
||||
index = 0;
|
||||
}
|
||||
}
|
||||
yn = sum / MVF_LENGTH;
|
||||
return yn;
|
||||
}
|
||||
|
||||
//float Current_Temp1, ii=0.0f, iii=0.0f;
|
||||
//#define PWM_FREQUENCY 24000
|
||||
//#define CURRENT_MEASURE_HZ PWM_FREQUENCY
|
||||
//#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)
|
||||
//
|
||||
//#define TIMER0_CLK_MHz 168
|
||||
//#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
|
||||
//#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)
|
||||
#define TS_CAL1 ((uint16_t *) ((uint32_t) 0x1FFF75A8))// 30摄氏度时的MCU内部温度传感器校准值
|
||||
#define TS_CAL2 ((uint16_t *) ((uint32_t) 0x1FFF75CA))//110摄氏度时的MCU内部温度传感器校准值
|
||||
|
||||
int ret;
|
||||
void APP_Main() {
|
||||
// TIM1->CCR1 = 0;
|
||||
// TIM1->CCR2 = 0;
|
||||
// TIM1->CCR3 = 100;
|
||||
// TIM1->CCR4 = 10;
|
||||
// MCT_set_state(CALIBRATION);
|
||||
printf("%f,%f,%f\n", Foc.i_a, Foc.i_b, Foc.i_c);
|
||||
|
||||
// ii += 0.000001f;
|
||||
//// if (ii == 0.1f) { ii = 0.1f-0.00001f; }
|
||||
// iii += 0.03f;
|
||||
// if (iii >= 360.0f) { iii = 0.0f; }
|
||||
// Current_Temp = ((110.0 - 30) / (*TS_CAL2 - *TS_CAL1) * (int16_t) (adc1_RegularBuf[1] * 3.3 / 3.0 - *TS_CAL1)) + 30;
|
||||
// Current_Temp1 = moving_average_filtre(Current_Temp * sinf(ii));
|
||||
// SendCurrent_Vofa(Current_Temp * sinf(ii), Current_Temp1, 0);
|
||||
|
||||
// Generate(0, 0.02f, iii);
|
||||
// SendCurrent_Vofa(FOC.dtc_a, iii,(uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES));
|
||||
// TIM1->CCR1 = (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
// TIM1->CCR2 = (uint16_t) (FOC.dtc_b * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
// TIM1->CCR3 = (uint16_t) (FOC.dtc_c * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
|
||||
// SendCurrent_Vofa(FOC.dtc_a, ii,0);
|
||||
|
||||
// usb_printf("%f\r\n", NowAngle);
|
||||
|
||||
|
||||
// usb_printf("CoreTemp:%d,%d, %.2f\r\n", adc1_RegularBuf[0], adc1_RegularBuf[1], Current_Temp);
|
||||
// usb_printf("PhaseCurrent:%d, %d, %d\r\n", ADC1->JDR1 - 2040, ADC2->JDR1 - 2024, ADC1->JDR2 - 2024);
|
||||
// usb_printf("PhaseVoltage:%d, %d, %d\r\n", ADC2->JDR2, ADC2->JDR3, ADC2->JDR4);
|
||||
// SendCurrent_Vofa(FOC.dtc_a, FOC.dtc_b,FOC.dtc_c);
|
||||
// SendCurrent_Vofa(ADC1->JDR1, FOC.dtc_a, 0 );
|
||||
|
||||
// CurrA = ia/4096*3.3/0.005f/7.33333;
|
||||
// SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, ADC2->JDR1, ADC1->JDR2);
|
||||
// usb_printf("PhaseVoltage:%d\r\n", (ADC1->JDR1-2048.0f)/4096.0f*3.3f/0.005f/7.33333f);
|
||||
|
||||
// NowAngle = Data.Angle.getAngle();
|
||||
// SendCurrent_Vofa(FOC.dtc_a, NowAngle, 0);
|
||||
// HAL_Delay(10);
|
||||
if (USART1_IRQHandler_Status) {
|
||||
switch (RxBuffer[0]) {
|
||||
case 10:
|
||||
switch (RxBuffer[1]) {
|
||||
case 10:
|
||||
ret = MCT_set_state(IDLE);
|
||||
printf("IDLE ret:%d\r\n", ret);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 00:
|
||||
switch (RxBuffer[1]) {
|
||||
case 00:
|
||||
UsrConfig.control_mode = CONTROL_MODE_TORQUE_RAMP;
|
||||
printf("UsrConfig.control_mode->CONTROL_MODE_TORQUE_RAMP\r\n");
|
||||
break;
|
||||
case 01:
|
||||
UsrConfig.control_mode = CONTROL_MODE_VELOCITY_RAMP;
|
||||
printf("UsrConfig.control_mode->CONTROL_MODE_VELOCITY_RAMP\r\n");
|
||||
break;
|
||||
case 02:
|
||||
UsrConfig.control_mode = CONTROL_MODE_POSITION_FILTER;
|
||||
printf("UsrConfig.control_mode->CONTROL_MODE_POSITION_FILTER\r\n");
|
||||
break;
|
||||
case 03:
|
||||
UsrConfig.control_mode = CONTROL_MODE_POSITION_PROFILE;
|
||||
printf("UsrConfig.control_mode->CONTROL_MODE_POSITION_PROFILE\r\n");
|
||||
break;
|
||||
case 04:
|
||||
ret = MCT_set_state(IDLE);
|
||||
printf("IDLE ret:%d\r\n", ret);
|
||||
break;
|
||||
case 05:
|
||||
ret = MCT_set_state(CALIBRATION);
|
||||
printf("CALIBRATION ret:%d\r\n", ret);
|
||||
break;
|
||||
case 06:
|
||||
ret = MCT_set_state(RUN);
|
||||
printf("RUN ret:%d\r\n", ret);
|
||||
break;
|
||||
}
|
||||
Controller.input_position = (float) ((float) RxBuffer[1] / 10.0f);
|
||||
Controller.input_velocity = (float) ((float) RxBuffer[1] / 10.0f);
|
||||
printf("Controller.input_position->%f\r\n", Controller.input_position);
|
||||
printf("Controller.input_position->%f\r\n", Controller.input_position);
|
||||
break;
|
||||
case 01:
|
||||
Controller.input_position = (float) ((float) RxBuffer[1] / 10.0f);
|
||||
Controller.input_velocity = (float) ((float) RxBuffer[1] / 10.0f);
|
||||
printf("Controller.input_position->%f\r\n", Controller.input_position);
|
||||
printf("Controller.input_position->%f\r\n", Controller.input_position);
|
||||
break;
|
||||
case 02:
|
||||
UsrConfig.pos_gain = (float) ((float) RxBuffer[1]);
|
||||
printf("UsrConfig.pos_gain->%f\r\n", UsrConfig.pos_gain);
|
||||
break;
|
||||
case 03:
|
||||
UsrConfig.vel_gain = (float) ((float) RxBuffer[1] / 100.0f);
|
||||
printf("UsrConfig.vel_gain->%f\r\n", UsrConfig.vel_gain);
|
||||
break;
|
||||
case 04:
|
||||
UsrConfig.vel_integrator_gain = (float) ((float) RxBuffer[1] / 1000.0f);
|
||||
printf("UsrConfig.vel_integrator_gain->%f\r\n", UsrConfig.vel_integrator_gain);
|
||||
break;
|
||||
}
|
||||
USART1_IRQHandler_Status = 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
//软件循环读取ADC
|
||||
// HAL_ADC_Start(&hadc1);
|
||||
// if (HAL_OK == HAL_ADC_PollForConversion(&hadc1, 50)) {
|
||||
// ADC_VAL1 = HAL_ADC_GetValue(&hadc1);
|
||||
// }
|
||||
|
@ -6,36 +6,42 @@
|
||||
#define BOOOOMFOC_STSPIN32G4_EVB_APP_MAIN_H
|
||||
|
||||
#include "main.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
|
||||
|
||||
#include "adc.h"
|
||||
#include "dma.h"
|
||||
#include "i2c.h"
|
||||
#include "gpio.h"
|
||||
#include "opamp.h"
|
||||
#include "spi.h"
|
||||
#include "tim.h"
|
||||
#include "usb_device.h"
|
||||
#include "gpio.h"
|
||||
|
||||
#include "usbd_cdc_if.h"
|
||||
|
||||
#include "arm_math.h"
|
||||
|
||||
#include "Communication.h"
|
||||
//#include "arm_math.h"
|
||||
//#include "Communication.h"
|
||||
#include "InteriorADC.h"
|
||||
|
||||
#include "Angle.h"
|
||||
|
||||
extern tData Data;
|
||||
|
||||
extern DMA_HandleTypeDef hdma_usart1_rx;
|
||||
extern DMA_HandleTypeDef hdma_usart1_tx;
|
||||
|
||||
extern uint8_t RxBuffer[2];//接收数据
|
||||
extern uint8_t USART1_IRQHandler_Status;
|
||||
extern volatile uint32_t SystickCount;
|
||||
// LED ACT
|
||||
#define LED_ACT_SET()
|
||||
#define LED_ACT_RESET()
|
||||
#define LED_ACT_GET()
|
||||
|
||||
static inline void watch_dog_feed(void)
|
||||
{
|
||||
// FWDGT_CTL = FWDGT_KEY_RELOAD;
|
||||
}
|
||||
static inline uint32_t get_ms_since(uint32_t tick)
|
||||
{
|
||||
return (uint32_t) ((SystickCount - tick) / 2U);
|
||||
}
|
||||
|
||||
|
||||
static __IO uint16_t adc1_RegularBuf[10];
|
||||
|
||||
void APP_Init();
|
||||
void APP_Main();
|
||||
void APP_Task();
|
||||
|
||||
#endif //BOOOOMFOC_STSPIN32G4_EVB_APP_MAIN_H
|
||||
#endif//BOOOOMFOC_STSPIN32G4_EVB_APP_MAIN_H
|
||||
|
487
APP/APP_Task.c
487
APP/APP_Task.c
@ -1,75 +1,460 @@
|
||||
//
|
||||
// Created by ZK on 24-5-10.
|
||||
//
|
||||
/*
|
||||
Copyright 2021 codenocold codenocold@qq.com
|
||||
Address : https://github.com/codenocold/dgm
|
||||
This file is part of the dgm firmware.
|
||||
The dgm firmware 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.
|
||||
The dgm firmware 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 "APP_Task.h"
|
||||
//#include "anticogging.h"
|
||||
#include "calibration.h"
|
||||
//#include "can.h"
|
||||
#include "APP_Main.h"
|
||||
#include "Angle.h"
|
||||
#include "InteriorADC.h"
|
||||
#include "Controller/Controller.h"
|
||||
#include "Controller/SVPWM/SVPWM.h"
|
||||
#include "APP_Task.h"
|
||||
#include "anticogging.h"
|
||||
#include "controller.h"
|
||||
#include "encoder.h"
|
||||
#include "foc.h"
|
||||
#include "pwm_curr.h"
|
||||
#include "usr_config.h"
|
||||
#include "util.h"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
|
||||
typedef struct sFSM {
|
||||
tFSMState state;
|
||||
tFSMState state_next;
|
||||
uint8_t state_next_ready;
|
||||
} tFSM;
|
||||
|
||||
float CurrA, CurrB, CurrC;
|
||||
float Current_Temp, V_Temp;
|
||||
//float32_t NowAngle;
|
||||
static volatile tFSM mFSM;
|
||||
|
||||
float Current_Temp1, ii = 0.0f, Test_Angle = 0.0f;
|
||||
volatile tMCStatusword StatuswordNew;
|
||||
volatile tMCStatusword StatuswordOld;
|
||||
;
|
||||
|
||||
#define PWM_FREQUENCY 24000
|
||||
#define CURRENT_MEASURE_HZ PWM_FREQUENCY
|
||||
#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)
|
||||
#define CHARGE_BOOT_CAP_MS 10
|
||||
#define CHARGE_BOOT_CAP_TICKS (uint16_t)((PWM_FREQUENCY * CHARGE_BOOT_CAP_MS) / 1000)
|
||||
static uint16_t mChargeBootCapDelay = 0;
|
||||
|
||||
#define TIMER0_CLK_MHz 168
|
||||
#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
|
||||
#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)
|
||||
static void enter_state(void);
|
||||
static void exit_state(void);
|
||||
static void led_act_loop(void);
|
||||
|
||||
int up = 1;
|
||||
float32_t A, Speedtarget;
|
||||
int32_t number;
|
||||
int32_t mode = 0;
|
||||
void MCT_init(void) {
|
||||
mFSM.state = BOOT_UP;
|
||||
mFSM.state_next = BOOT_UP;
|
||||
mFSM.state_next_ready = 0;
|
||||
|
||||
void High_Frequency_Task() {
|
||||
StatuswordNew.status.status_code = 0;
|
||||
StatuswordNew.errors.errors_code = 0;
|
||||
StatuswordOld = StatuswordNew;
|
||||
}
|
||||
|
||||
if (mode < 10000) {
|
||||
// HAL_Delay(1000);
|
||||
mode++;
|
||||
void MCT_reset_error(void) {
|
||||
StatuswordNew.errors.errors_code &= 0x0000FFFF;
|
||||
StatuswordOld.errors.errors_code &= 0x0000FFFF;
|
||||
}
|
||||
|
||||
tFSMState MCT_get_state(void) {
|
||||
return mFSM.state;
|
||||
}
|
||||
|
||||
// return
|
||||
// 0 Success
|
||||
// -1 Invalid
|
||||
// -2 Error code
|
||||
// -3 Calib invalid
|
||||
int MCT_set_state(tFSMState state) {
|
||||
int ret = 0;
|
||||
|
||||
switch (mFSM.state) {
|
||||
case BOOT_UP:
|
||||
if (state == IDLE) {
|
||||
mFSM.state_next = IDLE;
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
switch (state) {
|
||||
case IDLE:
|
||||
FOC_disarm();
|
||||
mChargeBootCapDelay = 0;
|
||||
mFSM.state_next = IDLE;
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
if (StatuswordNew.errors.errors_code) {
|
||||
ret = -2;
|
||||
} else if (!UsrConfig.calib_valid) {
|
||||
ret = -3;
|
||||
} else {
|
||||
FOC_arm();
|
||||
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
||||
mFSM.state_next = RUN;
|
||||
}
|
||||
break;
|
||||
|
||||
case CALIBRATION:
|
||||
if (StatuswordNew.errors.errors_code) {
|
||||
ret = -2;
|
||||
} else {
|
||||
FOC_arm();
|
||||
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
||||
mFSM.state_next = CALIBRATION;
|
||||
}
|
||||
break;
|
||||
|
||||
case ANTICOGGING:
|
||||
if (StatuswordNew.errors.errors_code) {
|
||||
ret = -2;
|
||||
} else if (!UsrConfig.calib_valid) {
|
||||
ret = -3;
|
||||
} else {
|
||||
FOC_arm();
|
||||
mChargeBootCapDelay = CHARGE_BOOT_CAP_TICKS;
|
||||
mFSM.state_next = ANTICOGGING;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
ret = -1;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
if (state == IDLE) {
|
||||
mFSM.state_next = IDLE;
|
||||
} else {
|
||||
ret = -1;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (mode >= 10000) {
|
||||
|
||||
mFSM.state_next_ready = 0;
|
||||
|
||||
number++;
|
||||
if (number >= 1 && number <= 10000) {
|
||||
Speedtarget = 1.0;
|
||||
} else if (number >= 10000 && number <= 20000) {
|
||||
Speedtarget = -1.0;
|
||||
} else if (number >= 20001) {
|
||||
number = 0;
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void enter_state(void) {
|
||||
switch (mFSM.state) {
|
||||
case BOOT_UP:
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
CONTROLLER_reset();
|
||||
StatuswordNew.status.switched_on = 1;
|
||||
StatuswordNew.status.target_reached = 1;
|
||||
StatuswordNew.status.current_limit_active = 0;
|
||||
StatuswordOld.status = StatuswordNew.status;
|
||||
break;
|
||||
|
||||
case CALIBRATION:
|
||||
CALIBRATION_start();
|
||||
break;
|
||||
|
||||
case ANTICOGGING:
|
||||
CONTROLLER_reset();
|
||||
ANTICOGGING_start();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void exit_state(void) {
|
||||
switch (mFSM.state) {
|
||||
case BOOT_UP:
|
||||
// CAN_reset_rx_timeout();
|
||||
// CAN_reset_tx_timeout();
|
||||
mFSM.state_next_ready = 1;
|
||||
break;
|
||||
|
||||
case IDLE:
|
||||
if (mChargeBootCapDelay) {
|
||||
mChargeBootCapDelay--;
|
||||
} else {
|
||||
mFSM.state_next_ready = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
FOC_disarm();
|
||||
StatuswordNew.status.switched_on = 0;
|
||||
StatuswordNew.status.target_reached = 0;
|
||||
StatuswordNew.status.current_limit_active = 0;
|
||||
StatuswordOld.status = StatuswordNew.status;
|
||||
mFSM.state_next_ready = 1;
|
||||
break;
|
||||
|
||||
case CALIBRATION:
|
||||
CALIBRATION_end();
|
||||
|
||||
USR_CONFIG_save_config();
|
||||
|
||||
printf("UsrConfig.motor_phase_resistance:%f\r\n", UsrConfig.motor_phase_resistance);
|
||||
printf("UsrConfig.motor_phase_inductance:%f\r\n", UsrConfig.motor_phase_inductance);
|
||||
printf("UsrConfig.motor_pole_pairs:%ld\r\n", UsrConfig.motor_pole_pairs);
|
||||
|
||||
mFSM.state_next_ready = 1;
|
||||
break;
|
||||
|
||||
case ANTICOGGING:
|
||||
ANTICOGGING_end();
|
||||
mFSM.state_next_ready = 1;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
int time = 1;
|
||||
int count = 0;
|
||||
void MCT_high_frequency_task(void) {
|
||||
/* state transition management */
|
||||
if (mFSM.state_next != mFSM.state) {
|
||||
exit_state();
|
||||
if (mFSM.state_next_ready) {
|
||||
mFSM.state = mFSM.state_next;
|
||||
enter_state();
|
||||
}
|
||||
// Test_Angle += 0.01f;
|
||||
// if (Test_Angle > 360.0f) {
|
||||
// Test_Angle = 0.0f;
|
||||
}
|
||||
|
||||
// if (time >= 0) {
|
||||
// time++;
|
||||
// }
|
||||
//
|
||||
// if (time == 5) {
|
||||
// StatuswordNew.errors.errors_code = 0;
|
||||
// int ret = MCT_set_state(IDLE);
|
||||
// printf("IDLE ret:%d\r\n", ret);
|
||||
// }
|
||||
//
|
||||
// if (time == 10) {
|
||||
// int ret = MCT_set_state(CALIBRATION);
|
||||
// printf("CALIBRATION ret:%d\r\n", ret);
|
||||
// // UsrConfig.control_mode = CONTROL_MODE_VELOCITY_RAMP;
|
||||
// // Controller.input_velocity = 0.01f;
|
||||
//
|
||||
// time = -1;
|
||||
// }
|
||||
// //
|
||||
// if (UsrConfig.calib_valid && time == -1) {
|
||||
// int ret = MCT_set_state(RUN);
|
||||
// printf("RUN ret:%d\r\n", ret);
|
||||
// UsrConfig.control_mode = CONTROL_MODE_POSITION_PROFILE;
|
||||
// Controller.input_position = 0.0f;
|
||||
// Controller.input_velocity = 0.0f;
|
||||
// time = -2;
|
||||
// }
|
||||
//
|
||||
// if (UsrConfig.calib_valid && time == -2) {
|
||||
// count++;
|
||||
// if (count < 50000) {
|
||||
// Controller.input_position = 5.0f;
|
||||
// Controller.input_updated = true;
|
||||
// }
|
||||
// if (count > 50000) {
|
||||
// Controller.input_position = 0.5f;
|
||||
// Controller.input_updated = true;
|
||||
// }
|
||||
// if (count > 100000) {
|
||||
// count = 0;
|
||||
// }
|
||||
//
|
||||
// time = -2;
|
||||
// }
|
||||
|
||||
|
||||
//Data.Angle.getAngle()
|
||||
// SendCurrent_Vofa(ADC1->JDR1, ADC2->JDR1, ADC1->JDR2);
|
||||
|
||||
// float test = 0;
|
||||
// UTILS_LP_FAST(test, 12, 0.05f);
|
||||
// usb_printf("%f\r\n", test);
|
||||
// ENCODER_loop();
|
||||
// FOC_voltage(0,0.001f,Encoder.phase);
|
||||
// Foc.v_bus = read_vbus();
|
||||
Foc.v_bus = 24.0f;
|
||||
UTILS_LP_FAST(Foc.v_bus_filt, Foc.v_bus, 0.05f);
|
||||
Foc.i_a = read_iphase_a();
|
||||
Foc.i_b = read_iphase_b();
|
||||
Foc.i_c = read_iphase_c();
|
||||
|
||||
|
||||
// Generate_SVM(0.0f, 0.05f, Data.Angle.getAngle());
|
||||
// float32_t Eangle = Data.Angle.getAngle();
|
||||
SpeedControl(Speedtarget, MT_ReadAngle(),10.0f);
|
||||
FOC_current(0, 10.0f, Data.Angle.getAngle(), 5000);
|
||||
switch (mFSM.state) {
|
||||
case BOOT_UP:
|
||||
break;
|
||||
|
||||
// SendCurrent_Vofa(Test_Angle,A, (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES));
|
||||
case CALIBRATION:
|
||||
CALIBRATION_loop();
|
||||
break;
|
||||
|
||||
case ANTICOGGING:
|
||||
ANTICOGGING_loop();
|
||||
|
||||
// SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, 0, 0);
|
||||
case RUN: {
|
||||
CONTROLLER_loop();
|
||||
|
||||
// check motor current
|
||||
// If Ia + Ib + Ic == 0 holds then we have: Inorm^2 = Id^2 + Iq^2 = Ialpha^2 + Ibeta^2 = 2/3 * (Ia^2 + Ib^2 + Ic^2)
|
||||
float Inorm_sq = 2.0f / 3.0f * (SQ(Foc.i_a) + SQ(Foc.i_b) + SQ(Foc.i_c));
|
||||
if (Inorm_sq > SQ(UsrConfig.protect_over_current)) {
|
||||
FOC_disarm();
|
||||
MCT_set_state(IDLE);
|
||||
StatuswordNew.errors.over_current = 1;
|
||||
}
|
||||
|
||||
// check I bus current
|
||||
if (Foc.i_bus > UsrConfig.protect_i_bus_max) {
|
||||
FOC_disarm();
|
||||
MCT_set_state(IDLE);
|
||||
StatuswordNew.errors.over_current = 1;
|
||||
}
|
||||
} break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void MCT_safety_task(void) {
|
||||
// VBUS check
|
||||
if (mFSM.state != BOOT_UP) {
|
||||
// Over voltage check
|
||||
if (Foc.v_bus > UsrConfig.protect_over_voltage) {
|
||||
StatuswordNew.errors.over_voltage = 1;
|
||||
}
|
||||
|
||||
// Under voltage check
|
||||
if (Foc.v_bus < UsrConfig.protect_under_voltage) {
|
||||
StatuswordNew.errors.under_voltage = 1;
|
||||
}
|
||||
|
||||
// Enchoder state check
|
||||
if (MT6816.check_err_count > 50 || MT6816.rx_err_count > 50) {
|
||||
StatuswordNew.errors.encoder_offline = 1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
watch_dog_feed();
|
||||
}
|
||||
|
||||
void MCT_low_priority_task(void) {
|
||||
bool isSend = false;
|
||||
|
||||
// State check
|
||||
if (StatuswordOld.status.status_code != StatuswordNew.status.status_code) {
|
||||
isSend = true;
|
||||
StatuswordOld.status.status_code = StatuswordNew.status.status_code;
|
||||
}
|
||||
|
||||
// Error check
|
||||
if (StatuswordOld.errors.errors_code != StatuswordNew.errors.errors_code) {
|
||||
if (StatuswordNew.errors.errors_code) {
|
||||
FOC_disarm();
|
||||
MCT_set_state(IDLE);
|
||||
}
|
||||
|
||||
isSend = true;
|
||||
StatuswordOld.errors.errors_code = StatuswordNew.errors.errors_code;
|
||||
}
|
||||
|
||||
if (isSend) {
|
||||
CAN_tx_statusword(StatuswordNew);
|
||||
}
|
||||
|
||||
led_act_loop();
|
||||
CAN_comm_loop();
|
||||
}
|
||||
|
||||
static void led_act_loop(void) {
|
||||
static uint16_t tick = 0;
|
||||
static uint32_t tick_100Hz = 0;
|
||||
|
||||
// 100Hz
|
||||
if (get_ms_since(tick_100Hz) < 10) {
|
||||
return;
|
||||
}
|
||||
tick_100Hz = SystickCount;
|
||||
|
||||
switch (mFSM.state) {
|
||||
case IDLE:
|
||||
if (tick == 0) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 10) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick > 100) {
|
||||
tick = 0xFFFF;
|
||||
}
|
||||
break;
|
||||
|
||||
case RUN:
|
||||
if (tick == 0) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 10) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 20) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 30) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick > 100) {
|
||||
tick = 0xFFFF;
|
||||
}
|
||||
break;
|
||||
|
||||
case CALIBRATION:
|
||||
if (tick == 0) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 10) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 20) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 30) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 40) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 50) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick > 150) {
|
||||
tick = 0xFFFF;
|
||||
}
|
||||
break;
|
||||
|
||||
case ANTICOGGING:
|
||||
if (tick == 0) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 10) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 20) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 30) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 40) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 50) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick == 60) {
|
||||
LED_ACT_SET();
|
||||
} else if (tick == 70) {
|
||||
LED_ACT_RESET();
|
||||
} else if (tick > 200) {
|
||||
tick = 0xFFFF;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
tick++;
|
||||
}
|
||||
|
@ -1,10 +1,57 @@
|
||||
//
|
||||
// Created by ZK on 24-5-10.
|
||||
//
|
||||
#ifndef __MC_TASKS_H__
|
||||
#define __MC_TASKS_H__
|
||||
|
||||
#ifndef BOOOOMFOC_STSPIN32G4_EVB_APP_TASK_H
|
||||
#define BOOOOMFOC_STSPIN32G4_EVB_APP_TASK_H
|
||||
#include "main.h"
|
||||
|
||||
void High_Frequency_Task();
|
||||
// Motor controler state
|
||||
typedef enum eFSMState {
|
||||
BOOT_UP = 0,
|
||||
IDLE = 1,
|
||||
RUN = 2,
|
||||
CALIBRATION = 3,
|
||||
ANTICOGGING = 4,
|
||||
} tFSMState;
|
||||
|
||||
#endif //BOOOOMFOC_STSPIN32G4_EVB_APP_TASK_H
|
||||
typedef struct sMCStatusword
|
||||
{
|
||||
union {
|
||||
uint32_t status_code;
|
||||
struct
|
||||
{
|
||||
uint32_t switched_on : 1;
|
||||
uint32_t target_reached : 1;
|
||||
uint32_t current_limit_active : 1;
|
||||
uint32_t PADDING : 29;
|
||||
};
|
||||
} status;
|
||||
|
||||
union {
|
||||
uint32_t errors_code;
|
||||
struct
|
||||
{
|
||||
// FATAL
|
||||
uint32_t adc_selftest_fatal : 1; // 1<<0
|
||||
uint32_t encoder_offline : 1; // 1<<1
|
||||
uint32_t PADDING_1 : 14;
|
||||
// ERROR
|
||||
uint32_t over_voltage : 1; // 1<<16
|
||||
uint32_t under_voltage : 1; // 1<<17
|
||||
uint32_t over_current : 1; // 1<<18
|
||||
uint32_t PADDING_2 : 13;
|
||||
};
|
||||
} errors;
|
||||
} tMCStatusword;
|
||||
|
||||
extern volatile tMCStatusword StatuswordNew;
|
||||
extern volatile tMCStatusword StatuswordOld;
|
||||
|
||||
void MCT_init(void);
|
||||
void MCT_reset_error(void);
|
||||
tFSMState MCT_get_state(void);
|
||||
int MCT_set_state(tFSMState state);
|
||||
|
||||
void MCT_high_frequency_task(void);
|
||||
void MCT_safety_task(void);
|
||||
void MCT_low_priority_task(void);
|
||||
|
||||
#endif
|
||||
|
@ -1,27 +1,27 @@
|
||||
////
|
||||
//// Created by ZK on 2023/3/14.
|
||||
////
|
||||
//
|
||||
// Created by ZK on 2023/3/14.
|
||||
//#include "PID.h"
|
||||
//
|
||||
|
||||
#include "PID.h"
|
||||
|
||||
float PID_Generate(PID *pid) {
|
||||
pid->error = pid->target - pid->value;
|
||||
|
||||
if (pid->error > pid->errMin || pid->error < -pid->errMin)
|
||||
pid->errSum += pid->error * pid->ki;
|
||||
|
||||
if (pid->errSum > pid->errSumMax)
|
||||
pid->errSum = pid->errSumMax;
|
||||
else if (pid->errSum < -pid->errSumMax)
|
||||
pid->errSum = -pid->errSumMax;
|
||||
|
||||
pid->errDt = pid->error - pid->lastErr;
|
||||
pid->lastErr = pid->error;
|
||||
|
||||
pid->result = pid->kp * pid->error + pid->errSum + pid->kd * pid->errDt;
|
||||
|
||||
if (pid->result > pid->valMax)
|
||||
pid->result = pid->valMax;
|
||||
else if (pid->result < -pid->valMax)
|
||||
pid->result = -pid->valMax;
|
||||
}
|
||||
//float PID_Generate(PID *pid) {
|
||||
// pid->error = pid->target - pid->value;
|
||||
//
|
||||
// if (pid->error > pid->errMin || pid->error < -pid->errMin)
|
||||
// pid->errSum += pid->error * pid->ki;
|
||||
//
|
||||
// if (pid->errSum > pid->errSumMax)
|
||||
// pid->errSum = pid->errSumMax;
|
||||
// else if (pid->errSum < -pid->errSumMax)
|
||||
// pid->errSum = -pid->errSumMax;
|
||||
//
|
||||
// pid->errDt = pid->error - pid->lastErr;
|
||||
// pid->lastErr = pid->error;
|
||||
//
|
||||
// pid->result = pid->kp * pid->error + pid->errSum + pid->kd * pid->errDt;
|
||||
//
|
||||
// if (pid->result > pid->valMax)
|
||||
// pid->result = pid->valMax;
|
||||
// else if (pid->result < -pid->valMax)
|
||||
// pid->result = -pid->valMax;
|
||||
//}
|
@ -1,32 +1,32 @@
|
||||
////
|
||||
//// Created by ZK on 2023/3/14.
|
||||
////
|
||||
//
|
||||
// Created by ZK on 2023/3/14.
|
||||
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
//#define BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
//
|
||||
|
||||
#ifndef BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
#define BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
|
||||
typedef struct pid {
|
||||
float kp;//比例系数
|
||||
float ki;//积分系数
|
||||
float kd;//微分系数
|
||||
|
||||
float target;
|
||||
float value;
|
||||
float error;
|
||||
|
||||
float errSum;
|
||||
float errSumMax;
|
||||
|
||||
float errMin;
|
||||
float valMax;
|
||||
|
||||
float lastErr;
|
||||
float errDt;
|
||||
|
||||
float result;
|
||||
} PID;
|
||||
|
||||
float PID_Generate(PID *pid);
|
||||
|
||||
|
||||
#endif //BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
//typedef struct pid {
|
||||
// float kp;//比例系数
|
||||
// float ki;//积分系数
|
||||
// float kd;//微分系数
|
||||
//
|
||||
// float target;
|
||||
// float value;
|
||||
// float error;
|
||||
//
|
||||
// float errSum;
|
||||
// float errSumMax;
|
||||
//
|
||||
// float errMin;
|
||||
// float valMax;
|
||||
//
|
||||
// float lastErr;
|
||||
// float errDt;
|
||||
//
|
||||
// float result;
|
||||
//} PID;
|
||||
//
|
||||
//float PID_Generate(PID *pid);
|
||||
//
|
||||
//
|
||||
//#endif //BOOOOMFOC_STSPIN32G4_EVB_PID_H
|
||||
|
@ -1,43 +1,43 @@
|
||||
////
|
||||
//// Created by ZK on 2023/3/14.
|
||||
////
|
||||
//
|
||||
// Created by ZK on 2023/3/14.
|
||||
////#include "Communication.h"
|
||||
//#include "APP_Main.h"
|
||||
//
|
||||
//#define BYTE0(dwTemp) (*(char*)(&dwTemp))
|
||||
//#define BYTE1(dwTemp) (*((char*)(&dwTemp) + 1))
|
||||
//#define BYTE2(dwTemp) (*((char*)(&dwTemp) + 2))
|
||||
//#define BYTE3(dwTemp) (*((char*)(&dwTemp) + 3))
|
||||
//
|
||||
//uint8_t USBCDC_SendBuffur[APP_TX_DATA_SIZE];
|
||||
//
|
||||
//void SendCurrent_Vofa(float a, float b, float c) {
|
||||
// uint16_t USBCDC_SendBuffur_count = 0;
|
||||
//
|
||||
|
||||
//#include "Communication.h"
|
||||
#include "APP_Main.h"
|
||||
|
||||
#define BYTE0(dwTemp) (*(char*)(&dwTemp))
|
||||
#define BYTE1(dwTemp) (*((char*)(&dwTemp) + 1))
|
||||
#define BYTE2(dwTemp) (*((char*)(&dwTemp) + 2))
|
||||
#define BYTE3(dwTemp) (*((char*)(&dwTemp) + 3))
|
||||
|
||||
uint8_t USBCDC_SendBuffur[APP_TX_DATA_SIZE];
|
||||
|
||||
void SendCurrent_Vofa(float a, float b, float c) {
|
||||
uint16_t USBCDC_SendBuffur_count = 0;
|
||||
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(a);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(a);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(b);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(b);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(c);
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(c);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(a);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(a);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(b);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(b);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
|
||||
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x80;
|
||||
USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x7f;
|
||||
|
||||
CDC_Transmit_FS(USBCDC_SendBuffur, USBCDC_SendBuffur_count);
|
||||
}
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE2(c);
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE3(c);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(a);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(a);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(b);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(b);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE0(c);
|
||||
//// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = BYTE1(c);
|
||||
//
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x00;
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x80;
|
||||
// USBCDC_SendBuffur[USBCDC_SendBuffur_count++] = 0x7f;
|
||||
//
|
||||
// CDC_Transmit_FS(USBCDC_SendBuffur, USBCDC_SendBuffur_count);
|
||||
//}
|
||||
|
@ -1,12 +1,12 @@
|
||||
////
|
||||
//// Created by ZK on 2023/3/14.
|
||||
////
|
||||
//
|
||||
// Created by ZK on 2023/3/14.
|
||||
//#ifndef BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
//#define BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
//
|
||||
|
||||
#ifndef BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
#define BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
|
||||
|
||||
void SendCurrent_Vofa(float a, float b, float c);
|
||||
|
||||
|
||||
#endif //BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
//
|
||||
//void SendCurrent_Vofa(float a, float b, float c);
|
||||
//
|
||||
//
|
||||
//#endif //BOOOOMFOC_STSPIN32G4_EVB_COMMUNICATION_H
|
||||
|
@ -1,239 +1,239 @@
|
||||
////
|
||||
//// Created by ZK on 2023/3/14.
|
||||
////
|
||||
////#include "main.h"
|
||||
//#include <stdbool.h>
|
||||
//#include "Controller.h"
|
||||
//#include "SVPWM/SVPWM.h"
|
||||
//#include "Communication.h"
|
||||
//#include "Angle.h"
|
||||
//
|
||||
// Created by ZK on 2023/3/14.
|
||||
//#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
|
||||
//
|
||||
//
|
||||
//tFOC FOC;
|
||||
//
|
||||
//float uAlpha1;
|
||||
//float uBeta1;
|
||||
//
|
||||
//#define PWM_FREQUENCY 24000
|
||||
//#define CURRENT_MEASURE_HZ PWM_FREQUENCY
|
||||
//#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)
|
||||
//
|
||||
//#define TIMER0_CLK_MHz 168
|
||||
//#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
|
||||
//#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)
|
||||
//float current1, current2, current3;
|
||||
//
|
||||
//float32_t id_curr_pi_kp = 0.001f;
|
||||
//float32_t id_curr_pi_ki = 0.000001f;
|
||||
//
|
||||
//float32_t id_curr_pi_target = 0.0f;
|
||||
//float32_t id_curr_pi_value;
|
||||
//float32_t id_curr_pi_error;
|
||||
//
|
||||
//float32_t id_curr_pi_errMin = 0.0f;
|
||||
//float32_t id_curr_pi_errSum;
|
||||
//float32_t id_curr_pi_errSumMax = 30.0f;
|
||||
//
|
||||
//float32_t id_curr_pi_result;
|
||||
//
|
||||
//
|
||||
//float32_t iq_curr_pi_kp = 0.001f;
|
||||
//float32_t iq_curr_pi_ki = 0.000001f;
|
||||
//
|
||||
//float32_t iq_curr_pi_target = 1.0f;
|
||||
//float32_t iq_curr_pi_value;
|
||||
//float32_t iq_curr_pi_error;
|
||||
//
|
||||
//float32_t iq_curr_pi_errMin = 0.0f;
|
||||
//float32_t iq_curr_pi_errSum;
|
||||
//float32_t iq_curr_pi_errSumMax = 30.0f;
|
||||
//
|
||||
//float32_t iq_curr_pi_result;
|
||||
//
|
||||
//
|
||||
//float32_t Speed_target = 0.0f;
|
||||
//
|
||||
//float32_t Speedpid_error;
|
||||
//float32_t Speedpid_errSum;
|
||||
//float32_t Speedpid_kp = 1.0f;
|
||||
//float32_t Speedpid_ki = 0.1f;
|
||||
//float32_t Speedpid_kd = 1.0f;
|
||||
//float32_t Speedpid_errSumMax = 30.0f;
|
||||
//float32_t Speedpid_lastErr;
|
||||
//float32_t Speedpid_errDt;
|
||||
//float32_t Speedpid_result;
|
||||
//
|
||||
//
|
||||
//float32_t u_d, u_q;
|
||||
//
|
||||
//
|
||||
//static void Current_PI_Cal_Id(float32_t resultMax) {
|
||||
// //curr_pi_target = target;
|
||||
//
|
||||
// id_curr_pi_error = id_curr_pi_target - id_curr_pi_value;
|
||||
//
|
||||
//// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin)
|
||||
// id_curr_pi_errSum += id_curr_pi_error * id_curr_pi_ki;
|
||||
//
|
||||
// if (id_curr_pi_errSum > id_curr_pi_errSumMax)
|
||||
// id_curr_pi_errSum = id_curr_pi_errSumMax;
|
||||
// else if (id_curr_pi_errSum < -id_curr_pi_errSumMax)
|
||||
// id_curr_pi_errSum = -id_curr_pi_errSumMax;
|
||||
//
|
||||
// id_curr_pi_result = id_curr_pi_kp * id_curr_pi_error + id_curr_pi_errSum;
|
||||
//
|
||||
// if (id_curr_pi_result > resultMax)
|
||||
// id_curr_pi_result = resultMax;
|
||||
// else if (id_curr_pi_result < -resultMax)
|
||||
// id_curr_pi_result = -resultMax;
|
||||
//}
|
||||
//
|
||||
//
|
||||
//static void Current_PI_Cal_Iq(float32_t resultMax) {
|
||||
// //curr_pi_target = target;
|
||||
//
|
||||
// iq_curr_pi_error = iq_curr_pi_target - iq_curr_pi_value;
|
||||
//
|
||||
//// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin)
|
||||
// iq_curr_pi_errSum += iq_curr_pi_error * iq_curr_pi_ki;
|
||||
//
|
||||
// if (iq_curr_pi_errSum > iq_curr_pi_errSumMax)
|
||||
// iq_curr_pi_errSum = iq_curr_pi_errSumMax;
|
||||
// else if (iq_curr_pi_errSum < -iq_curr_pi_errSumMax)
|
||||
// iq_curr_pi_errSum = -iq_curr_pi_errSumMax;
|
||||
//
|
||||
// iq_curr_pi_result = iq_curr_pi_kp * iq_curr_pi_error + iq_curr_pi_errSum;
|
||||
//
|
||||
// if (iq_curr_pi_result > resultMax)
|
||||
// iq_curr_pi_result = resultMax;
|
||||
// else if (iq_curr_pi_result < -resultMax)
|
||||
// iq_curr_pi_result = -resultMax;
|
||||
//}
|
||||
//
|
||||
//
|
||||
//static float32_t PIDGetResult(float32_t Speedpid_value, float32_t valMax, float32_t errMin) {
|
||||
// Speedpid_error = Speed_target - Speedpid_value;
|
||||
//
|
||||
// if (Speedpid_error > errMin || Speedpid_error < -errMin)
|
||||
// Speedpid_errSum += Speedpid_error * Speedpid_ki;
|
||||
//
|
||||
// if (Speedpid_errSum > Speedpid_errSumMax)
|
||||
// Speedpid_errSum = Speedpid_errSumMax;
|
||||
// else if (Speedpid_errSum < -Speedpid_errSumMax)
|
||||
// Speedpid_errSum = -Speedpid_errSumMax;
|
||||
//
|
||||
// Speedpid_errDt = Speedpid_error - Speedpid_lastErr;
|
||||
// Speedpid_lastErr = Speedpid_error;
|
||||
//
|
||||
// Speedpid_result = Speedpid_kp * Speedpid_error + Speedpid_errSum + Speedpid_kd * Speedpid_errDt;
|
||||
//
|
||||
// if (Speedpid_result > valMax)
|
||||
// Speedpid_result = valMax;
|
||||
// else if (Speedpid_result < -valMax)
|
||||
// Speedpid_result = -valMax;
|
||||
//
|
||||
// return Speedpid_result;
|
||||
//}
|
||||
//
|
||||
//
|
||||
//void SpeedControl(float32_t target, float32_t angleVal, float32_t valMax) {
|
||||
// Speed_target = target;
|
||||
// float32_t motorControl_speedValue = GetSpeed(angleVal);
|
||||
//// SendCurrent_Vofa(motorControl_speedValue, target, 0);
|
||||
//// speedPID_value = motorControl_speedValue;
|
||||
//
|
||||
// iq_curr_pi_target = PIDGetResult(motorControl_speedValue, valMax, 0.0f);
|
||||
//}
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
////bool Generate_SVM(float ud, float uq, float Theta) {
|
||||
//bool FOC_current(float Id_set, float Iq_set, float Theta, float bw) {
|
||||
// if (id_curr_pi_target > Id_set) { id_curr_pi_target = Id_set; }
|
||||
// if (iq_curr_pi_target > Iq_set) { iq_curr_pi_target = Iq_set; }
|
||||
//// id_curr_pi_target = Id_set;
|
||||
//// iq_curr_pi_target = Iq_set;
|
||||
//
|
||||
// current1 = ADC1->JDR2;
|
||||
// current2 = ADC2->JDR1;
|
||||
// current3 = ADC1->JDR1;
|
||||
// current1 = (current1 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) + 0.23;
|
||||
// current2 = (current2 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f);
|
||||
// current3 = (current3 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) - 0.4;
|
||||
//// SendCurrent_Vofa(current1, current2, current3);
|
||||
//
|
||||
// // Clarke transform
|
||||
// float i_alpha, i_beta;
|
||||
// clarke_transform(current1, current2, current3, &i_alpha, &i_beta);
|
||||
// // Park transform
|
||||
// float i_d, i_q;
|
||||
// park_transform(i_alpha, i_beta, Theta, &i_d, &i_q);
|
||||
//
|
||||
// id_curr_pi_value = i_d;
|
||||
// iq_curr_pi_value = i_q;
|
||||
// Current_PI_Cal_Id(1.0f);
|
||||
// Current_PI_Cal_Iq(1.0f);
|
||||
// u_d = id_curr_pi_result;
|
||||
// u_q = iq_curr_pi_result;
|
||||
//
|
||||
//
|
||||
// // float mod_to_V = FOC.v_bus_filt * 2.0f / 3.0f;
|
||||
// // float V_to_mod = 1.0f / mod_to_V;
|
||||
// float mod_to_V = 1.2f * 2.0f / 3.0f;
|
||||
// float V_to_mod = 1.0f / mod_to_V;
|
||||
//
|
||||
// float bandwidth = MIN(bw, 0.25f * PWM_FREQUENCY);
|
||||
// // Apply PI control
|
||||
// float Ierr_d = Id_set - i_d;
|
||||
// float Ierr_q = Iq_set - i_q;
|
||||
//// FOC.current_ctrl_p_gain = 5.0f * bandwidth;
|
||||
//// FOC.current_ctrl_i_gain = 0.000002f * bandwidth;
|
||||
// FOC.current_ctrl_p_gain = 0.001f;
|
||||
// FOC.current_ctrl_i_gain = 0.00001f;
|
||||
// FOC.current_ctrl_integral_d = 0;
|
||||
// FOC.current_ctrl_integral_q = 0;
|
||||
// float mod_d = V_to_mod * (FOC.current_ctrl_integral_d + Ierr_d * FOC.current_ctrl_p_gain);
|
||||
// float mod_q = V_to_mod * (FOC.current_ctrl_integral_q + Ierr_q * FOC.current_ctrl_p_gain);
|
||||
//
|
||||
// // Vector modulation saturation, lock integrator if saturated
|
||||
// float mod_scalefactor = 0.9f * SQRT3_BY_2 / sqrtf(SQ(mod_d) + SQ(mod_q));
|
||||
// if (mod_scalefactor < 1.0f) {
|
||||
// mod_d *= mod_scalefactor;
|
||||
// mod_q *= mod_scalefactor;
|
||||
// FOC.current_ctrl_integral_d *= 0.99f;
|
||||
// FOC.current_ctrl_integral_q *= 0.99f;
|
||||
// } else {
|
||||
// FOC.current_ctrl_integral_d += Ierr_d * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD);
|
||||
// FOC.current_ctrl_integral_q += Ierr_q * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD);
|
||||
// }
|
||||
//
|
||||
//
|
||||
// float pwm_phase = Theta + Theta * M_2PI * 20 * CURRENT_MEASURE_PERIOD;
|
||||
// inversePark(u_d, u_q, Theta, &uAlpha1, &uBeta1);
|
||||
//
|
||||
// FOC.i_q = i_q;
|
||||
// UTILS_LP_FAST(FOC.i_q_filt, FOC.i_q, 0.01f);
|
||||
// FOC.i_d = i_d;
|
||||
// UTILS_LP_FAST(FOC.i_d_filt, FOC.i_d, 0.01f);
|
||||
// FOC.i_bus = (mod_d * i_d + mod_q * i_q);
|
||||
// UTILS_LP_FAST(FOC.i_bus_filt, FOC.i_bus, 0.01f);
|
||||
// FOC.power_filt = FOC.v_bus_filt * FOC.i_bus_filt;
|
||||
//
|
||||
// SVPWM(uAlpha1, uBeta1, &FOC.dtc_a, &FOC.dtc_b, &FOC.dtc_c);
|
||||
//// FOC.dtc_a = FOC.dtc_a * 0.01;
|
||||
//// FOC.dtc_b = FOC.dtc_b * 0.01;
|
||||
//// FOC.dtc_c = FOC.dtc_c * 0.01;
|
||||
// SendCurrent_Vofa(mod_d, mod_q, iq_curr_pi_target);
|
||||
// TIM1->CCR1 = (uint16_t) (FOC.dtc_c * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
// TIM1->CCR2 = (uint16_t) (FOC.dtc_b * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
// TIM1->CCR3 = (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES);
|
||||
//// TIM1->CCR1 = (uint16_t) FOC.dtc_a;
|
||||
//// TIM1->CCR2 = (uint16_t) FOC.dtc_b;
|
||||
//// TIM1->CCR3 = (uint16_t) FOC.dtc_c;
|
||||
//
|
||||
// return 0;
|
||||
//}
|
||||
//
|
||||
//#include "main.h"
|
||||
#include <stdbool.h>
|
||||
#include "Controller.h"
|
||||
#include "SVPWM/SVPWM.h"
|
||||
#include "Communication.h"
|
||||
#include "Angle.h"
|
||||
|
||||
#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample)))
|
||||
|
||||
|
||||
tFOC FOC;
|
||||
|
||||
float uAlpha1;
|
||||
float uBeta1;
|
||||
|
||||
#define PWM_FREQUENCY 24000
|
||||
#define CURRENT_MEASURE_HZ PWM_FREQUENCY
|
||||
#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ)
|
||||
|
||||
#define TIMER0_CLK_MHz 168
|
||||
#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE)
|
||||
#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U)
|
||||
float current1, current2, current3;
|
||||
|
||||
float32_t id_curr_pi_kp = 0.001f;
|
||||
float32_t id_curr_pi_ki = 0.000001f;
|
||||
|
||||
float32_t id_curr_pi_target = 0.0f;
|
||||
float32_t id_curr_pi_value;
|
||||
float32_t id_curr_pi_error;
|
||||
|
||||
float32_t id_curr_pi_errMin = 0.0f;
|
||||
float32_t id_curr_pi_errSum;
|
||||
float32_t id_curr_pi_errSumMax = 30.0f;
|
||||
|
||||
float32_t id_curr_pi_result;
|
||||
|
||||
|
||||
float32_t iq_curr_pi_kp = 0.001f;
|
||||
float32_t iq_curr_pi_ki = 0.000001f;
|
||||
|
||||
float32_t iq_curr_pi_target = 1.0f;
|
||||
float32_t iq_curr_pi_value;
|
||||
float32_t iq_curr_pi_error;
|
||||
|
||||
float32_t iq_curr_pi_errMin = 0.0f;
|
||||
float32_t iq_curr_pi_errSum;
|
||||
float32_t iq_curr_pi_errSumMax = 30.0f;
|
||||
|
||||
float32_t iq_curr_pi_result;
|
||||
|
||||
|
||||
float32_t Speed_target = 0.0f;
|
||||
|
||||
float32_t Speedpid_error;
|
||||
float32_t Speedpid_errSum;
|
||||
float32_t Speedpid_kp = 1.0f;
|
||||
float32_t Speedpid_ki = 0.1f;
|
||||
float32_t Speedpid_kd = 1.0f;
|
||||
float32_t Speedpid_errSumMax = 30.0f;
|
||||
float32_t Speedpid_lastErr;
|
||||
float32_t Speedpid_errDt;
|
||||
float32_t Speedpid_result;
|
||||
|
||||
|
||||
float32_t u_d, u_q;
|
||||
|
||||
|
||||
static void Current_PI_Cal_Id(float32_t resultMax) {
|
||||
//curr_pi_target = target;
|
||||
|
||||
id_curr_pi_error = id_curr_pi_target - id_curr_pi_value;
|
||||
|
||||
// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin)
|
||||
id_curr_pi_errSum += id_curr_pi_error * id_curr_pi_ki;
|
||||
|
||||
if (id_curr_pi_errSum > id_curr_pi_errSumMax)
|
||||
id_curr_pi_errSum = id_curr_pi_errSumMax;
|
||||
else if (id_curr_pi_errSum < -id_curr_pi_errSumMax)
|
||||
id_curr_pi_errSum = -id_curr_pi_errSumMax;
|
||||
|
||||
id_curr_pi_result = id_curr_pi_kp * id_curr_pi_error + id_curr_pi_errSum;
|
||||
|
||||