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

pwm uart gipo i2c

Browse Source
This commit is contained in:
4x-tech
2025-11-12 10:32:45 +08:00
parent cd01dcf51e
commit 8908bf2332
13 changed files with 440 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
3rd/dfoc.c Normal file
View File

@@ -0,0 +1,129 @@
#include <stdlib.h>
#include "pwm.h"
#include "mt6701.h"
#include "delay.h"
#include "lowpass_filter.h"
#include "pid_control.h"
#include <math.h>
#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<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 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());
}

19
3rd/dfoc.h Normal file
View File

@@ -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

42
3rd/lowpass_filter.c Normal file
View File

@@ -0,0 +1,42 @@
#include <stdint.h>
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;
}

12
3rd/lowpass_filter.h Normal file
View File

@@ -0,0 +1,12 @@
#ifndef __LOWPASS_FILTER_H
#define __LOWPASS_FILTER_H
float Lowpassfilter(float Tf, float x);
float Lowpassfilter_sim(float x);
#endif

4
3rd/mt6701.c
View File

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

45
3rd/pid_control.c Normal file
View File

@@ -0,0 +1,45 @@
#include <stdint.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)));
}
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;
}

9
3rd/pid_control.h Normal file
View File

@@ -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

25
3rd/pwm.c Normal file
View File

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

11
3rd/pwm.h Normal file
View File

@@ -0,0 +1,11 @@
#ifndef __PWM_H
#define __PWM_H
#include <stdint.h>
void PWM_Channel1(uint16_t Compare);
void PWM_Channel2(uint16_t Compare);
void PWM_Channel3(uint16_t Compare);
#endif

1
empty.c
View File

@@ -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();

21
empty.syscfg
View File

@@ -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";

96
ti_msp_dl_config.c
View File

@@ -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,

27
ti_msp_dl_config.h
View File

@@ -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);