diff --git a/3rd/dfoc.c b/3rd/dfoc.c index 3b31d74..943ea45 100644 --- a/3rd/dfoc.c +++ b/3rd/dfoc.c @@ -12,64 +12,64 @@ #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(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) { - return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle); + 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); - + 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核心算法，克拉克逆变换/帕克逆变换 @@ -77,47 +77,47 @@ 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; + // 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); + 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(); + 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()); + 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/lowpass_filter.c b/3rd/lowpass_filter.c index 74c02b2..8e849c3 100644 --- a/3rd/lowpass_filter.c +++ b/3rd/lowpass_filter.c @@ -1,42 +1,48 @@ #include - +#include float y = 0; float Lowpassfilter_sim(float x) { - float out = 0.9*x + 0.1*y; - y = x; - return out; + 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; + 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; + 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) / 32 * 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; + 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; - - return y; + Last_y = y; + Last_Timesamp = Timesamp; + + + return y; } diff --git a/3rd/mt6701.c b/3rd/mt6701.c index 2676785..7b57a9e 100644 --- a/3rd/mt6701.c +++ b/3rd/mt6701.c @@ -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 */ @@ -110,51 +111,60 @@ volatile float Last_ts = 0.0; volatile float last_angle = 0.0; float GetAngle_NoTrack(void) { - MT6701_iic_read_angel(); + MT6701_iic_read_angel(); angle = ((int16_t)gRxPacket[0] << 6) | (gRxPacket[1] >> 2); - angle_f_rad = (float)angle * _2PI / 16384; - return angle_f_rad; + angle_f_rad = (float)angle * _2PI / 16384; + return angle_f_rad; } volatile float full_rotations = 0.0; volatile float Last_Angle = 0.0; float GetAngle(void) { - volatile float D_Angle = 0.0; - volatile float Angle = GetAngle_NoTrack(); - D_Angle = Angle - Last_Angle; - - if( fabs(D_Angle) > (0.8f*2*PI) ) - { - full_rotations = full_rotations + ((D_Angle > 0) ? -1 :1); - } - - Last_Angle = Angle; - - return (full_rotations * 2 * PI + Last_Angle); + volatile float D_Angle = 0.0; + volatile float Angle = GetAngle_NoTrack(); + D_Angle = Angle - Last_Angle; + + if(fabs(D_Angle) > (0.8f * 2 * PI)) + { + full_rotations = full_rotations + ((D_Angle > 0) ? -1 : 1); + } + + Last_Angle = Angle; + + return (full_rotations * 2 * PI + Last_Angle); } volatile float Last_Vel_ts = 0.0; volatile float Vel_Last_Angle = 0.0; 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(dt < 0.0001) dt = 10000; - - float Vel_Angle = GetAngle(); - - float dv = Vel_Angle - Vel_Last_Angle; + 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; + } - float velocity = (Vel_Angle - Vel_Last_Angle)/dt; - - Last_Vel_ts = Vel_ts; - Vel_Last_Angle = Vel_Angle; - - - return velocity; + 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; + + Last_Vel_ts = Vel_ts; + Vel_Last_Angle = Vel_Angle; + + + return velocity; } void MT6701_get_angle_degree(void) diff --git a/3rd/pid_control.c b/3rd/pid_control.c index 0c6ac22..b84ed63 100644 --- a/3rd/pid_control.c +++ b/3rd/pid_control.c @@ -1,12 +1,12 @@ #include - +#include #define limit 6.3 #define Output_ramp 10000 //限幅 -float _constrain(float amt, float low, float high) +float _constrain(float amt, float low, float high) { - return ((amt(high)?(high):(amt))); + return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt))); } unsigned long Timestamp_Last = 0.0; @@ -15,31 +15,37 @@ 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; + float Ts = 0.0; + uint32_t Timestamp = DL_SYSTICK_getValue(); + if(Timestamp < Timestamp_Last) + { + Ts = (float)(Timestamp_Last - Timestamp) / 32 * 1e-6; + } + else + { + Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 32 * 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/pwm.c b/3rd/pwm.c index fbca003..42d1d19 100644 --- a/3rd/pwm.c +++ b/3rd/pwm.c @@ -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); } diff --git a/3rd/uart_redircet.c b/3rd/uart_redircet.c index 817fc92..8f93338 100644 --- a/3rd/uart_redircet.c +++ b/3rd/uart_redircet.c @@ -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); } diff --git a/empty.c b/empty.c index d17ce26..b209e16 100644 --- a/empty.c +++ b/empty.c @@ -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) diff --git a/empty.syscfg b/empty.syscfg index 029e7a2..6996eca 100644 --- a/empty.syscfg +++ b/empty.syscfg @@ -9,15 +9,16 @@ /** * Import the modules used in this configuration. */ -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(); +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 SYSTICK = scripting.addModule("/ti/driverlib/SYSTICK"); +const UART = scripting.addModule("/ti/driverlib/UART", {}, false); +const UART1 = UART.addInstance(); /** * Write custom configuration values to the imported modules. @@ -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; diff --git a/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx b/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx index 21f06a2..f1331af 100644 --- a/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx +++ b/keil/empty_LP_MSPM0G3507_nortos_keil.uvprojx @@ -449,6 +449,36 @@ 5 ..\3rd\pwm.h + + dfoc.c + 1 + ..\3rd\dfoc.c + + + dfoc.h + 5 + ..\3rd\dfoc.h + + + lowpass_filter.c + 1 + ..\3rd\lowpass_filter.c + + + lowpass_filter.h + 5 + ..\3rd\lowpass_filter.h + + + pid_control.c + 1 + ..\3rd\pid_control.c + + + pid_control.h + 5 + ..\3rd\pid_control.h + diff --git a/ti_msp_dl_config.c b/ti_msp_dl_config.c index 1c4d201..073e066 100644 --- a/ti_msp_dl_config.c +++ b/ti_msp_dl_config.c @@ -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(); +} + diff --git a/ti_msp_dl_config.h b/ti_msp_dl_config.h index 2df1b93..7c81c1b 100644 --- a/ti_msp_dl_config.h +++ b/ti_msp_dl_config.h @@ -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);