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

add uart rx; 4 bytes cmd parse

Browse Source
This commit is contained in:
4x-tech
2025-11-17 10:13:18 +08:00
parent 5b49a0ee24
commit 6de9d53e1c
7 changed files with 237 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
3rd/config.h
View File

@@ -1,7 +1,7 @@
#ifndef CONFIG_H
#define CONFIG_H
#define DEBUG_ENABLED true
#define DEBUG_ENABLED false
#define DEBUG_MT_ENABLED false
#define DEBUG_DFOC false
#endif /* ti_msp_dl_config_h */
#define DEBUG_DFOC_ENABLED false
#endif

25
3rd/dfoc.c
View File

@@ -16,7 +16,7 @@
#define _2_SQRT3 1.15470053838f
volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0;
const float voltage_limit = 12;
const float voltage_limit = 8;
const float voltage_power_supply = 12.0f;
volatile float zero_electric_Angle = 0.0;
@@ -66,7 +66,7 @@ void SetPwm(float Ua, float Ub, float Uc)
U_a = constrain(Ua, 0.0f, voltage_limit);
U_b = constrain(Ub, 0.0f, voltage_limit);
U_c = constrain(Uc, 0.0f, voltage_limit);
//printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c);
//printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c);
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);
@@ -96,8 +96,8 @@ void SetPhaseVoltage(float Uq, float Ud, float angle_el)
SetPwm(Ua, Ub, Uc);
last_test_angle = angle_el;
}
@@ -124,8 +124,17 @@ void FOC_Init(float power)
// 单角度环
void Set_Angle(float Target)
{
volatile float angle = GetAngle();
volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * angle) * 180 / PI);
volatile float langle = GetAngle();
if(DEBUG_ENABLED & DEBUG_DFOC_ENABLED)
{
printf("angle readback in dfoc.c is %f \n", langle);
}
volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * langle) * 180 / PI);
//volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI);
if(DEBUG_ENABLED)
{
printf("Uq is %f \n", Uq);
}
SetPhaseVoltage(Uq, 0, electricAngle());
}
@@ -157,14 +166,14 @@ float velocityopenloop(float target)
if(DEBUG_ENABLED)
{
//printf("shaft_angle : %f -- target : %f -- Ts : %f",shaft_angle,target,Ts);
//printf("shaft_angle : %f -- target : %f -- Ts : %f \n",shaft_angle,target,Ts);
}
Uq = voltage_limit;
SetPhaseVoltage(Uq, 0, shaft_angle);
printf("shaft_angle : %f \n",shaft_angle);
printf("shaft_angle : %f \n", shaft_angle);
openloop_timestamp = now_ts;
return Uq;

39
3rd/mt6701.c
View File

