Below is the main() function in our code that calls initialization functions (not shown) and implements our closed-loop control loop. For all the details on our pwm and i2c initializations, you are welcome to download the main.c and main.h files:

main_brushlesspositioncontrol.zip
File Size:	11 kb
File Type:	zip

Download File

---

---

int main(void)
{
  RCC_ClocksTypeDef RCC_Clocks;
  
  // Initialize I2C communication and PWM ---------------------------
 
  init_I2C1();
  init_USART6(9600); // initialize USART6 @ 9600 baud
  USART_puts(USART6, "USART Initialized!\n");
  
  // SysTick end of count event each 10ms 
  RCC_GetClocksFreq(&RCC_Clocks);
  SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
  
  InitPWM(); //For details see the zipped file on this webpage
  
  // Zero the encoder --------------------------------------------------------
  
  //set the location to "zero"
  double sinVar1 = sin( 0 - (2*3.14/3) );
  double sinVar2 = sin( 0 );
  double sinVar3 = sin( 0 + (2*3.14/3) );
  
  CCR1_Val = (AMP) * sinVar1  + AMP;
  CCR2_Val = (AMP) * sinVar2  + AMP;
  CCR3_Val = (AMP) * sinVar3  + AMP;
  
  TIM_SetCompare1(TIM1, CCR1_Val);   
  TIM_SetCompare2(TIM1, CCR2_Val);  
  TIM_SetCompare3(TIM1, CCR3_Val);
  
  int16_t encoderZero;

  for( int i = 0; i < 2; i++) {
    Delay(100);
    I2C_run();
    
    //record what the encoder says when we are at "zero"
    encoderZero = (int16_t)encoderCount; 
    //should be about 1445, 3560, 5560, or 7650
  }
  
  //Print result to serial
  USART_puts(USART6, "\r\n"); //carrage return
  sprintf(output_buffer, "%u", encoderZero);
  USART_puts(USART6, output_buffer);
  USART_puts(USART6, "\r\n Calibrated "); //carrage return
  
  // Control Loop ------------------------------------------------------------
  
  while(1) {
    
    //Change location based on calibration offset
    int16_t calEncode = (int16_t)encoderCount - encoderZero;
    if (calEncode < 0) {
      calEncode = calEncode + ENCTICK;
    }
    
    //Calculate the error in position
    posErr = posDes - calEncode;
    
    //Integrated error term
    intPosErr = intPosErr + posErr;
    if(intPosErr > AMP) intPosErr = AMP; //anti-windup
    if(intPosErr < -AMP) intPosErr = -AMP; //anti-windup
    
    //Control Law for driving amplitude (PI only)
    int32_t amplitude = (12*posErr + 4*intPosErr)/100; //control law
    
    if (amplitude > AMP ) {
        amplitude = AMP; //limit current to steady state allowable amount
    }
    if (amplitude < -AMP ) {
        amplitude = -AMP; //limit current to steady state allowable amount
    }
    
    //Change from incoder ticks to radians and add 90 degree electrical offset
    positionVar = calEncode*3.14/ENCTICK*8 + 3.14/2;
    
    //Calculate the sine of the angle for each motor phase
    sinVar1 = sin( positionVar - (2*3.14/3) );
    sinVar2 = sin( positionVar );
    sinVar3 = sin( positionVar + (2*3.14/3) );
    
    //Calculate the input for the output compare pwm level
    CCR1_Val = (int16_t)( (double)amplitude * sinVar1 )  + abs(amplitude);
    CCR2_Val = (int16_t)( (double)amplitude * sinVar2 )  + abs(amplitude);
    CCR3_Val = (int16_t)( (double)amplitude * sinVar3 )  + abs(amplitude);
    
    //Change the output compare flag
    TIM_SetCompare1(TIM1, CCR1_Val);   
    TIM_SetCompare2(TIM1, CCR2_Val);  
    TIM_SetCompare3(TIM1, CCR3_Val);  
    TIM_SetCompare4(TIM1, CCR4_Val);  
    
    //measrue encoder location again
    I2C_run();
    
    //serial printing for debugging ------------------------------------------
    USART_puts(USART6, "\r\n"); //carrage return
    sprintf(output_buffer, "%i", calEncode);
    USART_puts(USART6, output_buffer); //carrage return    
  }
}