Skocz do zawartości

Sterownik silnika BLDC z FOC + SVM


Pomocna odpowiedź

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 przez ps19
Link to post
Share on other sites

Dołącz do dyskusji, napisz odpowiedź!

Jeśli masz już konto to zaloguj się teraz, aby opublikować wiadomość jako Ty. Możesz też napisać teraz i zarejestrować się później.
Uwaga: wgrywanie zdjęć i załączników dostępne jest po zalogowaniu!

Anonim
Dołącz do dyskusji! Kliknij i zacznij pisać...

×   Wklejony jako tekst z formatowaniem.   Przywróć formatowanie

  Dozwolonych jest tylko 75 emoji.

×   Twój link będzie automatycznie osadzony.   Wyświetlać jako link

×   Twoja poprzednia zawartość została przywrócona.   Wyczyść edytor

×   Nie możesz wkleić zdjęć bezpośrednio. Prześlij lub wstaw obrazy z adresu URL.

×
×
  • Utwórz nowe...

Ważne informacje

Ta strona używa ciasteczek (cookies), dzięki którym może działać lepiej. Więcej na ten temat znajdziesz w Polityce Prywatności.