@@ -10,6 +10,8 @@ volatile float angle_f_rad;
volatile bool gIsI2cError = false;
#define DEBUG_I2C false
/* Data sent to the Target */
uint8_t gTxPacket[I2C_TX_PACKET_SIZE] =
{
@@ -52,7 +54,7 @@ void MT6701_iic_read_angel(void)
*/
gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) *
(CPUCLK_FREQ / gClockSelFreq);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before writing -------\n");
@@ -75,7 +77,7 @@ void MT6701_iic_read_angel(void)
DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS,
DL_I2C_CONTROLLER_DIRECTION_TX, I2C_TX_PACKET_SIZE);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c writing done -------\n");
}
@@ -94,7 +96,7 @@ void MT6701_iic_read_angel(void)
{
gIsI2cError = true;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c error ------------------------------------------\n");
}
@@ -111,7 +113,7 @@ void MT6701_iic_read_angel(void)
/* Add delay between transfers */
delay_cycles(1000);
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c before reading -------\n");
}
@@ -131,7 +133,7 @@ void MT6701_iic_read_angel(void)
gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST);
}
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
if(DEBUG_ENABLED & DEBUG_MT_ENABLED & DEBUG_I2C)
{
printf("i2c reading done -------\n");
}
@@ -140,29 +142,36 @@ void MT6701_iic_read_angel(void)
}
volatile float Last_ts = 0.0;
volatile float last_angle = 0.0;
// 单圈值
float GetAngle_NoTrack(void)
{
MT6701_iic_read_angel();
angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2);
angle_f_rad = (float)angle * _2PI / 16384;
if(DEBUG_ENABLED & DEBUG_MT_ENABLED)
{
printf("angle_rad read back is %f \n", angle_f_rad);
}
return angle_f_rad;
}
volatile float full_rotations = 0.0;
volatile float Last_Angle = 0.0;
volatile float Last_Angle_rad = 0.0;
//多圈值
float GetAngle(void)
{
volatile float D_Angle = 0.0;
volatile float Angle = GetAngle_NoTrack();
D_Angle = Angle - Last_Angle;
volatile float D_Angle_rad = 0.0;
volatile float Angle_rad = GetAngle_NoTrack();
D_Angle_rad = Angle_rad - Last_Angle_rad;
if(fabs(D_Angle) > (0.8f * 2 * PI))
if(fabs(D_Angle_rad) > (0.8f * 2 * PI))
{
full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1);
full_rotations = full_rotations + ((D_Angle_rad > 0) ? -1 : 1);
}
Last_Angle = Angle;
Last_Angle_rad = Angle_rad;
return (full_rotations * 2 * PI + Last_Angle);
return (full_rotations * 2 * PI + Last_Angle_rad);
}
volatile float Last_Vel_ts = 0.0;
@@ -173,11 +182,11 @@ float GetVelocity(void)
volatile float Vel_ts = SysTick -> VAL;
if(Vel_ts < Last_Vel_ts)
{
dt = (Last_Vel_ts - Vel_ts) / 9 * 1e-6f;
dt = (Last_Vel_ts - Vel_ts) / 80 * 1e-6f;
}
else
{
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 9 * 1e-6f;
dt = (0xFFFFFF - Vel_ts + Last_Vel_ts) / 80 * 1e-6f;
}
if(dt < 0.0001)

111
empty.c
View File

@@ -38,6 +38,9 @@
#include <string.h>
#include <pwm.h>
#include "config.h"
#include <ctype.h>
#include <stdlib.h>
extern float angle_f;
@@ -47,44 +50,120 @@ extern bool gIsI2cError;
const float num_f = 0.123456f;
volatile uint16_t count = 0 ;
const float Target = 10; //串口目标值
volatile float Target = 0; //串口目标值
const int pp = 7; //电机极对数
const int Dir = -1; //电机编码器方向
const int Dir = 1; //电机编码器方向
#define UART_PACKET_SIZE (4)
/* Data received from UART */
volatile uint8_t gUartRxPacket[UART_PACKET_SIZE] ={'a','b','c','d'};
volatile bool gCheckUART;
void parse_uart_cmd(void)
{
char num[10]={0};
int i = 1;
int j = 0;
if(gUartRxPacket[0] == 'T' && gUartRxPacket[UART_PACKET_SIZE-1] == '\n')//数据第一位与数据的最后一位
{
if(gUartRxPacket[1] != '-')//接受到的数据为正数
{
while (gUartRxPacket[i] != '\n' && (isdigit(gUartRxPacket[i]) || gUartRxPacket[i] == '.')) {num[j++] = gUartRxPacket[i++];}
Target = atof(num);
// Serial_SendFloatNumber(Target,3,2);
}
else//接收到的数据为负数
{
i=2;
while (gUartRxPacket[i] != '\n' && (isdigit(gUartRxPacket[i]) || gUartRxPacket[i] == '.')) {num[j++] = gUartRxPacket[i++];}
Target = -(atof(num));
// Serial_SendFloatNumber(Target,3,2);
}
printf("%s",gUartRxPacket);
}
else
{
printf((char *)"Input format error\n");
}
}
int main(void)
{
SYSCFG_DL_init();
/* Configure DMA source, destination and size */
DL_DMA_setSrcAddr(DMA, DMA_CH1_CHAN_ID, (uint32_t)(&UART_0_INST->RXDATA));
DL_DMA_setDestAddr(DMA, DMA_CH1_CHAN_ID, (uint32_t) &gUartRxPacket[0]);
DL_DMA_setTransferSize(DMA, DMA_CH1_CHAN_ID, UART_PACKET_SIZE);
DL_DMA_enableChannel(DMA, DMA_CH1_CHAN_ID);
NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
DL_TimerA_startCounter(PWM_0_INST);
NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
DL_TimerG_startCounter(TIMER_0_INST);
FOC_Init(12.6);
DL_SYSTICK_resetValue();
while(1)
{
DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
//DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
//delay_ms(10);
if(DEBUG_ENABLED)
{
//volatile float angle_rad = GetAngle();
//printf("angle %.5f \n", angle_rad);
}
if(DEBUG_ENABLED)
{
volatile uint32_t tick = DL_SYSTICK_getValue();
//printf("systick is %x \n",tick);
}
//开环
//velocityopenloop(Target);
//闭环
Set_Angle(Target);
//Set_Angle(Target);
if(gCheckUART)
{
gCheckUART = false;
parse_uart_cmd();
Set_Angle(Target);
}
//delay_ms(10);
}
}
void TIMER_0_INST_IRQHandler(void)
{
switch(DL_TimerA_getPendingInterrupt(TIMER_0_INST))
{
case DL_TIMERG_INTERRUPT_ZERO_EVENT:
printf("Target is %f \n", Target);
//printf("%s",gUartRxPacket);
DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN);
break;
default:
break;
}
}
void UART_0_INST_IRQHandler(void)
{
switch (DL_UART_Main_getPendingInterrupt(UART_0_INST)) {
case DL_UART_MAIN_IIDX_DMA_DONE_RX:
gCheckUART = true;
break;
default:
break;
}
}

