Boooom/BoooomFOC_STSPIN32G4_EVB
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修正电流限幅

Browse Source
This commit is contained in:
BoomPC 2024-12-02 02:45:48 +08:00
parent 721796e105
commit 58e529da77
73 changed files with 1920 additions and 12247 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -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
View 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
View File

@ -0,0 +1,5 @@
<component name="ProjectCodeStyleConfiguration">
<state>
<option name="USE_PER_PROJECT_SETTINGS" value="true" />
</state>
</component>

40
.mxproject
View File

File diff suppressed because one or more lines are too long

238
APP/APP_Main.c
View File

@ -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);
// }

44
APP/APP_Main.h
View File

@ -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
View File

@ -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++;
}

61
APP/APP_Task.h
View File

@ -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

50
BoooomCTL/Common/PID/PID.c
View File

@ -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;
//}

60
BoooomCTL/Common/PID/PID.h
View File

@ -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

70
BoooomCTL/Communication/Communication.c
View File

@ -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);
//}

20
BoooomCTL/Communication/Communication.h
View File

@ -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

474
BoooomCTL/Controller/Controller.c
View File

@ -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;