156 lines
4.4 KiB
C
156 lines
4.4 KiB
C
#include "dfoc.h"
|
||
#include "config.h"
|
||
#include "delay.h"
|
||
#include "lowpass_filter.h"
|
||
#include "mt6701.h"
|
||
#include "pid_control.h"
|
||
#include "pwm.h"
|
||
#include <math.h>
|
||
#include <stdio.h>
|
||
#include <stdlib.h>
|
||
#include <ti_msp_dl_config.h>
|
||
|
||
#define PI 3.14159265359f
|
||
#define _3PI_2 4.71238898f
|
||
#define _1_SQRT3 0.57735026919f
|
||
#define _2_SQRT3 1.15470053838f
|
||
|
||
struct Motor_ M0 = {0};
|
||
struct Motor_ M1 = {1};
|
||
|
||
struct _PID M0_VEL_PID = {0.4, 2, 0};
|
||
struct _PID M1_VEL_PID = {0.4, 2, 0};
|
||
|
||
struct LOWPASS M0_VEL_Filter = {0.1};
|
||
struct LOWPASS M1_VEL_Filter = {0.1};
|
||
|
||
struct MT6701 Angle_Sensor0 = {0};
|
||
struct MT6701 Angle_Sensor1 = {1};
|
||
|
||
#define VOLTAGE_POWER_SUPPLY 12.0f
|
||
#define VOLTAGE_LIMIT 6
|
||
#define PWM_COMPARE_VALUE 750.0f
|
||
|
||
volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0,
|
||
dc_c = 0;
|
||
|
||
float M0_zero_elc_Angle = 0, M1_zero_elc_Angle = 0;
|
||
|
||
extern int M0_PP, M0_DIR;
|
||
extern int M1_PP, M1_DIR;
|
||
|
||
void Motor_en() {}
|
||
|
||
// 限制幅值
|
||
float constrain(float amt, float low, float high) {
|
||
return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
|
||
}
|
||
|
||
// 将角度归化到0-2PI
|
||
float normalizeAngle(float angle) {
|
||
volatile float a = fmod(angle, 2 * PI);
|
||
return ((a >= 0) ? a : (a + 2 * PI));
|
||
}
|
||
|
||
float M0_electricAngle(void) {
|
||
return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor0) * M0_PP * M0_DIR) -
|
||
M0_zero_elc_Angle);
|
||
}
|
||
|
||
float M1_electricAngle(void) {
|
||
return normalizeAngle((GetAngle_NoTrack(&Angle_Sensor1) * M1_PP * M1_DIR) -
|
||
M1_zero_elc_Angle);
|
||
}
|
||
void SetPwm(int Mot_num, float Ua, float Ub, float Uc) {
|
||
volatile float U_a = 0.0;
|
||
volatile float U_b = 0.0;
|
||
volatile 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);
|
||
|
||
if (Mot_num == 0) {
|
||
M0.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
M0.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
M0.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
|
||
M0_PWM_A(M0.dc_a * PWM_COMPARE_VALUE); // 频率15k
|
||
M0_PWM_B(M0.dc_b * PWM_COMPARE_VALUE);
|
||
M0_PWM_C(M0.dc_c * PWM_COMPARE_VALUE);
|
||
} else if (Mot_num == 1) {
|
||
M1.dc_a = constrain(U_a / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
M1.dc_b = constrain(U_b / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
M1.dc_c = constrain(U_c / VOLTAGE_POWER_SUPPLY, 0.0f, 1.0f);
|
||
|
||
M1_PWM_A(M1.dc_a * PWM_COMPARE_VALUE); // 频率15k
|
||
M1_PWM_B(M1.dc_b * PWM_COMPARE_VALUE);
|
||
M1_PWM_C(M1.dc_c * PWM_COMPARE_VALUE);
|
||
}
|
||
}
|
||
|
||
// FOC核心算法，克拉克逆变换/帕克逆变换
|
||
|
||
void SetPhaseVoltage(struct Motor_ *Motor, float Uq, float angle_el) {
|
||
// angle_el = normalizeAngle(angle_el);
|
||
|
||
Motor->Ualpha = -Uq * sin(angle_el);
|
||
Motor->Ubeta = Uq * cos(angle_el);
|
||
|
||
Motor->Ua = Motor->Ualpha + VOLTAGE_POWER_SUPPLY / 2;
|
||
Motor->Ub =
|
||
(sqrt(3) * Motor->Ubeta - Motor->Ualpha) / 2 + VOLTAGE_POWER_SUPPLY / 2;
|
||
Motor->Uc =
|
||
-(Motor->Ualpha + sqrt(3) * Motor->Ubeta) / 2 + VOLTAGE_POWER_SUPPLY / 2;
|
||
|
||
SetPwm(Motor->Mot_num, Motor->Ua, Motor->Ub, Motor->Uc);
|
||
}
|
||
|
||
void Check_Sensor(void) {
|
||
SetPhaseVoltage(&M0, 3, _3PI_2);
|
||
delay_ms(2000);
|
||
M0_zero_elc_Angle = M0_electricAngle();
|
||
SetPhaseVoltage(&M0, 0, _3PI_2);
|
||
delay_ms(500);
|
||
|
||
SetPhaseVoltage(&M1, 3, _3PI_2);
|
||
delay_ms(2000);
|
||
M1_zero_elc_Angle = M1_electricAngle();
|
||
SetPhaseVoltage(&M1, 0, _3PI_2);
|
||
delay_ms(500);
|
||
}
|
||
|
||
void FOC_Init() {
|
||
// PWM_Init();
|
||
// CurrSense_Init();
|
||
// mt6701_Init();
|
||
Check_Sensor();
|
||
}
|
||
|
||
void Print_Velocity(int Motor_Velocity) {
|
||
if (Motor_Velocity == 0) {
|
||
printf("M0:%f\n", GetVelocity(&Angle_Sensor0));
|
||
}
|
||
if (Motor_Velocity == 1) {
|
||
printf("M1:%f\n", GetVelocity(&Angle_Sensor1));
|
||
}
|
||
}
|
||
|
||
// 单速度环
|
||
void M0_Set_Velocity(float Target) {
|
||
Angle_Sensor0.velocity =
|
||
Lowpassfilter(&M0_VEL_Filter, GetVelocity(&Angle_Sensor0));
|
||
SetPhaseVoltage(
|
||
&M0,
|
||
PID_Controller(&M0_VEL_PID, M0_DIR * (Target - Angle_Sensor0.velocity)),
|
||
M0_electricAngle());
|
||
}
|
||
|
||
void M1_Set_Velocity(float Target) {
|
||
Angle_Sensor1.velocity =
|
||
Lowpassfilter(&M1_VEL_Filter, GetVelocity(&Angle_Sensor1));
|
||
SetPhaseVoltage(
|
||
&M1,
|
||
PID_Controller(&M1_VEL_PID, M1_DIR * (Target - Angle_Sensor1.velocity)),
|
||
M1_electricAngle());
|
||
} |