22
empty.syscfg
View File

@@ -17,6 +17,8 @@ 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 TIMER = scripting.addModule("/ti/driverlib/TIMER", {}, false);
const TIMER1 = TIMER.addInstance();
const UART = scripting.addModule("/ti/driverlib/UART", {}, false);
const UART1 = UART.addInstance();
@@ -77,9 +79,7 @@ PWM1.clockDivider = 2;
PWM1.timerCount = 2000;
PWM1.pwmMode = "CENTER_ALIGN";
PWM1.PWM_CHANNEL_0.$name = "ti_driverlib_pwm_PWMTimerCC0";
PWM1.PWM_CHANNEL_0.ccValue = 1000;
PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1";
PWM1.PWM_CHANNEL_1.ccValue = 1000;
PWM1.ccp0PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric4";
PWM1.ccp1PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric5";
PWM1.peripheral.$assign = "TIMA0";
@@ -87,7 +87,6 @@ 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.ccValue = 1000;
PWM1.ccp2PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric6";
SYSCTL.forceDefaultClkConfig = true;
@@ -97,6 +96,15 @@ SYSTICK.periodEnable = true;
SYSTICK.systickEnable = true;
SYSTICK.period = 16000000;
TIMER1.$name = "TIMER_0";
TIMER1.timerClkDiv = 8;
TIMER1.timerMode = "PERIODIC";
TIMER1.timerClkPrescale = 8;
TIMER1.interrupts = ["ZERO"];
TIMER1.timerClkSrc = "LFCLK";
TIMER1.timerPeriod = "1";
TIMER1.peripheral.$assign = "TIMG0";
UART1.$name = "UART_0";
UART1.rxFifoThreshold = "DL_UART_RX_FIFO_LEVEL_ONE_ENTRY";
UART1.targetBaudRate = 115200;
@@ -104,6 +112,8 @@ UART1.uartClkDiv = "8";
UART1.enableFIFO = true;
UART1.txFifoThreshold = "DL_UART_TX_FIFO_LEVEL_ONE_ENTRY";
UART1.enabledDMATXTriggers = "DL_UART_DMA_INTERRUPT_TX";
UART1.enabledInterrupts = ["DMA_DONE_RX"];
UART1.enabledDMARXTriggers = "DL_UART_DMA_INTERRUPT_RX";
UART1.peripheral.$assign = "UART0";
UART1.peripheral.rxPin.$assign = "PA11";
UART1.peripheral.txPin.$assign = "PA10";
@@ -114,6 +124,11 @@ UART1.DMA_CHANNEL_TX.addressMode = "b2f";
UART1.DMA_CHANNEL_TX.srcLength = "BYTE";
UART1.DMA_CHANNEL_TX.dstLength = "BYTE";
UART1.DMA_CHANNEL_TX.peripheral.$assign = "DMA_CH0";
UART1.DMA_CHANNEL_RX.$name = "DMA_CH1";
UART1.DMA_CHANNEL_RX.addressMode = "f2b";
UART1.DMA_CHANNEL_RX.srcLength = "BYTE";
UART1.DMA_CHANNEL_RX.dstLength = "BYTE";
UART1.DMA_CHANNEL_RX.transferMode = "FULL_CH_REPEAT_SINGLE";
const ProjectConfig = scripting.addModule("/ti/project_config/ProjectConfig", {}, false);
ProjectConfig.migrationCondition = true;
@@ -127,3 +142,4 @@ pinFunction4.peripheral.$suggestSolution = "SYSCTL";
pinFunction4.peripheral.hfxInPin.$suggestSolution = "PA5";
pinFunction4.peripheral.hfxOutPin.$suggestSolution = "PA6";
I2C1.peripheral.$suggestSolution = "I2C1";
UART1.DMA_CHANNEL_RX.peripheral.$suggestSolution = "DMA_CH1";

