From 6da314c2cdd667ec7a6c1b1f75b2d6b3a3e0e232 Mon Sep 17 00:00:00 2001 From: stancecoke Date: Fri, 5 Feb 2021 14:59:22 +0100 Subject: [PATCH] PI control with struct --- Inc/FOC.h | 103 +++++++++++++++++++++++++++-------------------------- Inc/main.h | 14 ++++++++ Src/FOC.c | 31 ++++++++-------- Src/main.c | 39 ++++++++++++++++---- 4 files changed, 114 insertions(+), 73 deletions(-) diff --git a/Inc/FOC.h b/Inc/FOC.h index 3ff56499..47db4604 100644 --- a/Inc/FOC.h +++ b/Inc/FOC.h @@ -1,51 +1,52 @@ -/* - * FOC.h - * - * Created on: 25.01.2019 - * Author: stancecoke - */ - -#ifndef FOC_H_ -#define FOC_H_ - -#include -#include "config.h" -//exportetd functions -void FOC_calculation(int16_t int16_i_as, int16_t int16_i_bs, q31_t q31_teta, int16_t int16_i_q_target, MotorState_t* MS_FOC); -q31_t PI_control_i_q (q31_t ist, q31_t soll); -q31_t PI_control_i_d (q31_t ist, q31_t soll); - -// Maximum Voltage applyed - -#ifdef DISABLE_DYNAMIC_ADC -#define _U_MAX 2000L //little lower than period of timer1 for proper phase current reading. Could be improved by dynamic timing of AD-conversion -#else -#define _U_MAX 2000L -#endif - - - -// Square Root of 3 -#define _SQRT3 28 //1.73205081*16 - -#define ADC_DUR 250//minimal duration for proper ADC reading deadtime + noise subsiding + sample time - - -//globals -extern q31_t temp1; -extern q31_t temp2; -extern q31_t temp3; -extern q31_t temp4; -extern q31_t temp5; -extern q31_t temp6; -extern char PI_flag; - - - -extern q31_t e_log[300][6]; -extern char Obs_flag; -extern uint8_t ui8_debug_state; - - - -#endif /* FOC_H_ */ +/* + * FOC.h + * + * Created on: 25.01.2019 + * Author: stancecoke + */ + +#ifndef FOC_H_ +#define FOC_H_ + +#include +#include "config.h" +//exportetd functions +void FOC_calculation(int16_t int16_i_as, int16_t int16_i_bs, q31_t q31_teta, int16_t int16_i_q_target, MotorState_t* MS_FOC); +q31_t PI_control (PI_control_t* PI_c); +//q31_t PI_control_i_q (q31_t ist, q31_t soll); +//q31_t PI_control_i_d (q31_t ist, q31_t soll); + +// Maximum Voltage applyed + +#ifdef DISABLE_DYNAMIC_ADC +#define _U_MAX 2000L //little lower than period of timer1 for proper phase current reading. Could be improved by dynamic timing of AD-conversion +#else +#define _U_MAX 2000L +#endif + + + +// Square Root of 3 +#define _SQRT3 28 //1.73205081*16 + +#define ADC_DUR 250//minimal duration for proper ADC reading deadtime + noise subsiding + sample time + + +//globals +extern q31_t temp1; +extern q31_t temp2; +extern q31_t temp3; +extern q31_t temp4; +extern q31_t temp5; +extern q31_t temp6; +extern char PI_flag; + + + +extern q31_t e_log[300][6]; +extern char Obs_flag; +extern uint8_t ui8_debug_state; + + + +#endif /* FOC_H_ */ diff --git a/Inc/main.h b/Inc/main.h index 53ea769d..ae1cece5 100644 --- a/Inc/main.h +++ b/Inc/main.h @@ -150,6 +150,20 @@ typedef struct }MotorParams_t; +typedef struct +{ + uint16_t gain_p; + uint16_t gain_i; + uint16_t limit_i; + uint16_t limit_output; + int16_t recent_vaule; + int16_t setpoint; + int32_t integral_part; + uint16_t max_step; + uint8_t shift; + +}PI_control_t; + /* USER CODE END Private defines */ #ifdef __cplusplus diff --git a/Src/FOC.c b/Src/FOC.c index 25801982..d2e4cbd8 100644 --- a/Src/FOC.c +++ b/Src/FOC.c @@ -131,32 +131,31 @@ if(!MS_FOC->hall_angle_detect_flag){ } //PI Control for quadrature current iq (torque) -q31_t PI_control_i_q (q31_t ist, q31_t soll) +q31_t PI_control (PI_control_t* PI_c) { q31_t q31_p; //proportional part - static q31_t q31_q_i = 0; //integral part - static q31_t q31_q_dc = 0; // sum of proportional and integral part - q31_p = ((soll - ist)*P_FACTOR_I_Q); - q31_q_i += ((soll - ist)*I_FACTOR_I_Q); - temp5=q31_p; - temp6=q31_q_i; - if (q31_q_i>_U_MAX<<10) q31_q_i=_U_MAX<<10; - if (q31_q_i<-_U_MAX<<10) q31_q_i = -_U_MAX<<10; - if(!READ_BIT(TIM1->BDTR, TIM_BDTR_MOE))q31_q_i = 0 ; //reset integral part if PWM is disabled + static q31_t q31_out = 0; // sum of proportional and integral part + q31_p = ((PI_c->setpoint - PI_c->recent_vaule)*PI_c->gain_p); + PI_c->integral_part += ((PI_c->setpoint - PI_c->recent_vaule)*PI_c->gain_i); + + + if (PI_c->integral_part > PI_c->limit_i << PI_c->shift) PI_c->integral_part = PI_c->limit_i << PI_c->shift; + if (PI_c->integral_part < -(PI_c->limit_i << PI_c->shift)) PI_c->integral_part = -(PI_c->limit_i << PI_c->shift); + if(!READ_BIT(TIM1->BDTR, TIM_BDTR_MOE))PI_c->integral_part = 0 ; //reset integral part if PWM is disabled //avoid too big steps in one loop run - if ((q31_p+q31_q_i)>>10>q31_q_dc+5) q31_q_dc+=5; - else if ((q31_p+q31_q_i)>>10>10; + if ((q31_p+PI_c->integral_part)>>10 > q31_out+PI_c->max_step) q31_out+=PI_c->max_step; + else if ((q31_p+PI_c->integral_part)>>10 < q31_out-PI_c->max_step)q31_out-=PI_c->max_step; + else q31_out=(q31_p+PI_c->integral_part)>>10; - if (q31_q_dc>_U_MAX) q31_q_dc = _U_MAX; - if (q31_q_dc<-_U_MAX) q31_q_dc = -_U_MAX; // allow no negative voltage. + if (q31_out>PI_c->limit_output) q31_out = PI_c->limit_output; + if (q31_out<-PI_c->limit_output) q31_out = -PI_c->limit_output; // allow no negative voltage. - return (q31_q_dc); + return (q31_out); } //PI Control for direct current id (loss) diff --git a/Src/main.c b/Src/main.c index 74367e04..153345ff 100644 --- a/Src/main.c +++ b/Src/main.c @@ -207,6 +207,11 @@ uint8_t ui8_LEV_Page_to_send=1; MotorState_t MS; MotorParams_t MP; +//structs for PI_control +PI_control_t PI_iq; +PI_control_t PI_id; +PI_control_t PI_speed; + int16_t battery_percent_fromcapacity = 50; //Calculation of used watthours not implemented yet int16_t wheel_time = 1000; //duration of one wheel rotation for speed calculation @@ -298,6 +303,23 @@ int main(void) MP.pulses_per_revolution = PULSES_PER_REVOLUTION; MP.wheel_cirumference = WHEEL_CIRCUMFERENCE; + //init PI structs + PI_id.gain_i=I_FACTOR_I_D; + PI_id.gain_p=P_FACTOR_I_D; + PI_id.setpoint = 0; + PI_id.limit_output = _U_MAX; + PI_id.max_step=5; + PI_id.shift=10; + PI_id.limit_i=1800; + + PI_iq.gain_i=I_FACTOR_I_Q; + PI_iq.gain_p=P_FACTOR_I_Q; + PI_iq.setpoint = 0; + PI_iq.limit_output = _U_MAX; + PI_iq.max_step=5; + PI_iq.shift=10; + PI_iq.limit_i=_U_MAX; + //Virtual EEPROM init HAL_FLASH_Unlock(); EE_Init(); @@ -490,19 +512,24 @@ int main(void) //if if (!ui8_BC_limit_flag){ - q31_u_q_temp = PI_control_i_q(MS.i_q, (q31_t) i8_direction*i8_reverse_flag*int32_current_target); + PI_iq.recent_vaule = MS.i_q; + PI_iq.setpoint = i8_direction*i8_reverse_flag*int32_current_target; } else{ if(HAL_GPIO_ReadPin(Brake_GPIO_Port, Brake_Pin)){ - q31_u_q_temp = PI_control_i_q((MS.Battery_Current>>6)*i8_direction*i8_reverse_flag, (q31_t) (BATTERYCURRENT_MAX>>6)*i8_direction*i8_reverse_flag); - } + PI_iq.recent_vaule= (MS.Battery_Current>>6)*i8_direction*i8_reverse_flag; + PI_iq.setpoint = (BATTERYCURRENT_MAX>>6)*i8_direction*i8_reverse_flag; + } else{ - q31_u_q_temp = PI_control_i_q((MS.Battery_Current>>6)*i8_direction*i8_reverse_flag, (q31_t) (-REGEN_CURRENT_MAX>>6)*i8_direction*i8_reverse_flag); - } + PI_iq.recent_vaule= (MS.Battery_Current>>6)*i8_direction*i8_reverse_flag; + PI_iq.setpoint = (-BATTERYCURRENT_MAX>>6)*i8_direction*i8_reverse_flag; + } } + q31_u_q_temp = PI_control(&PI_iq); //Control id - q31_u_d_temp = -PI_control_i_d(MS.i_d, 0); //control direct current to zero + PI_id.recent_vaule = MS.i_d; + q31_u_d_temp = -PI_control(&PI_id); //control direct current to zero //limit voltage in rotating frame, refer chapter 4.10.1 of UM1052