//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) { } } }