67
ti_msp_dl_config.c
View File

@@ -53,6 +53,7 @@ SYSCONFIG_WEAK void SYSCFG_DL_init(void)
/* Module-Specific Initializations*/
SYSCFG_DL_SYSCTL_init();
SYSCFG_DL_PWM_0_init();
SYSCFG_DL_TIMER_0_init();
SYSCFG_DL_I2C_1_init();
SYSCFG_DL_UART_0_init();
SYSCFG_DL_DMA_init();
@@ -61,6 +62,7 @@ SYSCONFIG_WEAK void SYSCFG_DL_init(void)
gPWM_0Backup.backupRdy = false;
}
/*
* User should take care to save and restore register configuration in application.
@@ -90,6 +92,7 @@ SYSCONFIG_WEAK void SYSCFG_DL_initPower(void)
DL_GPIO_reset(GPIOA);
DL_GPIO_reset(GPIOB);
DL_TimerA_reset(PWM_0_INST);
DL_TimerG_reset(TIMER_0_INST);
DL_I2C_reset(I2C_1_INST);
DL_UART_Main_reset(UART_0_INST);
@@ -98,6 +101,7 @@ SYSCONFIG_WEAK void SYSCFG_DL_initPower(void)
DL_GPIO_enablePower(GPIOA);
DL_GPIO_enablePower(GPIOB);
DL_TimerA_enablePower(PWM_0_INST);
DL_TimerG_enablePower(TIMER_0_INST);
DL_I2C_enablePower(I2C_1_INST);
DL_UART_Main_enablePower(UART_0_INST);
@@ -212,6 +216,45 @@ SYSCONFIG_WEAK void SYSCFG_DL_PWM_0_init(void) {
DL_TimerA_setCCPDirection(PWM_0_INST , DL_TIMER_CC0_OUTPUT | DL_TIMER_CC1_OUTPUT | DL_TIMER_CC2_OUTPUT );
}
/*
* Timer clock configuration to be sourced by LFCLK / (4096 Hz)
* timerClkFreq = (timerClkSrc / (timerClkDivRatio * (timerClkPrescale + 1)))
* 512 Hz = 4096 Hz / (8 * (7 + 1))
*/
static const DL_TimerG_ClockConfig gTIMER_0ClockConfig = {
.clockSel = DL_TIMER_CLOCK_LFCLK,
.divideRatio = DL_TIMER_CLOCK_DIVIDE_8,
.prescale = 7U,
};
/*
* Timer load value (where the counter starts from) is calculated as (timerPeriod * timerClockFreq) - 1
* TIMER_0_INST_LOAD_VALUE = (1 * 512 Hz) - 1
*/
static const DL_TimerG_TimerConfig gTIMER_0TimerConfig = {
.period = TIMER_0_INST_LOAD_VALUE,
.timerMode = DL_TIMER_TIMER_MODE_PERIODIC,
.startTimer = DL_TIMER_STOP,
};
SYSCONFIG_WEAK void SYSCFG_DL_TIMER_0_init(void) {
DL_TimerG_setClockConfig(TIMER_0_INST,
(DL_TimerG_ClockConfig *) &gTIMER_0ClockConfig);
DL_TimerG_initTimerMode(TIMER_0_INST,
(DL_TimerG_TimerConfig *) &gTIMER_0TimerConfig);
DL_TimerG_enableInterrupt(TIMER_0_INST , DL_TIMERG_INTERRUPT_ZERO_EVENT);
DL_TimerG_enableClock(TIMER_0_INST);
}
@@ -269,6 +312,12 @@ SYSCONFIG_WEAK void SYSCFG_DL_UART_0_init(void)
DL_UART_Main_setBaudRateDivisor(UART_0_INST, UART_0_IBRD_4_MHZ_115200_BAUD, UART_0_FBRD_4_MHZ_115200_BAUD);
/* Configure Interrupts */
DL_UART_Main_enableInterrupt(UART_0_INST,
DL_UART_MAIN_INTERRUPT_DMA_DONE_RX);
/* Configure DMA Receive Event */
DL_UART_Main_enableDMAReceiveEvent(UART_0_INST, DL_UART_DMA_INTERRUPT_RX);
/* Configure DMA Transmit Event */
DL_UART_Main_enableDMATransmitEvent(UART_0_INST);
/* Configure FIFOs */
@@ -286,7 +335,7 @@ static const DL_DMA_Config gDMA_CH0Config = {
.srcIncrement = DL_DMA_ADDR_INCREMENT,
.destWidth = DL_DMA_WIDTH_BYTE,
.srcWidth = DL_DMA_WIDTH_BYTE,
.trigger = UART_0_INST_DMA_TRIGGER,
.trigger = UART_0_INST_DMA_TRIGGER_0,
.triggerType = DL_DMA_TRIGGER_TYPE_EXTERNAL,
};
@@ -294,8 +343,24 @@ SYSCONFIG_WEAK void SYSCFG_DL_DMA_CH0_init(void)
{
DL_DMA_initChannel(DMA, DMA_CH0_CHAN_ID , (DL_DMA_Config *) &gDMA_CH0Config);
}
static const DL_DMA_Config gDMA_CH1Config = {
.transferMode = DL_DMA_FULL_CH_REPEAT_SINGLE_TRANSFER_MODE,
.extendedMode = DL_DMA_NORMAL_MODE,
.destIncrement = DL_DMA_ADDR_INCREMENT,
.srcIncrement = DL_DMA_ADDR_UNCHANGED,
.destWidth = DL_DMA_WIDTH_BYTE,
.srcWidth = DL_DMA_WIDTH_BYTE,
.trigger = UART_0_INST_DMA_TRIGGER_1,
.triggerType = DL_DMA_TRIGGER_TYPE_EXTERNAL,
};
SYSCONFIG_WEAK void SYSCFG_DL_DMA_CH1_init(void)
{
DL_DMA_initChannel(DMA, DMA_CH1_CHAN_ID , (DL_DMA_Config *) &gDMA_CH1Config);
}
SYSCONFIG_WEAK void SYSCFG_DL_DMA_init(void){
SYSCFG_DL_DMA_CH0_init();
SYSCFG_DL_DMA_CH1_init();
}

