From 8908bf2332b11a0e2b99282de3a352bb1c50ca6f Mon Sep 17 00:00:00 2001 From: 4x-tech Date: Wed, 12 Nov 2025 10:32:45 +0800 Subject: [PATCH] pwm uart gipo i2c --- 3rd/dfoc.c | 129 +++++++++++++++++++++++++++++++++++++++++++ 3rd/dfoc.h | 19 +++++++ 3rd/lowpass_filter.c | 42 ++++++++++++++ 3rd/lowpass_filter.h | 12 ++++ 3rd/mt6701.c | 4 +- 3rd/pid_control.c | 45 +++++++++++++++ 3rd/pid_control.h | 9 +++ 3rd/pwm.c | 25 +++++++++ 3rd/pwm.h | 11 ++++ empty.c | 1 + empty.syscfg | 21 +++++++ ti_msp_dl_config.c | 96 ++++++++++++++++++++++++++++++++ ti_msp_dl_config.h | 27 +++++++++ 13 files changed, 440 insertions(+), 1 deletion(-) create mode 100644 3rd/dfoc.c create mode 100644 3rd/dfoc.h create mode 100644 3rd/lowpass_filter.c create mode 100644 3rd/lowpass_filter.h create mode 100644 3rd/pid_control.c create mode 100644 3rd/pid_control.h create mode 100644 3rd/pwm.c create mode 100644 3rd/pwm.h diff --git a/3rd/dfoc.c b/3rd/dfoc.c new file mode 100644 index 0000000..3b31d74 --- /dev/null +++ b/3rd/dfoc.c @@ -0,0 +1,129 @@ + +#include +#include "pwm.h" +#include "mt6701.h" +#include "delay.h" +#include "lowpass_filter.h" +#include "pid_control.h" +#include + +#define PI 3.14159265359f +#define _3PI_2 4.71238898f +#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 voltage_limit = 8; +float voltage_power_supply = 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); +} + +//限制幅值 +float constrain(float amt, float low, float high) +{ + return ((amt(high)?(high):(amt))); +} + +//将角度归化到0-2PI +float normalizeAngle(float angle) +{ + 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); +} + +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); + +} + +//FOC核心算法，克拉克逆变换/帕克逆变换 +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; +} + +void Check_Sensor(void) +{ + 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(); +} + + +// 单角度环 +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()); +} + + + + + + + + diff --git a/3rd/dfoc.h b/3rd/dfoc.h new file mode 100644 index 0000000..ad07052 --- /dev/null +++ b/3rd/dfoc.h @@ -0,0 +1,19 @@ +#ifndef __DFOC_H +#define __DFOC_H + +void Motor_en(void); +float constrain(float amt, float low, float high); +void SetPwm(float Ua, float Ub, float Uc); +float normalizeAngle(float angle); +void SetPhaseVoltage(float Uq, float Ud, float angle_el); +void Check_Sensor(void); +void FOC_Init(float power); +float electricAngle(void); +float GetCommand(void); +void Set_Angle(float Target); + + + +#endif + + diff --git a/3rd/lowpass_filter.c b/3rd/lowpass_filter.c new file mode 100644 index 0000000..74c02b2 --- /dev/null +++ b/3rd/lowpass_filter.c @@ -0,0 +1,42 @@ + +#include + + +float y = 0; +float Lowpassfilter_sim(float x) +{ + 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; + + //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; + + 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; + + + return y; +} + diff --git a/3rd/lowpass_filter.h b/3rd/lowpass_filter.h new file mode 100644 index 0000000..4189639 --- /dev/null +++ b/3rd/lowpass_filter.h @@ -0,0 +1,12 @@ +#ifndef __LOWPASS_FILTER_H +#define __LOWPASS_FILTER_H + + +float Lowpassfilter(float Tf, float x); +float Lowpassfilter_sim(float x); + + + +#endif + + diff --git a/3rd/mt6701.c b/3rd/mt6701.c index 27c008b..2676785 100644 --- a/3rd/mt6701.c +++ b/3rd/mt6701.c @@ -1,6 +1,7 @@ #include "MT6701.h" #include "ti_msp_dl_config.h" - +#include "uart_redircet.h" +#include "stdio.h" volatile int16_t angle; volatile float angle_f; volatile float angle_f_rad; @@ -77,6 +78,7 @@ void MT6701_iic_read_angel(void) if(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_ERROR) { + //printf("i2c error \n"); /* LED will remain high if there is an error */ __BKPT(0); } diff --git a/3rd/pid_control.c b/3rd/pid_control.c new file mode 100644 index 0000000..0c6ac22 --- /dev/null +++ b/3rd/pid_control.c @@ -0,0 +1,45 @@ +#include + +#define limit 6.3 +#define Output_ramp 10000 + +//限幅 +float _constrain(float amt, float low, float high) +{ + return ((amt(high)?(high):(amt))); +} + +unsigned long Timestamp_Last = 0.0; +float Last_Error = 0.0; +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; +} + diff --git a/3rd/pid_control.h b/3rd/pid_control.h new file mode 100644 index 0000000..04155c1 --- /dev/null +++ b/3rd/pid_control.h @@ -0,0 +1,9 @@ +#ifndef __PID_CONTROL_H +#define __PID_CONTROL_H + +float PID_Controller(float Kp, float Ki, float Kd, float Error); +float _constrain(float amt, float low, float high); + +#endif + + diff --git a/3rd/pwm.c b/3rd/pwm.c new file mode 100644 index 0000000..c1d5998 --- /dev/null +++ b/3rd/pwm.c @@ -0,0 +1,25 @@ +#include "pwm.h" +#include "ti_msp_dl_config.h" + +void PWM_Channel1(uint16_t Compare) +{ + 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); +} + +void PWM_Channel3(uint16_t Compare) +{ + //DL_TimerA_setCaptureCompareValue(PWM_0_INST,Compare,GPIO_PWM_0_C2_IDX); +} + + + + + + + + diff --git a/3rd/pwm.h b/3rd/pwm.h new file mode 100644 index 0000000..2eb0e99 --- /dev/null +++ b/3rd/pwm.h @@ -0,0 +1,11 @@ +#ifndef __PWM_H +#define __PWM_H +#include + +void PWM_Channel1(uint16_t Compare); +void PWM_Channel2(uint16_t Compare); +void PWM_Channel3(uint16_t Compare); + +#endif + + diff --git a/empty.c b/empty.c index 5f4c9ad..5cecd43 100644 --- a/empty.c +++ b/empty.c @@ -47,6 +47,7 @@ int main(void) { SYSCFG_DL_init(); NVIC_EnableIRQ(UART_0_INST_INT_IRQN); + DL_TimerA_startCounter(PWM_0_INST); while (1) { DL_GPIO_togglePins(LED_PORT,LED_PA0_PIN); //MT6701_get_angle_degree(); diff --git a/empty.syscfg b/empty.syscfg index f51c5f8..029e7a2 100644 --- a/empty.syscfg +++ b/empty.syscfg @@ -13,6 +13,8 @@ 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(); @@ -47,6 +49,25 @@ I2C1.sclPinConfig.onlyInternalResistor = scripting.forceWrite(false); I2C1.sclPinConfig.passedPeripheralType = scripting.forceWrite("Digital"); I2C1.sclPinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric3"; +PWM1.$name = "PWM_0"; +PWM1.ccIndex = [0,1,2]; +PWM1.clockDivider = 2; +PWM1.timerCount = 2000; +PWM1.pwmMode = "CENTER_ALIGN"; +PWM1.PWM_CHANNEL_0.$name = "ti_driverlib_pwm_PWMTimerCC0"; +PWM1.PWM_CHANNEL_0.dutyCycle = 25; +PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1"; +PWM1.PWM_CHANNEL_1.dutyCycle = 50; +PWM1.ccp0PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric4"; +PWM1.ccp1PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric5"; +PWM1.peripheral.$assign = "TIMA0"; +PWM1.peripheral.ccp0Pin.$assign = "PA21"; +PWM1.peripheral.ccp1Pin.$assign = "PA22"; +PWM1.peripheral.ccp2Pin.$assign = "PB20"; +PWM1.PWM_CHANNEL_2.$name = "ti_driverlib_pwm_PWMTimerCC2"; +PWM1.PWM_CHANNEL_2.dutyCycle = 75; +PWM1.ccp2PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric6"; + SYSCTL.forceDefaultClkConfig = true; SYSCTL.peripheral.$assign = "SYSCTL"; diff --git a/ti_msp_dl_config.c b/ti_msp_dl_config.c index 329922c..1c4d201 100644 --- a/ti_msp_dl_config.c +++ b/ti_msp_dl_config.c @@ -40,6 +40,8 @@ #include "ti_msp_dl_config.h" +DL_TimerA_backupConfig gPWM_0Backup; + /* * ======== SYSCFG_DL_init ======== * Perform any initialization needed before using any board APIs @@ -50,23 +52,50 @@ SYSCONFIG_WEAK void SYSCFG_DL_init(void) SYSCFG_DL_GPIO_init(); /* Module-Specific Initializations*/ SYSCFG_DL_SYSCTL_init(); + SYSCFG_DL_PWM_0_init(); SYSCFG_DL_I2C_1_init(); SYSCFG_DL_UART_0_init(); SYSCFG_DL_DMA_init(); + /* Ensure backup structures have no valid state */ + gPWM_0Backup.backupRdy = false; + + +} +/* + * User should take care to save and restore register configuration in application. + * See Retention Configuration section for more details. + */ +SYSCONFIG_WEAK bool SYSCFG_DL_saveConfiguration(void) +{ + bool retStatus = true; + + retStatus &= DL_TimerA_saveConfiguration(PWM_0_INST, &gPWM_0Backup); + + return retStatus; } +SYSCONFIG_WEAK bool SYSCFG_DL_restoreConfiguration(void) +{ + bool retStatus = true; + + retStatus &= DL_TimerA_restoreConfiguration(PWM_0_INST, &gPWM_0Backup, false); + + return retStatus; +} SYSCONFIG_WEAK void SYSCFG_DL_initPower(void) { DL_GPIO_reset(GPIOA); DL_GPIO_reset(GPIOB); + DL_TimerA_reset(PWM_0_INST); DL_I2C_reset(I2C_1_INST); 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); @@ -76,6 +105,13 @@ SYSCONFIG_WEAK void SYSCFG_DL_initPower(void) SYSCONFIG_WEAK void SYSCFG_DL_GPIO_init(void) { + DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C0_IOMUX,GPIO_PWM_0_C0_IOMUX_FUNC); + DL_GPIO_enableOutput(GPIO_PWM_0_C0_PORT, GPIO_PWM_0_C0_PIN); + DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C1_IOMUX,GPIO_PWM_0_C1_IOMUX_FUNC); + DL_GPIO_enableOutput(GPIO_PWM_0_C1_PORT, GPIO_PWM_0_C1_PIN); + DL_GPIO_initPeripheralOutputFunction(GPIO_PWM_0_C2_IOMUX,GPIO_PWM_0_C2_IOMUX_FUNC); + DL_GPIO_enableOutput(GPIO_PWM_0_C2_PORT, GPIO_PWM_0_C2_PIN); + DL_GPIO_initPeripheralInputFunctionFeatures(GPIO_I2C_1_IOMUX_SDA, GPIO_I2C_1_IOMUX_SDA_FUNC, DL_GPIO_INVERSION_DISABLE, DL_GPIO_RESISTOR_NONE, DL_GPIO_HYSTERESIS_DISABLE, @@ -116,6 +152,66 @@ SYSCONFIG_WEAK void SYSCFG_DL_SYSCTL_init(void) } +/* + * Timer clock configuration to be sourced by / 2 (16000000 Hz) + * timerClkFreq = (timerClkSrc / (timerClkDivRatio * (timerClkPrescale + 1))) + * 16000000 Hz = 16000000 Hz / (2 * (0 + 1)) + */ +static const DL_TimerA_ClockConfig gPWM_0ClockConfig = { + .clockSel = DL_TIMER_CLOCK_BUSCLK, + .divideRatio = DL_TIMER_CLOCK_DIVIDE_2, + .prescale = 0U +}; + +static const DL_TimerA_PWMConfig gPWM_0Config = { + .pwmMode = DL_TIMER_PWM_MODE_CENTER_ALIGN, + .period = 2000, + .isTimerWithFourCC = true, + .startTimer = DL_TIMER_STOP, +}; + +SYSCONFIG_WEAK void SYSCFG_DL_PWM_0_init(void) { + + DL_TimerA_setClockConfig( + PWM_0_INST, (DL_TimerA_ClockConfig *) &gPWM_0ClockConfig); + + DL_TimerA_initPWMMode( + PWM_0_INST, (DL_TimerA_PWMConfig *) &gPWM_0Config); + + // Set Counter control to the smallest CC index being used + DL_TimerA_setCounterControl(PWM_0_INST,DL_TIMER_CZC_CCCTL0_ZCOND,DL_TIMER_CAC_CCCTL0_ACOND,DL_TIMER_CLC_CCCTL0_LCOND); + + DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW, + DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL, + DL_TIMERA_CAPTURE_COMPARE_0_INDEX); + + DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_0_INDEX); + DL_TimerA_setCaptureCompareValue(PWM_0_INST, 750, DL_TIMER_CC_0_INDEX); + + DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW, + DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL, + DL_TIMERA_CAPTURE_COMPARE_1_INDEX); + + DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_1_INDEX); + DL_TimerA_setCaptureCompareValue(PWM_0_INST, 500, DL_TIMER_CC_1_INDEX); + + DL_TimerA_setCaptureCompareOutCtl(PWM_0_INST, DL_TIMER_CC_OCTL_INIT_VAL_LOW, + DL_TIMER_CC_OCTL_INV_OUT_DISABLED, DL_TIMER_CC_OCTL_SRC_FUNCVAL, + DL_TIMERA_CAPTURE_COMPARE_2_INDEX); + + DL_TimerA_setCaptCompUpdateMethod(PWM_0_INST, DL_TIMER_CC_UPDATE_METHOD_IMMEDIATE, DL_TIMERA_CAPTURE_COMPARE_2_INDEX); + DL_TimerA_setCaptureCompareValue(PWM_0_INST, 250, DL_TIMER_CC_2_INDEX); + + DL_TimerA_enableClock(PWM_0_INST); + + + + DL_TimerA_setCCPDirection(PWM_0_INST , DL_TIMER_CC0_OUTPUT | DL_TIMER_CC1_OUTPUT | DL_TIMER_CC2_OUTPUT ); + + +} + + static const DL_I2C_ClockConfig gI2C_1ClockConfig = { .clockSel = DL_I2C_CLOCK_BUSCLK, .divideRatio = DL_I2C_CLOCK_DIVIDE_1, diff --git a/ti_msp_dl_config.h b/ti_msp_dl_config.h index 4a8fea5..2df1b93 100644 --- a/ti_msp_dl_config.h +++ b/ti_msp_dl_config.h @@ -77,6 +77,32 @@ extern "C" { +/* Defines for PWM_0 */ +#define PWM_0_INST TIMA0 +#define PWM_0_INST_IRQHandler TIMA0_IRQHandler +#define PWM_0_INST_INT_IRQN (TIMA0_INT_IRQn) +#define PWM_0_INST_CLK_FREQ 16000000 +/* GPIO defines for channel 0 */ +#define GPIO_PWM_0_C0_PORT GPIOA +#define GPIO_PWM_0_C0_PIN DL_GPIO_PIN_21 +#define GPIO_PWM_0_C0_IOMUX (IOMUX_PINCM46) +#define GPIO_PWM_0_C0_IOMUX_FUNC IOMUX_PINCM46_PF_TIMA0_CCP0 +#define GPIO_PWM_0_C0_IDX DL_TIMER_CC_0_INDEX +/* GPIO defines for channel 1 */ +#define GPIO_PWM_0_C1_PORT GPIOA +#define GPIO_PWM_0_C1_PIN DL_GPIO_PIN_22 +#define GPIO_PWM_0_C1_IOMUX (IOMUX_PINCM47) +#define GPIO_PWM_0_C1_IOMUX_FUNC IOMUX_PINCM47_PF_TIMA0_CCP1 +#define GPIO_PWM_0_C1_IDX DL_TIMER_CC_1_INDEX +/* GPIO defines for channel 2 */ +#define GPIO_PWM_0_C2_PORT GPIOB +#define GPIO_PWM_0_C2_PIN DL_GPIO_PIN_20 +#define GPIO_PWM_0_C2_IOMUX (IOMUX_PINCM48) +#define GPIO_PWM_0_C2_IOMUX_FUNC IOMUX_PINCM48_PF_TIMA0_CCP2 +#define GPIO_PWM_0_C2_IDX DL_TIMER_CC_2_INDEX + + + /* Defines for I2C_1 */ #define I2C_1_INST I2C1 @@ -133,6 +159,7 @@ void SYSCFG_DL_init(void); void SYSCFG_DL_initPower(void); void SYSCFG_DL_GPIO_init(void); void SYSCFG_DL_SYSCTL_init(void); +void SYSCFG_DL_PWM_0_init(void); void SYSCFG_DL_I2C_1_init(void); void SYSCFG_DL_UART_0_init(void); void SYSCFG_DL_DMA_init(void);