Skip to content

Commit

Permalink
PI control with struct
Browse files Browse the repository at this point in the history
  • Loading branch information
@stancecoke
stancecoke committed Feb 5, 2021
1 parent 92d95b0 commit 6da314c
Show file tree
Hide file tree
Showing 4 changed files with 114 additions and 73 deletions.
103 changes: 52 additions & 51 deletions Inc/FOC.h
View file
Original file line number Diff line number Diff line change
@@ -1,51 +1,52 @@
/*
* FOC.h
*
* Created on: 25.01.2019
* Author: stancecoke
*/

#ifndef FOC_H_
#define FOC_H_

#include <arm_math.h>
#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 <arm_math.h>
#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_ */
14 changes: 14 additions & 0 deletions Inc/main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
31 changes: 15 additions & 16 deletions Src/FOC.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<q31_q_dc-5)q31_q_dc-=5;
else q31_q_dc=(q31_p+q31_q_i)>>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)
Expand Down
39 changes: 33 additions & 6 deletions Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand Down

0 comments on commit 6da314c

Please sign in to comment.