Sterownik silnika BLDC z FOC + SVM

[ps19]

Cześć,

Mam taki mały problem z odpaleniem sterowania wektorowego (FOC) z modulacją przestrzenną wektora (SVM). 

Problem polega na tym, że silnik zamiast kręcić się stoi w miejscu, jak próbuję go ręcznie ruszyć to pomiędzy cewkami jest wyczuwalny luz natomiast w miejscu cewek próbuje trzymać pozycję.

Raczej nie liczę na sprawdzenie kodu, ale podpowiedź co może powodować takie zachowanie.

FOC:

void focAlgorithm(motorParamsTypedef *mot)
{
	__disable_irq();
	static float32_t pSinVal, pCosVal;
	arm_sin_cos_f32(mot->enc.elecAngleDegrees_f32, &pSinVal, &pCosVal);

	arm_clarke_f32(mot->current.n_f32[PHASE_A], mot->current.n_f32[PHASE_B], &mot->current.alpha_f32, &mot->current.beta_f32);
	arm_park_f32(mot->current.alpha_f32, mot->current.beta_f32, &mot->current.IdMagnFlux_f32, &mot->current.IqRotaryTorque_f32, pSinVal, pCosVal);

	mot->current.targetIqRotaryTorque_f32 = mot->pidAngVel.PIDout; //PI working in SysTick handler below

	PIDIdMagnFluxFunc(&mot->pidId, &mot->current, mot->current.targetIdMagnFlux_f32);
	PIDIqRotaryTorqueFunc(&mot->pidIq, &mot->current, mot->current.targetIqRotaryTorque_f32);

	arm_inv_park_f32(mot->pidId.PIDout, mot->pidIq.PIDout, &mot->current.alphaPostInvPark_f32, &mot->current.betaPostInvPark_f32, pSinVal, pCosVal);
	arm_inv_clarke_f32(mot->current.alphaPostInvPark_f32, mot->current.betaPostInvPark_f32, &mot->current.aPostInvClarke_f32, &mot->current.bPostInvClarke_f32);

	svmAlgorithm(mot);
	__enable_irq();
}

SVM:

void svmAlgorithm(motorParamsTypedef *mot)
{
	float32_t Uout = 0, angle = 0;

	angle = mot->enc.elecAngleRadians_f32 + M_PI;

	//angle = atan2f(mot->current.betaPostInvPark, mot->current.alphaPostInvPark);

	//sq -> a^2
	arm_sqrt_f32(sq(mot->current.alphaPostInvPark_f32) + sq(mot->current.alphaPostInvPark_f32), &Uout);

	float32_t t1 = M_SQRT3_2*arm_sin_f32(M_PI_3 - angle)*Uout;
	float32_t t2 = M_SQRT3_2*arm_sin_f32(angle)*Uout;
	float32_t t0 = 1.0F - (t1 + t2);

	if(angle >= 0 && angle < DEG2RAD_60) //sector 1
	{
		mot->svm.a_f32 = t1 + t2 + (t0/2);
		mot->svm.b_f32 = t2 + (t0/2);
		mot->svm.c_f32 = (t0/2);
	}
	else if(angle >= DEG2RAD_60 && angle < DEG2RAD_120) //sector 2
	{
		mot->svm.a_f32 = t1 + (t0/2);
		mot->svm.b_f32 = t1 + t2 + (t0/2);
		mot->svm.c_f32 = (t0/2);
	}
	else if(angle >= DEG2RAD_120 && angle < DEG2RAD_180) //sector 3
	{
		mot->svm.a_f32 = (t0/2);
		mot->svm.b_f32 = t1 + t2 + (t0/2);
		mot->svm.c_f32 = t2 + (t0/2);
	}
	else if(angle >= DEG2RAD_180 && angle < DEG2RAD_240) //sector 4
	{
		mot->svm.a_f32 = (t0/2);
		mot->svm.b_f32 = t1 + (t0/2);
		mot->svm.c_f32 = t1 + t2 + (t0/2);
	}
	else if(angle >= DEG2RAD_240 && angle < DEG2RAD_300) //sector 5
	{
		mot->svm.a_f32 = t2 + (t0/2);
		mot->svm.b_f32 = (t0/2);
		mot->svm.c_f32 = t1 + t2 + (t0/2);
	}
	else if(angle >= DEG2RAD_300 && angle <= DEG2RAD_360) //sector 6
	{
		mot->svm.a_f32 = t1 + t2 + (t0/2);
		mot->svm.b_f32 = (t0/2);
		mot->svm.c_f32 = t1 + (t0/2);
	}
	else
	{
		mot->svm.a_f32 = 0;
		mot->svm.b_f32 = 0;
		mot->svm.c_f32 = 0;
		angle = 0;
	}

	mot->timMot->CCR1 = mot->svm.a_f32;
	mot->timMot->CCR2 = mot->svm.b_f32;
	mot->timMot->CCR3 = mot->svm.c_f32;
}

