diff --git a/3rd/config.h b/3rd/config.h new file mode 100644 index 0000000..51db6bf --- /dev/null +++ b/3rd/config.h @@ -0,0 +1,7 @@ +#ifndef CONFIG_H +#define CONFIG_H + +#define DEBUG_ENABLED true +#define DEBUG_MT_ENABLED false +#define DEBUG_DFOC true +#endif /* ti_msp_dl_config_h */ \ No newline at end of file diff --git a/3rd/dfoc.c b/3rd/dfoc.c index 943ea45..6a41d86 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -1,11 +1,14 @@ #include +#include #include "pwm.h" #include "mt6701.h" #include "delay.h" #include "lowpass_filter.h" #include "pid_control.h" #include +#include +#include "config.h" #define PI 3.14159265359f #define _3PI_2 4.71238898f @@ -20,6 +23,8 @@ float zero_electric_Angle = 0.0; extern int pp; extern int Dir; + + void Motor_en() { // GPIO_InitTypeDef GPIO_InitStructure; @@ -66,9 +71,16 @@ void SetPwm(float Ua, float Ub, float Uc) 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); + PWM_Channel1(dc_a * 1000.0f); // 频率15k + PWM_Channel2(dc_b * 1000.0f); + PWM_Channel3(dc_c * 1000.0f); + if(DEBUG_ENABLED & DEBUG_DFOC) + { + printf("dc_a %f \n", dc_a); + printf("dc_b %f \n", dc_b); + printf("dc_c %f \n", dc_c); + } + } @@ -120,7 +132,40 @@ void Set_Angle(float Target) SetPhaseVoltage(Uq, 0, electricAngle()); } - +double shaft_angle; +double openloop_timestamp; +float velocityopenloop(float target) +{ + float Uq = 0.0; + double Ts = 0.0; + + uint32_t now_ts = DL_SYSTICK_getValue(); + + + if(now_ts < openloop_timestamp) + { + Ts = (openloop_timestamp - now_ts) / 32.0f * 1e-6f; + } + else + { + Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 32.0f * 1e-6f; + } + + if(Ts < 0 || Ts >= 0.005) + { + Ts = 0.001f; + } + + shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts); + + Uq = voltage_limit; + + SetPhaseVoltage(Uq, 0, shaft_angle); + + openloop_timestamp = now_ts; + + return Uq; +} diff --git a/3rd/dfoc.h b/3rd/dfoc.h index ad07052..bba2692 100644 --- a/3rd/dfoc.h +++ b/3rd/dfoc.h @@ -11,6 +11,7 @@ void FOC_Init(float power); float electricAngle(void); float GetCommand(void); void Set_Angle(float Target); +float velocityopenloop(float target); diff --git a/3rd/mt6701.c b/3rd/mt6701.c index 7b57a9e..2ba1df4 100644 --- a/3rd/mt6701.c +++ b/3rd/mt6701.c @@ -2,10 +2,14 @@ #include "ti_msp_dl_config.h" #include "uart_redircet.h" #include "stdio.h" +#include "config.h" + volatile int16_t angle; volatile float angle_f; volatile float angle_f_rad; +volatile bool gIsI2cError = false; + /* Data sent to the Target */ uint8_t gTxPacket[I2C_TX_PACKET_SIZE] = { @@ -13,7 +17,7 @@ uint8_t gTxPacket[I2C_TX_PACKET_SIZE] = }; /* Data received from Target */ -volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE]; +volatile uint8_t gRxPacket[I2C_RX_PACKET_SIZE] = {0}; /* I2C clock configuration */ DL_I2C_ClockConfig gI2CclockConfig; @@ -46,8 +50,13 @@ void MT6701_iic_read_angel(void) * gDelayCycles = 3 I2C functional clock cycles * gDelayCycles = 3 * I2C clock divider * (CPU clock freq / I2C clock freq) */ - gDelayCycles = (3 * (gI2CclockConfig.divideRatio + 1)) * + gDelayCycles = (5 * (gI2CclockConfig.divideRatio + 1)) * (CPUCLK_FREQ / gClockSelFreq); + if(DEBUG_ENABLED & DEBUG_MT_ENABLED) + { + printf("i2c before writing -------\n"); + + } /* * Fill FIFO with data. This example will send a MAX of 8 bytes since it @@ -66,6 +75,11 @@ 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) + { + printf("i2c writing done -------\n"); + } + /* Workaround for errata I2C_ERR_13 */ delay_cycles(gDelayCycles); @@ -78,7 +92,12 @@ void MT6701_iic_read_angel(void) if(DL_I2C_getControllerStatus(I2C_1_INST) & DL_I2C_CONTROLLER_STATUS_ERROR) { - printf("i2c error --------------------------------------------------------------------------------\n"); + gIsI2cError = true; + + if(DEBUG_ENABLED & DEBUG_MT_ENABLED) + { + printf("i2c error ------------------------------------------\n"); + } /* LED will remain high if there is an error */ __BKPT(0); return; @@ -92,6 +111,11 @@ void MT6701_iic_read_angel(void) /* Add delay between transfers */ delay_cycles(1000); + if(DEBUG_ENABLED & DEBUG_MT_ENABLED) + { + printf("i2c before reading -------\n"); + } + /* Send a read request to Target */ DL_I2C_startControllerTransfer(I2C_1_INST, I2C_TARGET_ADDRESS, DL_I2C_CONTROLLER_DIRECTION_RX, I2C_RX_PACKET_SIZE); @@ -106,6 +130,13 @@ void MT6701_iic_read_angel(void) ; gRxPacket[i] = DL_I2C_receiveControllerData(I2C_1_INST); } + + if(DEBUG_ENABLED & DEBUG_MT_ENABLED) + { + printf("i2c reading done -------\n"); + } + + } volatile float Last_ts = 0.0; volatile float last_angle = 0.0; diff --git a/empty.c b/empty.c index b209e16..6994ceb 100644 --- a/empty.c +++ b/empty.c @@ -31,18 +31,23 @@ */ #include "ti_msp_dl_config.h" -#include "Delay.h" -#include "MT6701.h" - +#include "delay.h" +#include "mt6701.h" +#include "dfoc.h" #include #include #include +#include "config.h" + extern float angle_f; +extern bool gIsI2cError; + const float num_f = 0.123456f; volatile uint16_t count = 0 ; +const float Target = 10.0; //串口目标值 int pp = 7; //电机极对数 @@ -53,20 +58,25 @@ 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(); - 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) - count = 0; - + FOC_Init(12); + while(1) + { + DL_GPIO_togglePins(LED_PORT, LED_PA0_PIN); + //delay_ms(10); + volatile float angle_rad = GetAngle(); + if(DEBUG_ENABLED) + { + printf("angle %.5f \n", angle_rad); + } + + // volatile uint32_t tick = DL_SYSTICK_getValue(); + // printf("systick is %x \n",tick); + velocityopenloop(Target); + + + + + } } diff --git a/empty.syscfg b/empty.syscfg index 563830e..9589d56 100644 --- a/empty.syscfg +++ b/empty.syscfg @@ -60,7 +60,6 @@ Board.peripheral.swdioPin.$assign = "PA19"; I2C1.$name = "I2C_1"; I2C1.advAnalogGlitchFilter = "DISABLED"; I2C1.basicEnableController = true; -I2C1.basicControllerStandardBusSpeed = "Fast"; I2C1.peripheral.sdaPin.$assign = "PB3"; I2C1.peripheral.sclPin.$assign = "PB2"; I2C1.sdaPinConfig.hideOutputInversion = scripting.forceWrite(false); @@ -78,9 +77,9 @@ 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_0.ccValue = 1000; PWM1.PWM_CHANNEL_1.$name = "ti_driverlib_pwm_PWMTimerCC1"; -PWM1.PWM_CHANNEL_1.dutyCycle = 50; +PWM1.PWM_CHANNEL_1.ccValue = 1000; PWM1.ccp0PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric4"; PWM1.ccp1PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric5"; PWM1.peripheral.$assign = "TIMA0"; @@ -88,7 +87,7 @@ 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.PWM_CHANNEL_2.ccValue = 1000; PWM1.ccp2PinConfig.$name = "ti_driverlib_gpio_GPIOPinGeneric6"; SYSCTL.forceDefaultClkConfig = true; diff --git a/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx b/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx index f1331af..904d71c 100644 --- a/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx +++ b/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx @@ -314,7 +314,7 @@ 1 - 3 + 1 0 0 1 @@ -479,6 +479,11 @@ 5 ..\3rd\pid_control.h + + config.h + 5 + ..\3rd\config.h + diff --git a/ti_msp_dl_config.c b/ti_msp_dl_config.c index 073e066..576c47b 100644 --- a/ti_msp_dl_config.c +++ b/ti_msp_dl_config.c @@ -189,21 +189,21 @@ SYSCONFIG_WEAK void SYSCFG_DL_PWM_0_init(void) { 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_setCaptureCompareValue(PWM_0_INST, 1000, 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_setCaptureCompareValue(PWM_0_INST, 1000, 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_setCaptureCompareValue(PWM_0_INST, 1000, DL_TIMER_CC_2_INDEX); DL_TimerA_enableClock(PWM_0_INST); @@ -228,8 +228,8 @@ SYSCONFIG_WEAK void SYSCFG_DL_I2C_1_init(void) { /* Configure Controller Mode */ DL_I2C_resetControllerTransfer(I2C_1_INST); - /* Set frequency to 400000 Hz*/ - DL_I2C_setTimerPeriod(I2C_1_INST, 7); + /* Set frequency to 100000 Hz*/ + DL_I2C_setTimerPeriod(I2C_1_INST, 31); DL_I2C_setControllerTXFIFOThreshold(I2C_1_INST, DL_I2C_TX_FIFO_LEVEL_EMPTY); DL_I2C_setControllerRXFIFOThreshold(I2C_1_INST, DL_I2C_RX_FIFO_LEVEL_BYTES_1); DL_I2C_enableControllerClockStretching(I2C_1_INST); diff --git a/ti_msp_dl_config.h b/ti_msp_dl_config.h index 7c81c1b..a061114 100644 --- a/ti_msp_dl_config.h +++ b/ti_msp_dl_config.h @@ -108,7 +108,7 @@ extern "C" { #define I2C_1_INST I2C1 #define I2C_1_INST_IRQHandler I2C1_IRQHandler #define I2C_1_INST_INT_IRQN I2C1_INT_IRQn -#define I2C_1_BUS_SPEED_HZ 400000 +#define I2C_1_BUS_SPEED_HZ 100000 #define GPIO_I2C_1_SDA_PORT GPIOB #define GPIO_I2C_1_SDA_PIN DL_GPIO_PIN_3 #define GPIO_I2C_1_IOMUX_SDA (IOMUX_PINCM16)