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
Download Patch File Download Diff File Expand all files Collapse all files

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