400 lines
11 KiB
C
400 lines
11 KiB
C
//
|
|
// Created by hu123456 on 2024/1/16.
|
|
//
|
|
#include "foc.h"
|
|
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
|
|
|
|
|
float Voltage_PowerSupply = 12;
|
|
float Pole_Pairs = 14;
|
|
float Voltage_Limit = 6.9;
|
|
float Velocity_Limit = 100;
|
|
float V_Acc_Limit = 100;
|
|
float dir = 1;
|
|
|
|
FOCTypeDef FOCStruct_X={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
|
FOCTypeDef FOCStruct_Y={0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
|
|
|
void PID_Iint(void)
|
|
{
|
|
FOCStruct_X.ZT_KP = 2;
|
|
FOCStruct_X.ZT_KI = 0;
|
|
FOCStruct_X.ZT_KD = 0.2;
|
|
|
|
FOCStruct_X.V_KP = 0.1;
|
|
FOCStruct_X.V_KI = 1.5;
|
|
FOCStruct_X.V_KD = 0;
|
|
//2024/1/2 Y轴效果不错
|
|
FOCStruct_Y.ZT_KP = 2;
|
|
FOCStruct_Y.ZT_KI = 0;
|
|
FOCStruct_Y.ZT_KD = 0.5;
|
|
|
|
FOCStruct_Y.V_KP = 0.1;
|
|
FOCStruct_Y.V_KI = 1;
|
|
FOCStruct_Y.V_KD = 0;
|
|
|
|
|
|
////2024/1/23 xY轴效果不错
|
|
// FOCStruct_X.ZT_KP = 2;
|
|
// FOCStruct_X.ZT_KI = 0;
|
|
// FOCStruct_X.ZT_KD = 0.2;
|
|
//
|
|
// FOCStruct_X.V_KP = 0.1;
|
|
// FOCStruct_X.V_KI = 1;
|
|
// FOCStruct_X.V_KD = 0;
|
|
////2024/1/22 Y轴效果不错
|
|
// FOCStruct_Y.ZT_KP = 2;
|
|
// FOCStruct_Y.ZT_KI = 0;
|
|
// FOCStruct_Y.ZT_KD = 0.5;
|
|
//
|
|
// FOCStruct_Y.V_KP = 0.1;
|
|
// FOCStruct_Y.V_KI = 1;
|
|
// FOCStruct_Y.V_KD = 0;
|
|
|
|
}
|
|
void setPhaseVoltage(unsigned char motor,float Uq, float Ud, double angle_el)
|
|
{
|
|
float Uout;
|
|
unsigned int sector;
|
|
float T0,T1,T2;
|
|
float Ta,Tb,Tc;
|
|
|
|
if(Ud) // only if Ud and Uq set
|
|
{
|
|
Uout = sqrt(Ud*Ud + Uq*Uq) / Voltage_PowerSupply;
|
|
// angle normalisation in between 0 and 2pi
|
|
// only necessary if using _sin and _cos - approximation functions
|
|
angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
|
|
}
|
|
else
|
|
{// only Uq available - no need for atan2 and sqrt
|
|
Uout = Uq / Voltage_PowerSupply;
|
|
// angle normalisation in between 0 and 2pi
|
|
// only necessary if using _sin and _cos - approximation functions
|
|
angle_el = _normalizeAngle(angle_el + _PI_2);
|
|
}
|
|
if(Uout> 0.577)Uout= 0.577;
|
|
if(Uout<-0.577)Uout=-0.577;
|
|
|
|
sector = (angle_el / _PI_3) + 1;
|
|
T1 = sqrt(3)*sin(sector*_PI_3 - angle_el) * Uout;
|
|
T2 = sqrt(3)*sin(angle_el - (sector-1.0)*_PI_3) * Uout;
|
|
T0 = 1 - T1 - T2;
|
|
|
|
// calculate the duty cycles(times)
|
|
switch(sector)
|
|
{
|
|
case 1:
|
|
Ta = T1 + T2 + T0/2;
|
|
Tb = T2 + T0/2;
|
|
Tc = T0/2;
|
|
break;
|
|
case 2:
|
|
Ta = T1 + T0/2;
|
|
Tb = T1 + T2 + T0/2;
|
|
Tc = T0/2;
|
|
break;
|
|
case 3:
|
|
Ta = T0/2;
|
|
Tb = T1 + T2 + T0/2;
|
|
Tc = T2 + T0/2;
|
|
break;
|
|
case 4:
|
|
Ta = T0/2;
|
|
Tb = T1+ T0/2;
|
|
Tc = T1 + T2 + T0/2;
|
|
break;
|
|
case 5:
|
|
Ta = T2 + T0/2;
|
|
Tb = T0/2;
|
|
Tc = T1 + T2 + T0/2;
|
|
break;
|
|
case 6:
|
|
Ta = T1 + T2 + T0/2;
|
|
Tb = T0/2;
|
|
Tc = T1 + T0/2;
|
|
break;
|
|
default: // possible error state
|
|
Ta = 0;
|
|
Tb = 0;
|
|
Tc = 0;
|
|
}
|
|
if (motor == motory)
|
|
{
|
|
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*4800);
|
|
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*4800);
|
|
__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*4800);
|
|
|
|
}
|
|
else if (motor == motorx)
|
|
{
|
|
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_1,Ta*4800);
|
|
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_2,Tb*4800);
|
|
__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,Tc*4800);
|
|
}
|
|
}
|
|
|
|
void speed_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float V_Target)
|
|
{
|
|
float Uq,Ud;
|
|
double electrical_angle;
|
|
unsigned long Time_Now;
|
|
double Ts;
|
|
|
|
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
|
|
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
|
|
else
|
|
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
|
|
FOCStruct->Time_Prev = Time_Now;
|
|
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
|
|
|
|
//获取角度求出V_Now
|
|
double Angle_Now=getAngle(motor);
|
|
|
|
double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
|
|
FOCStruct->Angle_Prev = Angle_Now;
|
|
V_Now = dir*LPF_velocity(V_Now,FOCStruct);
|
|
double V_Error = V_Target - V_Now;
|
|
|
|
float V_Pid_P = FOCStruct->V_KP * V_Error;
|
|
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
|
|
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
|
|
|
|
float V_Out = V_Pid_P+V_Pid_I;
|
|
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
|
|
|
|
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
|
|
if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
|
|
else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;
|
|
|
|
FOCStruct->V_Pid_IPrev = V_Pid_I;
|
|
FOCStruct->V_Out_Prev = V_Out;
|
|
FOCStruct->V_Error_Prev = V_Error;
|
|
|
|
Uq = V_Out;
|
|
Ud = 0;
|
|
|
|
|
|
// double shaft_angle = dir*Angle_Now;
|
|
double shaft_angle = dir*Angle_Now;
|
|
|
|
// electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
|
|
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
|
|
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
|
|
}
|
|
|
|
void Zitai_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float ZT_Error)
|
|
{
|
|
float Uq,Ud;
|
|
double electrical_angle;
|
|
unsigned long Time_Now;
|
|
double Ts;
|
|
|
|
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
|
|
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
|
|
else
|
|
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
|
|
FOCStruct->Time_Prev = Time_Now;
|
|
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
|
|
|
|
//姿态环
|
|
float V_Target = FOCStruct->ZT_KP * ZT_Error + FOCStruct->ZT_KD * (ZT_Error - FOCStruct->ZT_ErrorPrev) / Ts;
|
|
V_Target = _constrain(V_Target, -Velocity_Limit, Velocity_Limit);
|
|
FOCStruct->ZT_ErrorPrev = ZT_Error;
|
|
|
|
//速度环
|
|
//获取角度求出V_Now
|
|
double Angle_Now=getAngle(motor);
|
|
|
|
double V_Now = (Angle_Now - FOCStruct->Angle_Prev)/Ts;
|
|
FOCStruct->Angle_Prev = Angle_Now;
|
|
V_Now = dir*LPF_velocity(V_Now,FOCStruct);
|
|
double V_Error = V_Target - V_Now;
|
|
|
|
float V_Pid_P = FOCStruct->V_KP * V_Error;
|
|
float V_Pid_I = FOCStruct->V_Pid_IPrev + FOCStruct->V_KI*Ts*0.5*(V_Error + FOCStruct->V_Error_Prev);
|
|
V_Pid_I = _constrain(V_Pid_I, -Voltage_Limit, Voltage_Limit);
|
|
|
|
float V_Out = V_Pid_P+V_Pid_I;
|
|
V_Out = _constrain(V_Out, -Voltage_Limit, Voltage_Limit);
|
|
|
|
float V_Acc_Now = (V_Out - FOCStruct->V_Out_Prev)/Ts;
|
|
if(V_Acc_Now > V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev + V_Acc_Limit*Ts;
|
|
else if(V_Acc_Now < -V_Acc_Limit) V_Out = FOCStruct->V_Out_Prev - V_Acc_Limit*Ts;
|
|
|
|
FOCStruct->V_Pid_IPrev = V_Pid_I;
|
|
FOCStruct->V_Out_Prev = V_Out;
|
|
FOCStruct->V_Error_Prev = V_Error;
|
|
|
|
Uq = V_Out;
|
|
Ud = 0;
|
|
|
|
|
|
// double shaft_angle = dir*Angle_Now;
|
|
double shaft_angle = dir*Angle_Now;
|
|
|
|
// electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs);
|
|
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
|
|
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
|
|
}
|
|
|
|
void Angel_closed_loop(unsigned motor,FOCTypeDef *FOCStruct,float Angle_target)
|
|
{
|
|
float Uq,Ud;
|
|
double electrical_angle;
|
|
unsigned long Time_Now;
|
|
double Ts;
|
|
|
|
Time_Now = __HAL_TIM_GET_COUNTER(&htim5); //100ns
|
|
if(Time_Now > FOCStruct->Time_Prev)Ts = (float)(Time_Now - FOCStruct->Time_Prev)*1e-7f;
|
|
else
|
|
Ts = (float)(0xFFFFFFFF - FOCStruct->Time_Prev + Time_Now)*1e-7f;
|
|
FOCStruct->Time_Prev = Time_Now;
|
|
if(Ts == 0 || Ts > 400) Ts = 1e-3f;
|
|
|
|
//角度环
|
|
double Angle_Now=getAngle(motor);
|
|
|
|
float error = Angle_target - Angle_Now;
|
|
Uq = 20 * error;
|
|
Uq = _constrain(Uq, -Voltage_Limit, Voltage_Limit);
|
|
|
|
double shaft_angle = dir*Angle_Now;
|
|
electrical_angle = _normalizeAngle((shaft_angle ) * Pole_Pairs - FOCStruct->zero_electric_angle);
|
|
setPhaseVoltage(motor,Uq,Ud,electrical_angle);
|
|
}
|
|
|
|
double LPF_velocity(double x,FOCTypeDef *FOCStruct)
|
|
{
|
|
float y = 0.9*(FOCStruct->V_Prev)+ 0.1*x;
|
|
FOCStruct->V_Prev=y;
|
|
return y;
|
|
}
|
|
|
|
// normalizing radian angle to [0,2PI]
|
|
double _normalizeAngle(float angle)
|
|
{
|
|
float a = fmod(angle, _2PI);
|
|
return a >= 0 ? a : (a + _2PI);
|
|
}
|
|
|
|
|
|
void PWM_Start()
|
|
{
|
|
HAL_TIM_Base_Start(&htim2); //启动定时器
|
|
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1);
|
|
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
|
|
HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
|
|
|
|
|
|
HAL_TIM_Base_Start(&htim3); //启动定时器
|
|
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
|
|
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2);
|
|
HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
|
|
}
|
|
|
|
void moto_Init()
|
|
{
|
|
|
|
GPIO_InitTypeDef GPIO_InitStruct = {0};
|
|
__HAL_RCC_GPIOB_CLK_ENABLE();
|
|
__HAL_RCC_GPIOC_CLK_ENABLE();
|
|
|
|
GPIO_InitStruct.Pin = GPIO_PIN_0;
|
|
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
|
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
|
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
|
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
|
|
|
|
|
|
GPIO_InitStruct.Pin = GPIO_PIN_9;
|
|
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
|
|
GPIO_InitStruct.Pull = GPIO_NOPULL;
|
|
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
|
|
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
|
|
|
|
HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,1);
|
|
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,1);
|
|
|
|
PWM_Start();
|
|
|
|
HAL_Delay(1000);
|
|
alignSensor(motory,&FOCStruct_Y);
|
|
alignSensor(motorx,&FOCStruct_X);
|
|
HAL_Delay(50);
|
|
}
|
|
|
|
int alignSensor(unsigned char moto,FOCTypeDef * FOCStruct)
|
|
{
|
|
long i;
|
|
float angle;
|
|
float mid_angle = 0,end_angle = 0;
|
|
float moved;
|
|
float q = 3.5;
|
|
printf("MOT: Align sensor.\r\n");
|
|
|
|
// find natural direction
|
|
// move one electrical revolution forward
|
|
HAL_Delay(100);
|
|
for(i=0; i<=500; i++)
|
|
{
|
|
angle = _3PI_2 + _2PI * i / 500.0;
|
|
setPhaseVoltage(moto,q, 0, angle);
|
|
HAL_Delay(2);
|
|
}
|
|
mid_angle=getAngle(moto);
|
|
for(i=500; i>=0; i--)
|
|
{
|
|
angle = _3PI_2 + _2PI * i / 500.0 ;
|
|
setPhaseVoltage(moto,q, 0, angle);
|
|
HAL_Delay(2);
|
|
}
|
|
end_angle=getAngle(moto);
|
|
setPhaseVoltage(moto,0, 0, 0);
|
|
HAL_Delay(200);
|
|
|
|
printf("mid_angle=%.4f\r\n",mid_angle);
|
|
printf("end_angle=%.4f\r\n",end_angle);
|
|
|
|
moved = fabs(mid_angle - end_angle);
|
|
if((mid_angle == end_angle)||(moved < 0.02)) //相等或者几乎没有动
|
|
{
|
|
printf("MOT: Failed to notice movement loop222.\r\n");
|
|
// M1_Disable; //电机检测不正常，关闭驱动
|
|
return 0;
|
|
}
|
|
else if(mid_angle < end_angle)
|
|
{
|
|
printf("MOT: sensor_direction==CCW\r\n");
|
|
// dir = -1;
|
|
}
|
|
else
|
|
{
|
|
printf("MOT: sensor_direction==CW\r\n");
|
|
// dir = 1;
|
|
}
|
|
printf("MOT: PP check: \n"); //计算Pole_Pairs
|
|
int pp;
|
|
if( fabs(moved*Pole_Pairs - _2PI) > 0.5 ) // 0.5 is arbitrary number it can be lower or higher!
|
|
{
|
|
printf("fail - estimated pp\n");
|
|
pp = _2PI/moved+0.5; //浮点数转整形，四舍五入
|
|
printf("pp=%d\r\n",pp);
|
|
}
|
|
else
|
|
printf("OK!\r\n");
|
|
setPhaseVoltage(moto,q,0,_3PI_2); //计算零点偏移角度
|
|
HAL_Delay(700);
|
|
FOCStruct->zero_electric_angle = _normalizeAngle(dir*getAngle(moto) * Pole_Pairs);
|
|
// delay_ms(20);
|
|
// printf("MOT: Zero elec. angle:");
|
|
// printf("%.4f\r\n",zero_electric_angle);
|
|
|
|
setPhaseVoltage(moto,0, 0, 0);
|
|
HAL_Delay(50);
|
|
|
|
return 1;
|
|
}
|
|
|
|
|