526b689693
1/ Added Spi that used to read data from AS5045b position sensor 2/ Added Base FOC code to project 3/ Added Standart DSP lib with optimazes sincos cals Need for FOC
117 lines
4.4 KiB
C
117 lines
4.4 KiB
C
/*
|
||
* ControlFuncs.c
|
||
*
|
||
* Created on: 20 авг. 2023 г.
|
||
* Author: on4ip
|
||
*/
|
||
#include "ControlFuncs.h"
|
||
/*current offsets form ADC channels*/
|
||
float offset_Ia=0;
|
||
float offset_Ib=0;
|
||
float offset_Ic=0;
|
||
|
||
|
||
void FAST_loop(SYSTEMobj_st *SYSTEMobj)
|
||
{
|
||
/*Get HW protection state*/
|
||
if(GET_HWfault_state(SYSTEMobj->DRIVE->drive_ID))
|
||
{
|
||
SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 1;
|
||
}
|
||
if(!SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED)
|
||
{
|
||
offset_Ia += SYSTEMobj->DRIVE->currentABC.current_Ia * SYSTEMobj->DRIVE->Tsample_fast;
|
||
offset_Ib += SYSTEMobj->DRIVE->currentABC.current_Ib * SYSTEMobj->DRIVE->Tsample_fast;
|
||
offset_Ic += SYSTEMobj->DRIVE->currentABC.current_Ic * SYSTEMobj->DRIVE->Tsample_fast;
|
||
}
|
||
SYSTEMobj->DRIVE->currentABC.current_Ia = SYSTEMobj->IO->current_A - offset_Ia;
|
||
SYSTEMobj->DRIVE->currentABC.current_Ib = SYSTEMobj->IO->current_B - offset_Ib;
|
||
SYSTEMobj->DRIVE->currentABC.current_Ic = SYSTEMobj->IO->current_C - offset_Ic;
|
||
SYSTEMobj->DRIVE->voltage_DC = SYSTEMobj->IO->voltage_DC;
|
||
/*Process FOC control*/
|
||
fast_loop_control(SYSTEMobj->DRIVE);
|
||
/*Calculate shaft torque*/
|
||
SYSTEMobj->DRIVE->torque_calc = SYSTEMobj->DRIVE->currentPark.current_Iq * SYSTEMobj->DRIVE->motor_data.Kt;
|
||
/*Process Electrical Position Observer*/
|
||
SYSTEMobj->OBSERVER_El->fullMode_ena = 0; /*Only speed calculation */
|
||
speed_observer(SYSTEMobj->OBSERVER_El);
|
||
SYSTEMobj->DRIVE->ElSpeed = SYSTEMobj->OBSERVER_El->W;
|
||
SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->OBSERVER_El->Teta_rad;// + SYSTEMobj->OBSERVER_El->teta_correction;
|
||
//if(SYSTEMobj->DRIVE->ElAngle >=toPI) SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->DRIVE->ElAngle -toPI;
|
||
//if(SYSTEMobj->DRIVE->ElAngle <0.f) SYSTEMobj->DRIVE->ElAngle = SYSTEMobj->DRIVE->ElAngle +toPI;
|
||
/*Process Mechanical Position Observer */
|
||
/*Observer dynamic correction in speed function*/
|
||
SYSTEMobj->OBSERVER_M->refresh_ena = 1;
|
||
SYSTEMobj->OBSERVER_M->fullMode_ena = 1; /*Full mode observer */
|
||
SYSTEMobj->OBSERVER_M->Ml_izm = SYSTEMobj->DRIVE->torque_calc;
|
||
SYSTEMobj->OBSERVER_M->L0 = mod(SYSTEMobj->OBSERVER_M->W)*-0.01f+0.93f;
|
||
if(SYSTEMobj->OBSERVER_M->L0 < 0.9f ) SYSTEMobj->OBSERVER_M->L0 = 0.90f;
|
||
speed_observer(SYSTEMobj->OBSERVER_M);
|
||
SYSTEMobj->DRIVE->speed_Fdb = SYSTEMobj->OBSERVER_M->W;
|
||
|
||
if(SYSTEMobj->DRIVE->CMD.cmd.bit_RST) /*Clear fault control*/
|
||
{
|
||
CLEAR_HWfault(SYSTEMobj->DRIVE->drive_ID);
|
||
SYSTEMobj->DRIVE->ERRORS.errors_word = 0;
|
||
SYSTEMobj->DRIVE->CMD.cmd.bit_RST = 0;
|
||
SYSTEMobj->DRIVE->STAT.stat.bit_FAULT = 0;
|
||
}
|
||
|
||
if(SYSTEMobj->DRIVE->CMD.cmd.bit_ON && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
|
||
{
|
||
SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 1; //Enable Drive
|
||
}else
|
||
{
|
||
SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED = 0; //Disable Drive
|
||
}
|
||
/*Calculate PWM reference values*/
|
||
float inv_dc = 1.0f / (SYSTEMobj->DRIVE->voltage_DC * VoltageNormalisation_coeff);
|
||
SYSTEMobj->PWM->Ualfa = DRIVE.voltage_Valpha_ref*inv_dc;
|
||
SYSTEMobj->PWM->Ubeta = DRIVE.voltage_Vbeta_ref*inv_dc;
|
||
SYSTEMobj->PWM->Uo = 0 * inv_dc;
|
||
SYSTEMobj->PWM->Trpd_pwm = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID);
|
||
PWM_proc(SYSTEMobj->PWM);
|
||
/*PWM module control*/
|
||
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH1N, LL_TIM_OCMODE_FORCED_ACTIVE);
|
||
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH2N, LL_TIM_OCMODE_FORCED_ACTIVE);
|
||
//LL_TIM_OC_SetMode(TIM1, LL_TIM_CHANNEL_CH3N, LL_TIM_OCMODE_FORCED_ACTIVE);
|
||
if(SYSTEMobj->DRIVE->STAT.stat.bit_ENABLED && (!SYSTEMobj->DRIVE->STAT.stat.bit_FAULT))
|
||
{
|
||
PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,(uint16_t)SYSTEMobj->PWM->Ta,(uint16_t)SYSTEMobj->PWM->Tb,(uint16_t)SYSTEMobj->PWM->Tc);
|
||
PWM_OUT_ENABLE(SYSTEMobj->DRIVE->drive_ID);
|
||
}else
|
||
{
|
||
uint16_t HALF_period = GET_PWM_Period(SYSTEMobj->DRIVE->drive_ID)>>1;
|
||
PWM_COMPARE_SET(SYSTEMobj->DRIVE->drive_ID,HALF_period,HALF_period,HALF_period);
|
||
PWM_OUT_DISABLE(SYSTEMobj->DRIVE->drive_ID);
|
||
}
|
||
}
|
||
/*Rate limiter function for Theta */
|
||
static inline void Rate_limiter(float *Rated_value, float Ref_value, float Up_rate, float Down_rate,float Tsample)
|
||
{
|
||
const float Down = Tsample*Down_rate;
|
||
const float Up = Tsample*Up_rate;
|
||
if((Ref_value - *Rated_value) >=PI)
|
||
{
|
||
*Rated_value = (*Rated_value+toPI);
|
||
}
|
||
if((Ref_value - *Rated_value)<=-PI)
|
||
{
|
||
*Rated_value = (*Rated_value-toPI);
|
||
}
|
||
if (Ref_value >(*Rated_value + Up))
|
||
{
|
||
*Rated_value += Up;
|
||
}
|
||
else if (Ref_value <(*Rated_value - Down))
|
||
{
|
||
*Rated_value -= Down;
|
||
}
|
||
else *Rated_value = Ref_value;
|
||
}
|
||
void SLOW_loop(SYSTEMobj_st *SYSTEMobj)
|
||
{
|
||
|
||
slow_loop_control(SYSTEMobj->DRIVE);
|
||
}
|
||
|