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

50
3rd/dfoc.c
View File

@@ -12,39 +12,39 @@
#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)
@@ -54,9 +54,9 @@ float electricAngle(void)
void SetPwm(float Ua, float Ub, float Uc)
{
float U_a=0.0;
float U_b=0.0;
float U_c=0.0;
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);
@@ -77,17 +77,17 @@ 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);
// angle_el = normalizeAngle(angle_el);
test_angle = angle_el - last_test_angle;
Ualpha = -Uq*sin(angle_el);
Ubeta = Uq*cos(angle_el);
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;
Ub = (sqrt(3) * Ubeta - Ualpha) / 2 + voltage_power_supply / 2;
Uc = -(Ualpha + sqrt(3) * Ubeta) / 2 + voltage_power_supply / 2;
SetPwm(Ua,Ub,Uc);
SetPwm(Ua, Ub, Uc);
last_test_angle = angle_el;
}
@@ -116,7 +116,7 @@ void FOC_Init(float power)
void Set_Angle(float Target)
{
float angle = GetAngle();
float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir*angle)*180/PI);
float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI);
SetPhaseVoltage(Uq, 0, electricAngle());
}

24
3rd/lowpass_filter.c
View File

@@ -1,11 +1,11 @@
#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;
float out = 0.9 * x + 0.1 * y;
y = x;
return out;
}
@@ -16,15 +16,21 @@ float Lowpassfilter(float Tf, float x)
{
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;
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)/9*1e-6;
{
dt = (float)(0xFFFFFF - Timesamp + Last_Timesamp) / 32 * 1e-6;
}
if(dt<0.0||dt==0) dt = 0.0015f;
else if(dt>0.005f)
if(dt < 0.0 || dt == 0)
{
dt = 0.0015f;
}
else if(dt > 0.005f)
{
Last_y = x;
Last_Timesamp = Timesamp;

24
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 */
@@ -123,9 +124,9 @@ float GetAngle(void)
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
if( fabs(D_Angle) > (0.8f*2*PI) )
if(fabs(D_Angle) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1);
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
}
Last_Angle = Angle;
@@ -139,16 +140,25 @@ 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(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;
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;
float velocity = (Vel_Angle - Vel_Last_Angle) / dt;
Last_Vel_ts = Vel_ts;
Vel_Last_Angle = Vel_Angle;

24
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)
{
return ((amt<low)?(low):((amt)>(high)?(high):(amt)));
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}
unsigned long Timestamp_Last = 0.0;
@@ -16,21 +16,27 @@ 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;
uint32_t Timestamp = DL_SYSTICK_getValue();
if(Timestamp < Timestamp_Last)
{
Ts = (float)(Timestamp_Last - Timestamp) / 32 * 1e-6;
}
else
Ts = (0xFFFFFF - Timestamp + Timestamp_Last)/9*1e-6;
{
Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 32 * 1e-6;
}
if(Ts<=0 || Ts > 0.05f) Ts = 0.001;
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 differential = Kd * (Error - Last_Error) / Ts; //D环
float Output = proportion + intergration + differential;
Output = _constrain(Output, -limit, limit);

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)

5
empty.syscfg
View File

@@ -16,6 +16,7 @@ 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();
@@ -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);