void pwm_motorStart()
{
motor_running=1;
pwm_InitialBLDCCommutation();
}
//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;
}
}
void TIM3_IRQHandler(void) {
if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
if(s.inputMethod==inputMethod_stepDir)
{
updatePid();
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
else
{
updateCtr++;
if(updateCtr>50)
{
//pwm_motorStop();
//ERROR_LED_ON;
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
uint16_t tim3_dc = TIM3->CCR2;
uint16_t tim3_per = TIM3->CCR1;
static uint8_t prevdir;
uint16_t DC;
#if DIR_POLARITY == 1
dir = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
#else
dir = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7)))&1;
#endif
if(dir!=prevdir)
{
pwm_InitialBLDCCommutation();
}
prevdir=dir;
if(tim3_per>0 && tim3_per>tim3_dc)
{
DC = ((uint32_t)(BLDC_CHOPPER_PERIOD*tim3_dc)/(uint32_t)tim3_per);
}
else
DC=0;
if(DC<BLDC_NOL*3) DC=BLDC_NOL*3;
pwm_setDutyCycle(DC);
updateCtr=0;
/* if(motor_running==0)
{
motor_running=1;
//BLDCMotorPrepareCommutation();
}*/
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
}
else if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)
{
//if PWM signal is lost for some reason, stop motor
TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
}
}
