187 lines
4.4 KiB
C
187 lines
4.4 KiB
C
|
||
#include <stdlib.h>
|
||
#include <stdio.h>
|
||
#include "pwm.h"
|
||
#include "mt6701.h"
|
||
#include "delay.h"
|
||
#include "lowpass_filter.h"
|
||
#include "pid_control.h"
|
||
#include <math.h>
|
||
#include <ti_msp_dl_config.h>
|
||
#include "config.h"
|
||
|
||
#define PI 3.14159265359f
|
||
#define _3PI_2 4.71238898f
|
||
#define _1_SQRT3 0.57735026919f
|
||
#define _2_SQRT3 1.15470053838f
|
||
|
||
volatile float Ua = 0, Ub = 0, Uc = 0, Ualpha, Ubeta = 0, dc_a = 0, dc_b = 0, dc_c = 0;
|
||
const float voltage_limit = 8;
|
||
const float voltage_power_supply = 12.0f;
|
||
volatile 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);
|
||
}
|
||
|
||
//限制幅值
|
||
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 electricAngle(void)
|
||
{
|
||
return normalizeAngle((GetAngle_NoTrack() * pp * Dir) - zero_electric_Angle);
|
||
}
|
||
|
||
void SetPwm(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);
|
||
//printf("Ua : %f -- Ub : %f -- Uc : %f -- U_a : %f -- U_b : %f -- U_c : %f \n",Ua,Ub,Uc,U_a,U_b,U_c);
|
||
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 * 1000.0f); // 频率15k
|
||
PWM_Channel2(dc_b * 1000.0f);
|
||
PWM_Channel3(dc_c * 1000.0f);
|
||
|
||
|
||
|
||
}
|
||
|
||
//FOC核心算法，克拉克逆变换/帕克逆变换
|
||
volatile float test_angle = 0.0;
|
||
volatile 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;
|
||
}
|
||
|
||
void Check_Sensor(void)
|
||
{
|
||
//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();
|
||
}
|
||
|
||
|
||
// 单角度环
|
||
void Set_Angle(float Target)
|
||
{
|
||
volatile float langle = GetAngle();
|
||
if(DEBUG_ENABLED & DEBUG_DFOC_ENABLED)
|
||
{
|
||
printf("angle readback in dfoc.c is %f \n", langle);
|
||
}
|
||
volatile float Uq = PID_Controller(0.133, 0.01, 0, (Target - Dir * langle) * 180 / PI);
|
||
//volatile float Uq = PID_Controller(0.06, 0, 0, (Target - Dir * langle) * 180 / PI);
|
||
if(DEBUG_ENABLED)
|
||
{
|
||
printf("Uq is %f \n", Uq);
|
||
}
|
||
SetPhaseVoltage(Uq, 0, electricAngle());
|
||
}
|
||
|
||
volatile double openloop_timestamp;
|
||
float velocityopenloop(float target)
|
||
{
|
||
volatile float Uq = 0.0;
|
||
volatile double Ts = 0.0;
|
||
volatile double shaft_angle;
|
||
|
||
volatile uint32_t now_ts = DL_SYSTICK_getValue();
|
||
|
||
|
||
if(now_ts < openloop_timestamp)
|
||
{
|
||
Ts = (openloop_timestamp - now_ts) / 80.0f * 1e-6f;
|
||
}
|
||
else
|
||
{
|
||
Ts = (0xFFFFFF - now_ts + openloop_timestamp) / 80.0f * 1e-6f;
|
||
}
|
||
|
||
if(Ts < 0 || Ts >= 0.005)
|
||
{
|
||
Ts = 0.001f;
|
||
}
|
||
|
||
shaft_angle = normalizeAngle(shaft_angle + pp * target * Ts);
|
||
|
||
if(DEBUG_ENABLED)
|
||
{
|
||
//printf("shaft_angle : %f -- target : %f -- Ts : %f \n",shaft_angle,target,Ts);
|
||
}
|
||
|
||
|
||
Uq = voltage_limit;
|
||
|
||
SetPhaseVoltage(Uq, 0, shaft_angle);
|
||
printf("shaft_angle : %f \n", shaft_angle);
|
||
openloop_timestamp = now_ts;
|
||
|
||
return Uq;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|