389 lines
11 KiB
C
389 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)))
|
||
const float sqrt_3 = 1.7320508075f;
|
||
|
||
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 = 3.5;
|
||
FOCStruct_X.ZT_KI = 0;
|
||
FOCStruct_X.ZT_KD = 0.5;
|
||
|
||
FOCStruct_X.V_KP = 0.1;
|
||
FOCStruct_X.V_KI = 2;
|
||
FOCStruct_X.V_KD = 0;
|
||
//////////
|
||
FOCStruct_Y.ZT_KP = 3.0;
|
||
FOCStruct_Y.ZT_KI = 0;
|
||
FOCStruct_Y.ZT_KD = 0.5;
|
||
|
||
FOCStruct_Y.V_KP = 0.1;
|
||
FOCStruct_Y.V_KI = 1.4;
|
||
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 ==motorx )
|
||
{
|
||
__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 == motory)
|
||
{
|
||
__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;
|
||
|
||
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>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;
|
||
|
||
//<2F><>̬<EFBFBD><CCAC>
|
||
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;
|
||
|
||
//<2F>ٶȻ<D9B6>
|
||
//<2F><>ȡ<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>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;
|
||
|
||
//<2F>ǶȻ<C7B6>
|
||
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); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
||
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); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
|
||
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_gpio_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);
|
||
|
||
}
|
||
|
||
|
||
void moto_Init()
|
||
{
|
||
Voltage_PowerSupply = get_VCC();
|
||
Voltage_Limit = Voltage_PowerSupply / sqrt_3 - 0.1;
|
||
printf("Voltage_PowerSupply: %f V\r\n",Voltage_PowerSupply);
|
||
moto_gpio_init();
|
||
PWM_Start();
|
||
|
||
HAL_Delay(1000);
|
||
alignSensor(motory,&FOCStruct_Y); //<2F><><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD>
|
||
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)) //<2F><><EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD><DFBC><EFBFBD>û<EFBFBD>ж<EFBFBD>
|
||
{
|
||
printf("MOT: Failed to notice movement loop222.\r\n");
|
||
// M1_Disable; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ⲻ<EFBFBD><E2B2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD>
|
||
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"); //<2F><><EFBFBD><EFBFBD>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; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Σ<EFBFBD><CEA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
printf("pp=%d\r\n",pp);
|
||
}
|
||
else
|
||
printf("OK!\r\n");
|
||
setPhaseVoltage(moto,q,0,_3PI_2); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD>ƽǶ<C6BD>
|
||
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;
|
||
}
|
||
|
||
|