修正电流限幅

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

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>

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>

View File

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

File diff suppressed because one or more lines are too long

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

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

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

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

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

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

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

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

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;