14
ti_msp_dl_config.h
View File

@@ -103,6 +103,14 @@ extern "C" {
/* Defines for TIMER_0 */
#define TIMER_0_INST (TIMG0)
#define TIMER_0_INST_IRQHandler TIMG0_IRQHandler
#define TIMER_0_INST_INT_IRQN (TIMG0_INT_IRQn)
#define TIMER_0_INST_LOAD_VALUE (511U)
/* Defines for I2C_1 */
#define I2C_1_INST I2C1
@@ -142,7 +150,10 @@ extern "C" {
/* Defines for DMA_CH0 */
#define DMA_CH0_CHAN_ID (0)
#define UART_0_INST_DMA_TRIGGER (DMA_UART0_TX_TRIG)
#define UART_0_INST_DMA_TRIGGER_0 (DMA_UART0_TX_TRIG)
/* Defines for DMA_CH1 */
#define DMA_CH1_CHAN_ID (1)
#define UART_0_INST_DMA_TRIGGER_1 (DMA_UART0_RX_TRIG)
/* Port definition for Pin Group LED */
@@ -162,6 +173,7 @@ 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_TIMER_0_init(void);
void SYSCFG_DL_I2C_1_init(void);
void SYSCFG_DL_UART_0_init(void);
void SYSCFG_DL_DMA_init(void);