//This is called from TIM3 update interrupt. defined in input.c
void updatePid()
{
uint32_t abs_position_error;
int32_t position_delta;
int32_t position_delta_delta;
static uint8_t prevdir;
getEncoderCount();
if (!motor_running)
{
pid_integrated_error=0;
return;
}
position_error = encoder_count - pid_requested_position;
position_delta= pid_requested_position - pid_last_requested_position;
position_delta_delta = position_delta - pid_last_requested_position_delta;
pid_last_requested_position = pid_requested_position;
pid_last_requested_position_delta = position_delta;
if(abs(position_error)> max_error)
max_error = abs(position_error);
abs_position_error = abs(position_error);
if (abs_position_error > pid_max_pos_error)
{
pwm_motorStop();
ERROR_LED_ON;
return;
}
//P
int32_t output = position_error * s.pid_Kp;
//I
pid_integrated_error += position_error * s.pid_Ki;
if (pid_integrated_error > 400000)
pid_integrated_error = 400000;
if (pid_integrated_error < -400000)
pid_integrated_error = -400000;
output += pid_integrated_error;
//D
output += (position_error - pid_prev_position_error) * s.pid_Kd;
pid_prev_position_error = position_error;
//FF1
output += position_delta * s.pid_FF1;
//FF2
output += position_delta_delta * s.pid_FF2;
output /= 100; //provide larger dynamic range for pid. (without this, having pid_Ki = 1 was enough for oscillation.
//limit output power
if (output > MAX_DUTY)
output = MAX_DUTY;
if (output < -MAX_DUTY)
output = -MAX_DUTY;
if(output>0)
{
dir=0;
}
else
{
dir=1;
}
pwm_setDutyCycle(abs(output));
if(dir!=prevdir)
{
pwm_InitialBLDCCommutation();
prevdir=dir;
}
}
int
main()
{
motor_running=0;
updateCtr=0;
dir=1;
FLASH_Unlock();
getConfig();
FLASH_Lock();
initUSART(s.usart_baud);
printConfiguration();
initPWM();
initADC();
if( s.commutationMethod == commutationMethod_HALL)
{
initHALL();
}
if(s.inputMethod == inputMethod_stepDir)
{
initStepDirInput();
}
else if (s.inputMethod == inputMethod_pwmVelocity)
{
initPWMInput();
}
if(s.inputMethod == inputMethod_stepDir || s.commutationMethod == commutationMethod_Encoder)
{
initEncoder();
}
if(s.inputMethod == inputMethod_stepDir)
{
initPid();
}
initLeds();
// errorInCommutation=1;
uint8_t ena;
//check if ENA is on already at start. If it is, start motor.
#if ENA_POLARITY == 1
ena = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5);
#else
ena = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5)))&1;
#endif
if(ena)
{
pwm_motorStart();
ENABLE_LED_ON;
}
//two different types of main loops depending on commutation method
if(s.commutationMethod == commutationMethod_Encoder)
{
while (1)
{
getEncoderCount();
if(encoder_commutation_pos != encoder_commutation_table[encoder_shaft_pos])
{
//usart_sendStr("commutation to ");
//usart_sendChar(encoder_commutation_table[encoder_shaft_pos]+48);
encoder_commutation_pos = encoder_commutation_table[encoder_shaft_pos];
pwm_Commute(encoder_commutation_pos);
// usart_sendStr("\n\r");
}
}
}
else
{
while(1)
{
}
}
}
