Boooom
/
BoooomFOC_STSPIN32G4_EVB
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完成速度、电流闭环

This commit is contained in:
BoomPC 2024-05-17 00:43:58 +08:00
parent d531b7cc09
commit f404369a32
11 changed files with 507 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
APP/APP_Main.c
View File

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

4
APP/APP_Main.h
View File

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

54
APP/APP_Task.c
View File

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

235
BoooomCTL/Controller/Controller.c
View File

@ -5,25 +5,238 @@
#include <stdbool.h>
#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;
}

6
BoooomCTL/Controller/Controller.h
View File

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

3
BoooomCTL/Controller/PreDrive/PreDrive.c
View File

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

144
BoooomCTL/Controller/SVPWM/SVPWM.c
View File

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

6
BoooomCTL/Controller/SVPWM/SVPWM.h
View File

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

148
BoooomCTL/Data/Angle/Angle.c
View File

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

43
BoooomCTL/Data/Angle/Angle.h
View File

@ -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 <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#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

2
Core/Src/main.c
View File

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