4x-tech/foc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add systick ； issue -- i2c transfer unstable

Browse Source
This commit is contained in:
4x-tech
2025-11-12 21:26:40 +08:00
parent 4dc04bdd7e
commit 79d7fca725
11 changed files with 241 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

118
3rd/dfoc.c
View File

@@ -12,64 +12,64 @@
#define _1_SQRT3 0.57735026919f
#define _2_SQRT3 1.15470053838f
float Ua=0,Ub=0,Uc=0,Ualpha,Ubeta=0,dc_a=0,dc_b=0,dc_c=0;
float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0;
float voltage_limit = 8;
float voltage_power_supply = 0;
float zero_electric_Angle=0.0;
float zero_electric_Angle = 0.0;
extern int pp;
extern int Dir;
void Motor_en()
{
// GPIO_InitTypeDef GPIO_InitStructure;
//
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
// GPIO_Init(GPIOA,&GPIO_InitStructure);
//
// GPIO_SetBits(GPIOA, GPIO_Pin_8);
// GPIO_InitTypeDef GPIO_InitStructure;
//
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
//
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
// GPIO_Init(GPIOA,&GPIO_InitStructure);
//
// GPIO_SetBits(GPIOA, GPIO_Pin_8);
}
//限制幅值
float constrain(float amt, float low, float high)
{
return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}
//将角度归化到0-2PI
float normalizeAngle(float angle)
{
float a = fmod(angle, 2*PI);
return ((a>=0) ? a : (a + 2*PI));
float a = fmod(angle, 2 * PI);
return ((a >= 0) ? a : (a + 2 * PI));
}
float electricAngle(void)
{
return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
}
void SetPwm(float Ua, float Ub, float Uc)
{
float U_a=0.0;
float U_b=0.0;
float U_c=0.0;
U_a = constrain(Ua, 0.0f, voltage_limit);
U_b = constrain(Ub, 0.0f, voltage_limit);
U_c = constrain(Uc, 0.0f, voltage_limit);
dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
PWM_Channel1(dc_a * 4800.0f); // 频率15k
PWM_Channel2(dc_b * 4800.0f);
PWM_Channel3(dc_c * 4800.0f);
float U_a = 0.0;
float U_b = 0.0;
float U_c = 0.0;
U_a = constrain(Ua, 0.0f, voltage_limit);
U_b = constrain(Ub, 0.0f, voltage_limit);
U_c = constrain(Uc, 0.0f, voltage_limit);
dc_a = constrain(U_a / voltage_power_supply, 0.0f, 1.0f);
dc_b = constrain(U_b / voltage_power_supply, 0.0f, 1.0f);
dc_c = constrain(U_c / voltage_power_supply, 0.0f, 1.0f);
PWM_Channel1(dc_a * 4800.0f); // 频率15k
PWM_Channel2(dc_b * 4800.0f);
PWM_Channel3(dc_c * 4800.0f);
}
//FOC核心算法，克拉克逆变换/帕克逆变换
@@ -77,47 +77,47 @@ float test_angle = 0.0;
float last_test_angle = 0.0;
void SetPhaseVoltage(float Uq, float Ud, float angle_el)
{
// angle_el = normalizeAngle(angle_el);
test_angle = angle_el - last_test_angle;
Ualpha = -Uq*sin(angle_el);
Ubeta = Uq*cos(angle_el);
Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3)*Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = -(Ualpha + sqrt(3)*Ubeta) / 2 + voltage_power_supply / 2;
SetPwm(Ua,Ub,Uc);
last_test_angle = angle_el;
// angle_el = normalizeAngle(angle_el);
test_angle = angle_el - last_test_angle;
Ualpha = -Uq * sin(angle_el);
Ubeta = Uq * cos(angle_el);
Ua = Ualpha + voltage_power_supply / 2;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
SetPwm(Ua, Ub, Uc);
last_test_angle = angle_el;
}
void Check_Sensor(void)
{
SetPhaseVoltage(3, 0, _3PI_2);
delay_ms(3000);
zero_electric_Angle = electricAngle();
SetPhaseVoltage(0, 0, _3PI_2);
delay_ms(500);
SetPhaseVoltage(3, 0, _3PI_2);
delay_ms(3000);
zero_electric_Angle = electricAngle();
SetPhaseVoltage(0, 0, _3PI_2);
delay_ms(500);
}
void FOC_Init(float power)
{
voltage_power_supply = power;
//PWM_Init();
//CurrSense_Init();
//AS5600_Init();
Check_Sensor();
voltage_power_supply = power;
//PWM_Init();
//CurrSense_Init();
//AS5600_Init();
Check_Sensor();
}
// 单角度环
void Set_Angle(float Target)
{
float angle = GetAngle();
float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir*angle)*180/PI);
SetPhaseVoltage(Uq, 0, electricAngle());
float angle = GetAngle();
float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI);
SetPhaseVoltage(Uq, 0, electricAngle());
}

