From f404369a32ee435934506637d0c2ee202f20e6b6 Mon Sep 17 00:00:00 2001 From: BoomPC Date: Fri, 17 May 2024 00:43:58 +0800 Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E9=80=9F=E5=BA=A6=E3=80=81?= =?UTF-8?q?=E7=94=B5=E6=B5=81=E9=97=AD=E7=8E=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APP/APP_Main.c | 36 ++-- APP/APP_Main.h | 4 + APP/APP_Task.c | 54 +++++- BoooomCTL/Controller/Controller.c | 235 +++++++++++++++++++++-- BoooomCTL/Controller/Controller.h | 6 +- BoooomCTL/Controller/PreDrive/PreDrive.c | 3 +- BoooomCTL/Controller/SVPWM/SVPWM.c | 144 ++------------ BoooomCTL/Controller/SVPWM/SVPWM.h | 6 +- BoooomCTL/Data/Angle/Angle.c | 148 +++++++++++++- BoooomCTL/Data/Angle/Angle.h | 43 ++++- Core/Src/main.c | 2 +- 11 files changed, 507 insertions(+), 174 deletions(-) diff --git a/APP/APP_Main.c b/APP/APP_Main.c index da72a7e..71891cd 100644 --- a/APP/APP_Main.c +++ b/APP/APP_Main.c @@ -3,26 +3,24 @@ // #include "APP_Main.h" - -#include "Angle.h" -#include "InteriorADC.h" #include "Controller/Controller.h" +#include "PreDrive.h" -//float CurrA, CurrB, CurrC; -//float Current_Temp, V_Temp; float32_t NowAngle; +tData Data; -void APP_Init() { - tData Data; - - Data.Angle.EncoderModel = MT6816; - Data_Init(&Data); - - NowAngle = Data.Angle.getAngle(); +void APP_Init() +{ __HAL_SPI_ENABLE(&hspi1); HAL_Delay(10); + Data.Angle.EncoderModel = MT6816; + Data_Init(&Data); +// NowAngle = Data.Angle.getAngle(); + + + //开启内部运放 HAL_OPAMP_Start(&hopamp1); @@ -43,8 +41,14 @@ void APP_Init() { __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE); HAL_Delay(10); + + PreDrive_Init(); GPIOE->BSRR = 1 << 7; HAL_Delay(10); + TIM1->CCR1 = 0; + TIM1->CCR2 = 0; + TIM1->CCR3 = 0; + } uint16_t ADC_VAL1, ADC_VAL2; @@ -123,12 +127,12 @@ void APP_Main() { // SendCurrent_Vofa(ADC1->JDR1, FOC.dtc_a, 0 ); // CurrA = ia/4096*3.3/0.005f/7.33333; - SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, ADC2->JDR1, ADC1->JDR2); +// SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, ADC2->JDR1, ADC1->JDR2); // usb_printf("PhaseVoltage:%d\r\n", (ADC1->JDR1-2048.0f)/4096.0f*3.3f/0.005f/7.33333f); - - - HAL_Delay(10); +// NowAngle = Data.Angle.getAngle(); +// SendCurrent_Vofa(FOC.dtc_a, NowAngle, 0); +// HAL_Delay(10); } diff --git a/APP/APP_Main.h b/APP/APP_Main.h index 8edd5c7..ae96381 100644 --- a/APP/APP_Main.h +++ b/APP/APP_Main.h @@ -26,7 +26,11 @@ #include "arm_math.h" #include "Communication.h" +#include "InteriorADC.h" +#include "Angle.h" + +extern tData Data; static __IO uint16_t adc1_RegularBuf[10]; diff --git a/APP/APP_Task.c b/APP/APP_Task.c index 1a9e8d3..71a35ce 100644 --- a/APP/APP_Task.c +++ b/APP/APP_Task.c @@ -7,13 +7,15 @@ #include "Angle.h" #include "InteriorADC.h" #include "Controller/Controller.h" +#include "Controller/SVPWM/SVPWM.h" +#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample))) float CurrA, CurrB, CurrC; float Current_Temp, V_Temp; //float32_t NowAngle; -float Current_Temp1, ii = 0.0f, iii = 0.0f; +float Current_Temp1, ii = 0.0f, Test_Angle = 0.0f; #define PWM_FREQUENCY 24000 #define CURRENT_MEASURE_HZ PWM_FREQUENCY @@ -23,19 +25,51 @@ float Current_Temp1, ii = 0.0f, iii = 0.0f; #define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE) #define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U) +int up = 1; +float32_t A, Speedtarget; +int32_t number; +int32_t mode = 0; void High_Frequency_Task() { - ii += 0.000001f; -// if (ii == 0.1f) { ii = 0.1f-0.00001f; } - iii += 0.1f; - if (iii >= 360.0f) { iii = 0.0f; } + if (mode < 10000) { +// HAL_Delay(1000); + mode++; + } + if (mode >= 10000) { + + + number++; + if (number >= 1 && number <= 50000) { + Speedtarget = 0.1; + } else if (number >= 50000 && number <= 100000) { + Speedtarget = -1.0; + } else if (number >= 100001) { + number = 0; + } +// Test_Angle += 0.01f; +// if (Test_Angle > 360.0f) { +// Test_Angle = 0.0f; +// } + + + //Data.Angle.getAngle() +// SendCurrent_Vofa(ADC1->JDR1, ADC2->JDR1, ADC1->JDR2); + +// float test = 0; +// UTILS_LP_FAST(test, 12, 0.05f); +// usb_printf("%f\r\n", test); + + +// Generate_SVM(0.0f, 0.05f, Data.Angle.getAngle()); +// float32_t Eangle = Data.Angle.getAngle(); + SpeedControl(Speedtarget, MT_ReadAngle()); + FOC_current(0, 5.0f, Data.Angle.getAngle(), 5000); + +// SendCurrent_Vofa(Test_Angle,A, (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES)); - Generate(0, 0.02f, iii); -// SendCurrent_Vofa(FOC.dtc_a, iii, (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES)); - TIM1->CCR1 = (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES); - TIM1->CCR2 = (uint16_t) (FOC.dtc_b * (float) HALF_PWM_PERIOD_CYCLES); - TIM1->CCR3 = (uint16_t) (FOC.dtc_c * (float) HALF_PWM_PERIOD_CYCLES); // SendCurrent_Vofa((ADC1->JDR1)/4096.0f*3.3f/0.005f/7.33333f, 0, 0); + } + } \ No newline at end of file diff --git a/BoooomCTL/Controller/Controller.c b/BoooomCTL/Controller/Controller.c index 3fcdbdc..d73f0e0 100644 --- a/BoooomCTL/Controller/Controller.c +++ b/BoooomCTL/Controller/Controller.c @@ -5,25 +5,238 @@ #include #include "Controller.h" #include "SVPWM/SVPWM.h" +#include "Communication.h" +#include "Angle.h" + +#define UTILS_LP_FAST(value, sample, filter_constant) (value -= (filter_constant) * ((value) - (sample))) + tFOC FOC; float uAlpha1; float uBeta1; -bool Generate(float ud, float uq, float Theta) { +#define PWM_FREQUENCY 24000 +#define CURRENT_MEASURE_HZ PWM_FREQUENCY +#define CURRENT_MEASURE_PERIOD (float) (1.0f / (float) CURRENT_MEASURE_HZ) - inversePark(ud, uq, Theta, &uAlpha1, &uBeta1); +#define TIMER0_CLK_MHz 168 +#define PWM_PERIOD_CYCLES (uint16_t)((TIMER0_CLK_MHz * (uint32_t) 1000000u / ((uint32_t) (PWM_FREQUENCY))) & 0xFFFE) +#define HALF_PWM_PERIOD_CYCLES (uint16_t)(PWM_PERIOD_CYCLES / 2U) +float current1, current2, current3; - if (0 == SVPWM(uAlpha1, uBeta1, &FOC.dtc_a, &FOC.dtc_b, &FOC.dtc_c)) { -// set_a_duty((uint16_t)(Foc.dtc_a * (float) HALF_PWM_PERIOD_CYCLES)); -// set_b_duty((uint16_t)(Foc.dtc_b * (float) HALF_PWM_PERIOD_CYCLES)); -// set_c_duty((uint16_t)(Foc.dtc_c * (float) HALF_PWM_PERIOD_CYCLES)); -// TIM1->CCR1 = (uint16_t) FOC.dtc_a; -// TIM1->CCR2 = (uint16_t) FOC.dtc_b; -// TIM1->CCR3 = (uint16_t) FOC.dtc_c; - } +float32_t id_curr_pi_kp = 0.001f; +float32_t id_curr_pi_ki = 0.000001f; - return FOC.dtc_a; +float32_t id_curr_pi_target = 0.0f; +float32_t id_curr_pi_value; +float32_t id_curr_pi_error; + +float32_t id_curr_pi_errMin = 0.0f; +float32_t id_curr_pi_errSum; +float32_t id_curr_pi_errSumMax = 5.0f; + +float32_t id_curr_pi_result; + + +float32_t iq_curr_pi_kp = 0.001f; +float32_t iq_curr_pi_ki = 0.000001f; + +float32_t iq_curr_pi_target = 1.0f; +float32_t iq_curr_pi_value; +float32_t iq_curr_pi_error; + +float32_t iq_curr_pi_errMin = 0.0f; +float32_t iq_curr_pi_errSum; +float32_t iq_curr_pi_errSumMax = 5.0f; + +float32_t iq_curr_pi_result; + + + +float32_t Speed_target = 0.0f; + +float32_t Speedpid_error; +float32_t Speedpid_errSum; +float32_t Speedpid_kp = 1.0f; +float32_t Speedpid_ki = 0.1f; +float32_t Speedpid_kd = 1.0f; +float32_t Speedpid_errSumMax = 5.0f; +float32_t Speedpid_lastErr; +float32_t Speedpid_errDt; +float32_t Speedpid_result; + + + + +float32_t u_d, u_q; + + +static void Current_PI_Cal_Id(float32_t resultMax) { + //curr_pi_target = target; + + id_curr_pi_error = id_curr_pi_target - id_curr_pi_value; + +// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin) + id_curr_pi_errSum += id_curr_pi_error * id_curr_pi_ki; + + if (id_curr_pi_errSum > id_curr_pi_errSumMax) + id_curr_pi_errSum = id_curr_pi_errSumMax; + else if (id_curr_pi_errSum < -id_curr_pi_errSumMax) + id_curr_pi_errSum = -id_curr_pi_errSumMax; + + id_curr_pi_result = id_curr_pi_kp * id_curr_pi_error + id_curr_pi_errSum; + + if (id_curr_pi_result > resultMax) + id_curr_pi_result = resultMax; + else if (id_curr_pi_result < -resultMax) + id_curr_pi_result = -resultMax; +} + + +static void Current_PI_Cal_Iq(float32_t resultMax) { + //curr_pi_target = target; + + iq_curr_pi_error = iq_curr_pi_target - iq_curr_pi_value; + +// if(curr_pi_error > PI_Control->errMin || curr_pi_error < -PI_Control->errMin) + iq_curr_pi_errSum += iq_curr_pi_error * iq_curr_pi_ki; + + if (iq_curr_pi_errSum > iq_curr_pi_errSumMax) + iq_curr_pi_errSum = iq_curr_pi_errSumMax; + else if (iq_curr_pi_errSum < -iq_curr_pi_errSumMax) + iq_curr_pi_errSum = -iq_curr_pi_errSumMax; + + iq_curr_pi_result = iq_curr_pi_kp * iq_curr_pi_error + iq_curr_pi_errSum; + + if (iq_curr_pi_result > resultMax) + iq_curr_pi_result = resultMax; + else if (iq_curr_pi_result < -resultMax) + iq_curr_pi_result = -resultMax; +} + + +static float32_t PIDGetResult(float32_t Speedpid_value, float32_t valMax, float32_t errMin) { + Speedpid_error = Speed_target - Speedpid_value; + + if (Speedpid_error > errMin || Speedpid_error < -errMin) + Speedpid_errSum += Speedpid_error * Speedpid_ki; + + if (Speedpid_errSum > Speedpid_errSumMax) + Speedpid_errSum = Speedpid_errSumMax; + else if (Speedpid_errSum < -Speedpid_errSumMax) + Speedpid_errSum = -Speedpid_errSumMax; + + Speedpid_errDt = Speedpid_error - Speedpid_lastErr; + Speedpid_lastErr = Speedpid_error; + + Speedpid_result = Speedpid_kp * Speedpid_error + Speedpid_errSum + Speedpid_kd * Speedpid_errDt; + + if (Speedpid_result > valMax) + Speedpid_result = valMax; + else if (Speedpid_result < -valMax) + Speedpid_result = -valMax; + + return Speedpid_result; +} + + +void SpeedControl(float32_t target,float32_t angleVal) { + Speed_target = target; + float32_t motorControl_speedValue = GetSpeed(angleVal); + SendCurrent_Vofa(motorControl_speedValue, target, 0); +// speedPID_value = motorControl_speedValue; + + iq_curr_pi_target = PIDGetResult(motorControl_speedValue, angleVal, 0.0f); +} + + + + +//bool Generate_SVM(float ud, float uq, float Theta) { +bool FOC_current(float Id_set, float Iq_set, float Theta, float bw) { + if (id_curr_pi_target>Id_set){id_curr_pi_target=Id_set;} + if (iq_curr_pi_target>Iq_set){iq_curr_pi_target=Iq_set;} +// id_curr_pi_target = Id_set; +// iq_curr_pi_target = Iq_set; + + current1 = ADC1->JDR2; + current2 = ADC2->JDR1; + current3 = ADC1->JDR1; + current1 = (current1 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) + 0.23; + current2 = (current2 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f); + current3 = (current3 - 2048) * ((3.3f / 4095.0f) / 0.005f / 7.333333f) - 0.4; +// SendCurrent_Vofa(current1, current2, current3); + + // Clarke transform + float i_alpha, i_beta; + clarke_transform(current1, current2, current3, &i_alpha, &i_beta); + // Park transform + float i_d, i_q; + park_transform(i_alpha, i_beta, Theta, &i_d, &i_q); + + id_curr_pi_value = i_d; + iq_curr_pi_value = i_q; + Current_PI_Cal_Id(1.0f); + Current_PI_Cal_Iq(1.0f); + u_d = id_curr_pi_result; + u_q = iq_curr_pi_result; + + + // float mod_to_V = FOC.v_bus_filt * 2.0f / 3.0f; + // float V_to_mod = 1.0f / mod_to_V; + float mod_to_V = 1.2f * 2.0f / 3.0f; + float V_to_mod = 1.0f / mod_to_V; + + float bandwidth = MIN(bw, 0.25f * PWM_FREQUENCY); + // Apply PI control + float Ierr_d = Id_set - i_d; + float Ierr_q = Iq_set - i_q; +// FOC.current_ctrl_p_gain = 5.0f * bandwidth; +// FOC.current_ctrl_i_gain = 0.000002f * bandwidth; + FOC.current_ctrl_p_gain = 0.001f; + FOC.current_ctrl_i_gain = 0.00001f; + FOC.current_ctrl_integral_d = 0; + FOC.current_ctrl_integral_q = 0; + float mod_d = V_to_mod * (FOC.current_ctrl_integral_d + Ierr_d * FOC.current_ctrl_p_gain); + float mod_q = V_to_mod * (FOC.current_ctrl_integral_q + Ierr_q * FOC.current_ctrl_p_gain); + + // Vector modulation saturation, lock integrator if saturated + float mod_scalefactor = 0.9f * SQRT3_BY_2 / sqrtf(SQ(mod_d) + SQ(mod_q)); + if (mod_scalefactor < 1.0f) { + mod_d *= mod_scalefactor; + mod_q *= mod_scalefactor; + FOC.current_ctrl_integral_d *= 0.99f; + FOC.current_ctrl_integral_q *= 0.99f; + } else { + FOC.current_ctrl_integral_d += Ierr_d * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD); + FOC.current_ctrl_integral_q += Ierr_q * (FOC.current_ctrl_i_gain * CURRENT_MEASURE_PERIOD); + } + + + float pwm_phase = Theta + Theta * M_2PI * 20 * CURRENT_MEASURE_PERIOD; + inversePark(u_d, u_q, Theta, &uAlpha1, &uBeta1); + + FOC.i_q = i_q; + UTILS_LP_FAST(FOC.i_q_filt, FOC.i_q, 0.01f); + FOC.i_d = i_d; + UTILS_LP_FAST(FOC.i_d_filt, FOC.i_d, 0.01f); + FOC.i_bus = (mod_d * i_d + mod_q * i_q); + UTILS_LP_FAST(FOC.i_bus_filt, FOC.i_bus, 0.01f); + FOC.power_filt = FOC.v_bus_filt * FOC.i_bus_filt; + + SVPWM(uAlpha1, uBeta1, &FOC.dtc_a, &FOC.dtc_b, &FOC.dtc_c); +// FOC.dtc_a = FOC.dtc_a * 0.01; +// FOC.dtc_b = FOC.dtc_b * 0.01; +// FOC.dtc_c = FOC.dtc_c * 0.01; +// SendCurrent_Vofa(mod_d, mod_q, current1); + TIM1->CCR1 = (uint16_t) (FOC.dtc_c * (float) HALF_PWM_PERIOD_CYCLES); + TIM1->CCR2 = (uint16_t) (FOC.dtc_b * (float) HALF_PWM_PERIOD_CYCLES); + TIM1->CCR3 = (uint16_t) (FOC.dtc_a * (float) HALF_PWM_PERIOD_CYCLES); +// TIM1->CCR1 = (uint16_t) FOC.dtc_a; +// TIM1->CCR2 = (uint16_t) FOC.dtc_b; +// TIM1->CCR3 = (uint16_t) FOC.dtc_c; + + return 0; } diff --git a/BoooomCTL/Controller/Controller.h b/BoooomCTL/Controller/Controller.h index 3496d2b..0622fc9 100644 --- a/BoooomCTL/Controller/Controller.h +++ b/BoooomCTL/Controller/Controller.h @@ -5,6 +5,8 @@ #ifndef BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H #define BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H +#include "arm_math.h" + typedef struct sFOC { bool is_armed; @@ -20,7 +22,9 @@ typedef struct sFOC extern tFOC FOC; -extern bool Generate(float ud, float uq, float Theta); + +extern void SpeedControl(float32_t target,float32_t angleVal); +extern bool FOC_current(float Id_set, float Iq_set, float Theta, float bw); #endif //BOOOOMFOC_STSPIN32G4_EVB_CONTROLLER_H diff --git a/BoooomCTL/Controller/PreDrive/PreDrive.c b/BoooomCTL/Controller/PreDrive/PreDrive.c index 233a8b2..241066f 100644 --- a/BoooomCTL/Controller/PreDrive/PreDrive.c +++ b/BoooomCTL/Controller/PreDrive/PreDrive.c @@ -5,11 +5,10 @@ #include "PreDrive.h" - uint8_t PreDrive_Init_Buffur[2] = {0x00, 0x00}; -bool PreDrive_Init(void){ +bool PreDrive_Init(void) { while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x0c, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {} PreDrive_Init_Buffur[0] = 0x00; while (HAL_I2C_Mem_Write(&hi2c3, 0x8e, 0x0c, I2C_MEMADD_SIZE_8BIT, PreDrive_Init_Buffur, 1, 1000) != HAL_OK) {} diff --git a/BoooomCTL/Controller/SVPWM/SVPWM.c b/BoooomCTL/Controller/SVPWM/SVPWM.c index da3150a..c8b8380 100644 --- a/BoooomCTL/Controller/SVPWM/SVPWM.c +++ b/BoooomCTL/Controller/SVPWM/SVPWM.c @@ -12,144 +12,38 @@ extern uint16_t cycleNum; #define SQRT3 1.732050808f #define LIMIT (float32_t)(0.9f / SQRT3) #define LIMIT_UDC 16.0f -#define TS 3400 +#define TS 3300 #define SQRT3_MULT_TS (float32_t)((float32_t)TS * SQRT3) uint8_t sectionMap[7] = {0, 2, 6, 1, 4, 3, 5}; float32_t uAlpha, uBeta; float32_t ud, uq; -uint16_t channel1, channel2, channel3,udc=12; +float channel1, channel2, channel3, udc = 12; float uAlpha, uBeta; float32_t iAlpha, iBeta; float32_t id, iq; + +inline void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta) { + *Ialpha = Ia; + *Ibeta = (Ib - Ic) * ONE_BY_SQRT3; +} + +inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq) { + float s = sinf(Theta / 57.29577951326093f); + float c = cosf(Theta / 57.29577951326093f); + *Id = Ialpha * c + Ibeta * s; + *Iq = -Ialpha * s + Ibeta * c; +} + + inline void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta) { - float s = sinf(Theta / 57.2958f); - float c = cosf(Theta / 57.2958f); + float s = sinf(Theta / 57.29577951326093f); + float c = cosf(Theta / 57.29577951326093f); *uAlpha = ud * c - uq * s; *uBeta = ud * s + uq * c; } -#ifdef false - -inline int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC) { - float U1, U2, U3; - uint8_t a, b, c, n = 0; - - U1 = uBeta; - U2 = (SQRT3 * uAlpha - uBeta) / 2; - U3 = (-SQRT3 * uAlpha - uBeta) / 2; - - if (U1 > 0) - a = 1; - else - a = 0; - if (U2 > 0) - b = 1; - else - b = 0; - if (U3 > 0) - c = 1; - else - c = 0; - - n = (c << 2) + (b << 1) + a; - - switch (sectionMap[n]) - { - case 0: - { - channel1 = TS / 2; - channel2 = TS / 2; - channel3 = TS / 2; - } - break; - - case 1: - { - int16_t t4 = SQRT3 * TS * U2 / udc; - int16_t t6 = SQRT3 * TS * U1 / udc; - int16_t t0 = (TS - t4 - t6) / 2; - - channel1 = t4 + t6 + t0; - channel2 = t6 + t0; - channel3 = t0; - } - break; - - case 2: - { - int16_t t2 = -SQRT3 * TS * U2 / udc; - int16_t t6 = -SQRT3 * TS * U3 / udc; - int16_t t0 = (TS - t2 - t6) / 2; - - channel1 = t6 + t0; - channel2 = t2 + t6 + t0; - channel3 = t0; - } - break; - - case 3: - { - int16_t t2 = SQRT3 * TS * U1 / udc; - int16_t t3 = SQRT3 * TS * U3 / udc; - int16_t t0 = (TS - t2 - t3) / 2; - - channel1 = t0; - channel2 = t2 + t3 + t0; - channel3 = t3 + t0; - } - break; - - case 4: - { - int16_t t1 = -SQRT3 * TS * U1 / udc; - int16_t t3 = -SQRT3 * TS * U2 / udc; - int16_t t0 = (TS - t1 - t3) / 2; - - channel1 = t0; - channel2 = t3 + t0; - channel3 = t1 + t3 + t0; - } - break; - - case 5: - { - int16_t t1 = SQRT3 * TS * U3 / udc; - int16_t t5 = SQRT3 * TS * U2 / udc; - int16_t t0 = (TS - t1 - t5) / 2; - - channel1 = t5 + t0; - channel2 = t0; - channel3 = t1 + t5 + t0; - } - break; - - case 6: - { - int16_t t4 = -SQRT3 * TS * U3 / udc; - int16_t t5 = -SQRT3 * TS * U1 / udc; - int16_t t0 = (TS - t4 - t5) / 2; - - channel1 = t4 + t5 + t0; - channel2 = t0; - channel3 = t5 + t0; - } - break; - - default: - break; - } - - *tA = channel1; - *tB = channel2; - *tC = channel3; - - return 0; - - -} -#else inline int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC) { int Sextant; @@ -274,5 +168,3 @@ inline int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC) { // TIM1->CCR2 = channelB; // TIM1->CCR3 = channelC; } - -#endif \ No newline at end of file diff --git a/BoooomCTL/Controller/SVPWM/SVPWM.h b/BoooomCTL/Controller/SVPWM/SVPWM.h index fd2bf31..6934b0d 100644 --- a/BoooomCTL/Controller/SVPWM/SVPWM.h +++ b/BoooomCTL/Controller/SVPWM/SVPWM.h @@ -7,7 +7,7 @@ #include "arm_math.h" -#define M_PI (3.14159265358f) +//#define M_PI (3.14159265358f) #define M_2PI (6.28318530716f) #define ONE_BY_SQRT3 (0.57735026919f) #define TWO_BY_SQRT3 (2.0f * 0.57735026919f) @@ -24,6 +24,10 @@ // //extern inline void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq); +extern void clarke_transform(float Ia, float Ib, float Ic, float *Ialpha, float *Ibeta); + +extern void park_transform(float Ialpha, float Ibeta, float Theta, float *Id, float *Iq); + extern void inversePark(float ud, float uq, float Theta, float *uAlpha, float *uBeta); extern int SVPWM(float uAlpha, float uBeta, float *tA, float *tB, float *tC); diff --git a/BoooomCTL/Data/Angle/Angle.c b/BoooomCTL/Data/Angle/Angle.c index e5ed291..af8c968 100644 --- a/BoooomCTL/Data/Angle/Angle.c +++ b/BoooomCTL/Data/Angle/Angle.c @@ -3,14 +3,152 @@ // #include "Angle.h" +#include "usbd_cdc_if.h" +#include "Communication.h" + + +#define RD_REG_3 0x8300 // 0xffff// +#define RD_REG_4 0x8400 // 0x0000// + +//#define POLE_PAIRS 7 //小电机 +//双天电机：14，极飞A12：21 +//#define POLE_PAIRS 21//大电机 +#define POLE_PAIRS 20//大电机 +#define LINE_NUM 16384 + +#define AA_LINE ((float32_t)LINE_NUM / (float32_t)POLE_PAIRS) // 2340.57 +#define AA_ANGLE ((float32_t)LINE_NUM / 360.0f) // 45.51 +#define BB_ANGLE ((float32_t)AA_LINE / 360.0f) // 6.50 + +#define ANGLE_RATIO (float32_t)(360.0f / 16384.0f) + +//#define OFFSET 680 //小电机 +//#define OFFSET 730 //大电机 +//#define OFFSET 1020 //大电机 +// uint16_t OFFSET_0 = 280; +//uint16_t OFFSET_0 = -10000; +float32_t OFFSET=3000; +#define OFFSET_1 1100 + +//#define OFFSET_0 30 +//#define OFFSET_1 300 + +uint16_t sendBuf[2] = {RD_REG_3, RD_REG_4}; + +uint32_t timeout; + +float32_t speed, speedFilt, filt = 0.01f; +uint16_t cycleNum; + +struct ENCODER_Type encoder0 = { + .hspi = &hspi1, + .CS_Port = GPIOD, + .CS_Pin = GPIO_PIN_2, + .sendBuf = {RD_REG_3, RD_REG_4}, +// .offset = OFFSET_0, +}; + + +uint8_t SPI_TransmitReceive(SPI_HandleTypeDef *hspi, uint16_t TxData, uint16_t *RxData) { + uint32_t cnt = 0; + + while ((hspi->Instance->SR & SPI_SR_TXE) == 0); + hspi->Instance->DR = TxData; + while (cnt < 50) { + if ((hspi->Instance->SR & SPI_SR_RXNE)) { + *RxData = hspi->Instance->DR; + return 0; + } + cnt++; + } + + return 1; +} + +uint16_t MT_ReadAngle(void) { + HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_RESET); + timeout = SPI_TransmitReceive(encoder0.hspi, encoder0.sendBuf[0], &encoder0.recvBuf[0]); + HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_SET); + for (int i = 0; i < 2; i++); + HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_RESET); + timeout = SPI_TransmitReceive(encoder0.hspi, encoder0.sendBuf[1], &encoder0.recvBuf[1]); + HAL_GPIO_WritePin(encoder0.CS_Port, encoder0.CS_Pin, GPIO_PIN_SET); + /*处理原始数据并转换为机械角度*/ + encoder0.angleVal = ((encoder0.recvBuf[0] & 0x00FF) << 6) + ((encoder0.recvBuf[1] & 0x00FF) >> 2); + encoder0.angle_360 = (float32_t) encoder0.angleVal * ANGLE_RATIO; + + /*软件记圈*/ + if (encoder0.lastAngle > 9000 && encoder0.angleVal < 7000) + encoder0.cycleNum++; + else if (encoder0.lastAngle < 7000 && encoder0.angleVal > 9000) + encoder0.cycleNum--; + + /*记速*/ + // encoder->angleArr[encoder->cnt][0] = encoder->angleVal; + // encoder->angleArr[encoder->cnt][1] = encoder->cycleNum; + + // int32_t temp1 = encoder->angleArr[encoder->cnt][0] - encoder->angleArr[(encoder->cnt +1) % 8][0]; + // int32_t temp2 = (encoder->angleArr[encoder->cnt][1] - encoder->angleArr[(encoder->cnt +1) % 8][1]) << 14; + // encoder->speed = (float32_t)(temp1 + temp2) ;// / 32.0f; + + // int32_t temp1 = 0; + // int32_t temp2 = 0; + // int32_t sum = 0; + // for(uint8_t i = 0; i < 8 - 1; i++) + // { + // temp1 = encoder->angleArr[i + 1][0] - encoder->angleArr[i][0]; + // temp2 = (encoder->angleArr[i + 1][1] - encoder->angleArr[i][1]) << 14; + // sum += (temp1 + temp2) ;// / 32.0f; + // } + // encoder->speed = (float32_t)(sum) / 7.0f;// / 32.0f; + + // encoder->cnt ++; + // encoder->cnt &= 0x07; + + encoder0.lastAngle = encoder0.angleVal; + return encoder0.angleVal; +} + + +float32_t GetSpeed(uint16_t angleVal) { + static int32_t lastAngle, lastCycle; + + speed = (float32_t) (angleVal - lastAngle + ((encoder0.cycleNum - lastCycle) << 14)); + + lastAngle = angleVal; + lastCycle = encoder0.cycleNum; + + speedFilt = speed * filt + (1.0f - filt) * speedFilt; + + return speedFilt; +// return speed; +} + +float32_t getAngle_MT6816() { + +// float32_t temp = (float32_t)angleVal + (float32_t)encoder0.offset; +// OFFSET+=0.01f; +// usb_printf("%d\r\n", OFFSET); +// if (OFFSET >= 16384.0f){OFFSET= 0.0f;} + + float32_t temp = (float32_t) MT_ReadAngle()+16274; +// SendCurrent_Vofa(temp,encoder0.eAngle_360, 0); + while (temp > AA_LINE) { + temp -= AA_LINE; + } + + encoder0.eAngle_360 = temp / BB_ANGLE; +// SendCurrent_Vofa(OFFSET,encoder0.eAngle_360, 0); + return encoder0.eAngle_360; +} //#define EncoderName MT6816 -float32_t getAngle_MT6816() { -// printf("Test"); - - return M_PI; -} +//float32_t getAngle_MT6816() { +//// printf("Test"); +// +// return M_PI; +//} float32_t getAngle_AS5600() { // printf("Test"); diff --git a/BoooomCTL/Data/Angle/Angle.h b/BoooomCTL/Data/Angle/Angle.h index e26aaee..f9d87af 100644 --- a/BoooomCTL/Data/Angle/Angle.h +++ b/BoooomCTL/Data/Angle/Angle.h @@ -5,7 +5,22 @@ #ifndef BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H #define BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H -#include "APP_Main.h" +//#include "APP_Main.h" +#include "arm_math.h" +#include "main.h" +#include +#include +#include +#include "adc.h" +#include "dma.h" +#include "i2c.h" +#include "opamp.h" +#include "spi.h" +#include "tim.h" +#include "usb_device.h" +#include "gpio.h" + + typedef enum { MT6816, @@ -24,6 +39,9 @@ typedef struct { float32_t (*Angle_Init)(); } tData; +uint16_t MT_ReadAngle(void); +float32_t GetSpeed(uint16_t angleVal); + float32_t getAngle_MT6816(); float32_t getAngle_AS5600(); @@ -63,4 +81,27 @@ typedef struct sEncoder { float snap_threshold; } tEncoder; +struct ENCODER_Type { + SPI_HandleTypeDef *hspi; + GPIO_TypeDef *CS_Port; + uint16_t CS_Pin; + + uint16_t sendBuf[2]; + uint16_t recvBuf[2]; + + uint16_t offset; + uint16_t angleVal, lastAngle; + + int32_t cycleNum; + + float32_t angle_360; + float32_t eAngle_360; + +// int32_t angleArr[32][2]; +// uint8_t cnt; + + float32_t speed; + float32_t filt; + float32_t speedFilt; +}; #endif //BOOOOMFOC_STSPIN32G4_EVB_ANGLE_H diff --git a/Core/Src/main.c b/Core/Src/main.c index 8e2dfac..ea8f6cd 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -99,8 +99,8 @@ int main(void) { MX_OPAMP2_Init(); MX_OPAMP3_Init(); MX_ADC2_Init(); - MX_USB_Device_Init(); MX_SPI1_Init(); + MX_USB_Device_Init(); /* USER CODE BEGIN 2 */ APP_Init(); /* USER CODE END 2 */