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); } }