56
3rd/lowpass_filter.c
View File

@@ -1,42 +1,48 @@
#include <stdint.h>
#include <ti_msp_dl_config.h>
float y = 0;
float Lowpassfilter_sim(float x)
{
float out = 0.9*x + 0.1*y;
y = x;
return out;
float out = 0.9 * x + 0.1 * y;
y = x;
return out;
}
uint32_t Last_Timesamp = 0.0;
float Last_y = 0.0;
float Lowpassfilter(float Tf, float x)
{
float dt = 0.0;
float dt = 0.0;
//TODO
uint32_t Timesamp = 0;
//uint32_t Timestamp = SysTick->VAL;
if(Timesamp < Last_Timesamp) dt = (float)(Last_Timesamp - Timesamp)/9*1e-6;
else
dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp)/9*1e-6;
uint32_t Timesamp = DL_SYSTICK_getValue();
if(Timesamp < Last_Timesamp)
{
dt = (float)(Last_Timesamp - Timesamp) / 32 * 1e-6;
}
else
{
dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp) / 32 * 1e-6;
}
if(dt<0.0||dt==0) dt = 0.0015f;
else if(dt>0.005f)
{
Last_y = x;
Last_Timesamp = Timesamp;
return x;
}
float alpha = Tf / (Tf + dt);
float y = alpha * Last_y + (1.0f - alpha) * x;
Last_y = y;
Last_Timesamp = Timesamp;
if(dt < 0.0 || dt == 0)
{
dt = 0.0015f;
}
else if(dt > 0.005f)
{
Last_y = x;
Last_Timesamp = Timesamp;
return x;
}
float alpha = Tf / (Tf + dt);
float y = alpha * Last_y + (1.0f - alpha) * x;
return y;
Last_y = y;
Last_Timesamp = Timesamp;
return y;
}

76
3rd/mt6701.c
View File

@@ -78,9 +78,10 @@ void MT6701_iic_read_angel(void)
if(DL_I2C_getControllerStatus(I2C_1_INST) &
DL_I2C_CONTROLLER_STATUS_ERROR)
{
//printf("i2c error \n");
printf("i2c error --------------------------------------------------------------------------------\n");
/* LED will remain high if there is an error */
__BKPT(0);
return;
}
/* Wait for I2C to be Idle */
@@ -110,51 +111,60 @@ volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
return angle_f_rad;
angle_f_rad = (float)angle * _2PI / 16384;
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if( fabs(D_Angle) > (0.8f*2*PI) )
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1);
}
Last_Angle = Angle;
return (full_rotations * 2 * PI + Last_Angle);
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if(fabs(D_Angle) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
}
Last_Angle = Angle;
return (full_rotations * 2 * PI + Last_Angle);
}
volatile float Last_Vel_ts = 0.0;
volatile float Vel_Last_Angle = 0.0;
float GetVelocity(void)
{
volatile float dt = 0.0;
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts) dt = (Last_Vel_ts - Vel_ts)/9*1e-6f;
else dt = (0xFFFFFF - Vel_ts + Last_Vel_ts)/9*1e-6f;
if(dt < 0.0001) dt = 10000;
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
volatile float dt = 0.0;
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts)
{
dt = (Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 9 * 1e-6f;
}
float velocity = (Vel_Angle - Vel_Last_Angle)/dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
if(dt < 0.0001)
{
dt = 10000;
}
float Vel_Angle = GetAngle();
float dv = Vel_Angle - Vel_Last_Angle;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;
return velocity;
}
void MT6701_get_angle_degree(void)

64
3rd/pid_control.c
View File