FOCInit:

void focInit(motorParamsTypedef *mot)
{
	float32_t Ts = 1.0F/TIM_PWM_FREQUENCY;
	float32_t Td = Ts;
	float32_t Ti = Ts;

	myAdc.adcFilterSteps_u16 = 10;
	myAdc.adcCalibSteps_u16 = 1000;
	myAdc.adcCalibFlag_enum = CALIB_START;

	mot->motorMaxVoltage_f32 = 1;
	mot->current.targetIdMagnFlux_f32 = 0;
	mot->svm.ts_f32 = Ts;

	mot->targetAngularVelocity_f32 = 10000;

	//PID1
	mot->pidAngVel.kp = 0.01;
	mot->pidAngVel.ki = 0.1;
	mot->pidAngVel.kd = 0;

	mot->pidAngVel.Ts = 0.001;
	mot->pidAngVel.Td = mot->pidAngVel.Ts;
	mot->pidAngVel.Ti = mot->pidAngVel.Ts;

	mot->pidAngVel.PIDoutMax = MOTOR_MAX_ANG_VEL_RAD;
	mot->pidAngVel.PIDoutMin = -MOTOR_MAX_ANG_VEL_RAD;

	mot->pidAngVel.errorTolerance = 0.01;

	//PID2
	mot->pidId.kp = computeKp(13);
	mot->pidId.ki = computeKi(13, 1);
	mot->pidId.kd = 0;

	mot->pidId.Ts = Ts;
	mot->pidId.Td = Td;
	mot->pidId.Ti = Ti;

	mot->pidId.PIDoutMax = MOTOR_MAX_CURRENT;
	mot->pidId.PIDoutMin = -MOTOR_MAX_CURRENT;

	mot->pidId.errorTolerance = 0.01;

	//PID3
	mot->pidIq.kp = computeKp(13);
	mot->pidIq.ki = computeKi(13, 1);
	mot->pidIq.kd = 0;

	mot->pidIq.Ts = Ts;
	mot->pidIq.Td = Td;
	mot->pidIq.Ti = Ti;

	mot->pidIq.PIDoutMax = MOTOR_MAX_CURRENT;
	mot->pidIq.PIDoutMin = -MOTOR_MAX_CURRENT;

	mot->pidIq.errorTolerance = 0.01;
}

 

Inne:

void PIDIdMagnFluxFunc(PidTypedef *pid, MotorCurrentTypedef *motI, float32_t targetIdMagnFlux)
{
	pid->valPres = motI->IdMagnFlux_f32;
	pid->valTarget = targetIdMagnFlux;
	PIDController(pid);
}

void PIDIqRotaryTorqueFunc(PidTypedef *pid, MotorCurrentTypedef *motI, float32_t targetIqRotaryTorque)
{
	pid->valPres = motI->IqRotaryTorque_f32;
	pid->valTarget = targetIqRotaryTorque;
	PIDController(pid);
}

void PIDMotorAngularVelocity(PidTypedef *pid, encoderTypedef *enc, float32_t targetAngularVelocity)
{
	pid->valPres = enc->angleRadiansSpeed_f32;
	pid->valTarget = targetAngularVelocity;
	PIDController(pid);
}

float32_t computeAB(float32_t VBusDC)
{
	//https://www.st.com/resource/en/user_manual/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf
	//page 38
	return (VBusDC*SHUNT_RES_VAL*SHUNT_AMPLIFICATION)*UC_REF_VOLTAGE;
}

float32_t computeKp(float32_t VBusDC)
{
	return MOTOR_LS_H*(MOTOR_MAX_ANG_VEL_RAD/computeAB(VBusDC))*PID_KP_DIV;
}

float32_t computeKi(float32_t VBusDC, float32_t T)
{
	return ((MOTOR_RS_OHM*MOTOR_MAX_ANG_VEL_RAD*PID_KI_DIV)/computeAB(VBusDC))*T;
}

void SysTick_Handler(void)
{
	encoderAngularSpeedRead(&motorLeftStruct.enc);
	encoderAngularSpeedRead(&motorRightStruct.enc);
	motorLeftStruct.targetAngularVelocity_f32=50;
	PIDMotorAngularVelocity(&motorLeftStruct.pidAngVel, &motorLeftStruct.enc, motorLeftStruct.targetAngularVelocity_f32);
	PIDMotorAngularVelocity(&motorRightStruct.pidAngVel, &motorRightStruct.enc, motorRightStruct.targetAngularVelocity_f32);
}

 

Edytowano Kwiecień 14, 2021 przez ps19