Przeszukaj forum

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); }