@@ -1,12 +1,12 @@
#include <stdint.h>
#include <ti_msp_dl_config.h>
#define limit 6.3
#define Output_ramp 10000
//限幅
float _constrain(float amt, float low, float high)
float _constrain(float amt, float low, float high)
{
return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}
unsigned long Timestamp_Last = 0.0;
@@ -15,31 +15,37 @@ float Last_intergration = 0.0;
float Last_Output = 0.0;
float PID_Controller(float Kp, float Ki, float Kd, float Error)
{
float Ts = 0.0;
//TODO
uint32_t Timestamp = 0;
//uint32_t Timestamp = SysTick->VAL;
if(Timestamp < Timestamp_Last) Ts = (float)(Timestamp_Last - Timestamp)/9*1e-6;
else
Ts = (0xFFFFFF - Timestamp + Timestamp_Last)/9*1e-6;
if(Ts<=0 || Ts > 0.05f) Ts = 0.001;
float proportion = Kp * Error;//P环
float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
intergration = _constrain(intergration, -limit, limit);
float differential = Kd * (Error - Last_Error)/Ts;//D环
float Output = proportion + intergration + differential;
Output = _constrain(Output, -limit, limit);
Last_Error = Error;
Last_intergration = intergration;
Last_Output = Output;
Timestamp_Last = Timestamp;
return Output;
float Ts = 0.0;
uint32_t Timestamp = DL_SYSTICK_getValue();
if(Timestamp < Timestamp_Last)
{
Ts = (float)(Timestamp_Last - Timestamp) / 32 * 1e-6;
}
else
{
Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 32 * 1e-6;
}
if(Ts <= 0 || Ts > 0.05f)
{
Ts = 0.001;
}
float proportion = Kp * Error;//P环
float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
intergration = _constrain(intergration, -limit, limit);
float differential = Kd * (Error - Last_Error) / Ts; //D环
float Output = proportion + intergration + differential;
Output = _constrain(Output, -limit, limit);
Last_Error = Error;
Last_intergration = intergration;
Last_Output = Output;
Timestamp_Last = Timestamp;
return Output;
}

6
3rd/pwm.c
View File

@@ -3,17 +3,17 @@
void PWM_Channel1(uint16_t Compare)
{
DL_TimerA_setCaptureCompareValue(PWM_0_INST,Compare,GPIO_PWM_0_C0_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C0_IDX);
}
void PWM_Channel2(uint16_t Compare)
{
DL_TimerA_setCaptureCompareValue(PWM_0_INST,Compare,GPIO_PWM_0_C1_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C1_IDX);
}
void PWM_Channel3(uint16_t Compare)
{
DL_TimerA_setCaptureCompareValue(PWM_0_INST,Compare,GPIO_PWM_0_C2_IDX);
DL_TimerA_setCaptureCompareValue(PWM_0_INST, Compare, GPIO_PWM_0_C2_IDX);
}

12
3rd/uart_redircet.c
View File

