void EXTI9_5_IRQHandler()
{
uint8_t ena,idir;
if(EXTI_GetITStatus(EXTI_Line5)!= RESET)
{
//ENA PIN INTERRUPT
#if ENA_POLARITY == 1
ena = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5);
#else
ena = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_5)))&1;
#endif
if(ena && !motor_running)
{
//enable rose. Start
pid_requested_position = encoder_count; //reset the desired position to current.
pwm_motorStart();
ENABLE_LED_ON;
ERROR_LED_OFF;
}
else if(!ena && motor_running)
{
//enable fell. Stop.
pwm_motorStop();
ENABLE_LED_OFF;
}
else if(!ena)
{
ENABLE_LED_OFF;
}
EXTI_ClearITPendingBit(EXTI_Line5);
}
if(EXTI_GetITStatus(EXTI_Line6)!= RESET)
{
//STEP PIN INTERRUPT
#if DIR_POLARITY == 1
idir = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);
#else
idir = (~(GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7)))&1;
#endif
if(idir)
{
pid_requested_position+=s.encoder_counts_per_step;
}
else
pid_requested_position-=s.encoder_counts_per_step;
EXTI_ClearITPendingBit(EXTI_Line6);
}
}
//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;
}
}