@@ -5,10 +5,10 @@
/* ARM Compiler 6 半主机模式禁用声明 */
#if (__ARMCC_VERSION >= 6010050)
__asm(".global __use_no_semihosting\n\t");
__asm(".global __ARM_use_no_argv \n\t");
__asm(".global __use_no_semihosting\n\t");
__asm(".global __ARM_use_no_argv \n\t");
#else
#pragma import(__use_no_semihosting)
#pragma import(__use_no_semihosting)
#endif
FILE __stdout;
@@ -17,7 +17,7 @@ FILE __stdout;
void _sys_exit(int x)
{
x = x;
while (1)
while(1)
; /* 死循环防止程序异常退出 */
}
@@ -25,7 +25,7 @@ void _sys_exit(int x)
int fputc(int c, FILE *stream)
{
/* 等待UART发送缓冲区就绪 */
while (DL_UART_Main_isBusy(UART_0_INST))
while(DL_UART_Main_isBusy(UART_0_INST))
;
/* 发送字符 */
@@ -37,7 +37,7 @@ int fputs(const char *restrict s, FILE *restrict stream)
{
uint16_t i, len;
len = strlen(s);
for (i = 0; i < len; i++)
for(i = 0; i < len; i++)
{
fputc(s[i], stream);
}

7
empty.c
View File

@@ -45,6 +45,9 @@ const float num_f = 0.123456f;
volatile uint16_t count = 0 ;
int pp = 7; //电机极对数
int Dir = 1; //电机编码器方向
int main(void)
{
SYSCFG_DL_init();
@@ -53,10 +56,12 @@ int main(void)
while (1) {
DL_GPIO_togglePins(LED_PORT,LED_PA0_PIN);
//MT6701_get_angle_degree();
delay_ms(20);
delay_ms(50);
volatile float angle_rad = GetAngle();
//printf("test \n");
printf("angle %.5f \n" , angle_rad);
volatile uint32_t tick = DL_SYSTICK_getValue();
printf("systick is %x \n",tick);
PWM_Channel1(count ++);
if(count == 750)

23
empty.syscfg
View File

@@ -9,15 +9,16 @@
/**
* Import the modules used in this configuration.
*/
const GPIO = scripting.addModule("/ti/driverlib/GPIO", {}, false);
const GPIO1 = GPIO.addInstance();
const I2C = scripting.addModule("/ti/driverlib/I2C", {}, false);
const I2C1 = I2C.addInstance();
const PWM = scripting.addModule("/ti/driverlib/PWM", {}, false);
const PWM1 = PWM.addInstance();
const SYSCTL = scripting.addModule("/ti/driverlib/SYSCTL");
const UART = scripting.addModule("/ti/driverlib/UART", {}, false);
const UART1 = UART.addInstance();
const GPIO = scripting.addModule("/ti/driverlib/GPIO", {}, false);
const GPIO1 = GPIO.addInstance();
const I2C = scripting.addModule("/ti/driverlib/I2C", {}, false);
const I2C1 = I2C.addInstance();
const PWM = scripting.addModule("/ti/driverlib/PWM", {}, false);
const PWM1 = PWM.addInstance();
const SYSCTL = scripting.addModule("/ti/driverlib/SYSCTL");
const SYSTICK = scripting.addModule("/ti/driverlib/SYSTICK");
const UART = scripting.addModule("/ti/driverlib/UART", {}, false);
const UART1 = UART.addInstance();
/**
* Write custom configuration values to the imported modules.
@@ -71,6 +72,10 @@ PWM1.ccp2PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric6";
SYSCTL.forceDefaultClkConfig = true;
SYSCTL.peripheral.$assign = "SYSCTL";
SYSTICK.periodEnable = true;
SYSTICK.systickEnable = true;
SYSTICK.period = 16000000;
UART1.$name = "UART_0";
UART1.rxFifoThreshold = "DL_UART_RX_FIFO_LEVEL_ONE_ENTRY";
UART1.targetBaudRate = 115200;

30
keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx
View File

@@ -449,6 +449,36 @@
<FileType>5</FileType>
<FilePath>..\3rd\pwm.h</FilePath>
</File>
<File>
<FileName>dfoc.c</FileName>
<FileType>1</FileType>
<FilePath>..\3rd\dfoc.c</FilePath>
</File>
<File>
<FileName>dfoc.h</FileName>
<FileType>5</FileType>
<FilePath>..\3rd\dfoc.h</FilePath>
</File>
<File>
<FileName>lowpass_filter.c</FileName>
<FileType>1</FileType>
<FilePath>..\3rd\lowpass_filter.c</FilePath>
</File>
<File>
<FileName>lowpass_filter.h</FileName>
<FileType>5</FileType>
<FilePath>..\3rd\lowpass_filter.h</FilePath>
</File>
<File>
<FileName>pid_control.c</FileName>
<FileType>1</FileType>
<FilePath>..\3rd\pid_control.c</FilePath>
</File>
<File>
<FileName>pid_control.h</FileName>
<FileType>5</FileType>
<FilePath>..\3rd\pid_control.h</FilePath>
</File>
</Files>
</Group>
</Groups>

11
ti_msp_dl_config.c
View File

@@ -56,6 +56,7 @@ SYSCONFIG_WEAK void SYSCFG_DL_init(void)
SYSCFG_DL_I2C_1_init();
SYSCFG_DL_UART_0_init();
SYSCFG_DL_DMA_init();
SYSCFG_DL_SYSTICK_init();
/* Ensure backup structures have no valid state */
gPWM_0Backup.backupRdy = false;
@@ -93,12 +94,14 @@ SYSCONFIG_WEAK void SYSCFG_DL_initPower(void)
DL_UART_Main_reset(UART_0_INST);
DL_GPIO_enablePower(GPIOA);
DL_GPIO_enablePower(GPIOB);
DL_TimerA_enablePower(PWM_0_INST);
DL_I2C_enablePower(I2C_1_INST);
DL_UART_Main_enablePower(UART_0_INST);
delay_cycles(POWER_STARTUP_DELAY);
}
@@ -296,3 +299,11 @@ SYSCONFIG_WEAK void SYSCFG_DL_DMA_init(void){
}
SYSCONFIG_WEAK void SYSCFG_DL_SYSTICK_init(void)
{
/* Initialize the period to 500.00 ms */
DL_SYSTICK_init(16000000);
/* Enable the SysTick and start counting */
DL_SYSTICK_enable();
}

3
ti_msp_dl_config.h
View File

@@ -153,6 +153,8 @@ extern "C" {
#define LED_PA0_IOMUX (IOMUX_PINCM1)
/* clang-format on */
void SYSCFG_DL_init(void);
@@ -164,6 +166,7 @@ void SYSCFG_DL_I2C_1_init(void);
void SYSCFG_DL_UART_0_init(void);
void SYSCFG_DL_DMA_init(void);
void SYSCFG_DL_SYSTICK_init(void);
bool SYSCFG_DL_saveConfiguration(void);
bool SYSCFG_DL_restoreConfiguration(void);