środa, 9 maja 2018

[24] STM32F7 - Obsługa serwomechanizmu

W tym poście chciałbym opisać sposób obsługi serwomechanizmu za pomocą jednego z timerów skonfigurowanego w trybie PWM.

[Źródło: http://www.st.com/en/evaluation-tools/32f746gdiscovery.html]


Opis funkcji


Poniżej przejdę przez opis poszczególnych funkcji odpowiedzialnych za poszczególne etapy uruchomienia.

Dane dla danego timera są przechowywane w strukturze:

  1. typedef struct {
  2.   PWM_TIM_NAME_t ServoPinGenerateName;
  3.   TIM_TypeDef* selectedTim;
  4.   const TimerPwmChannel_Typedef ServoPwmChannel;
  5.   GPIO_TypeDef* ServoPwmPort;
  6.   const uint16_t ServoPwmPin;
  7.   float servoPosition;
  8.   float servoMicros;
  9. }ServoTimerPwmSetting_Typedef;

Zdefiniowane dane wyglądają następująco:

  1. ServoTimerPwmSetting_Typedef servoPwmTimer2[] = {
  2.   {PWM_CH1_TIM2_PA0, TIM2, 1, GPIOA, GPIO_PIN_0, 0, 0},
  3. };

Uruchomienie timera:

  1. static void servo_tim2_init(void)
  2. {
  3.     #if SERVO_TIMER_2_ENABLE == 1
  4.     /* Enable Pin */
  5.     GPIO_InitTypeDef GPIO_InitStruct;
  6.     uint8_t enableCount;
  7.  
  8.     for(enableCount=0; enableCount<SERVO_TIMER_2_EN_CHANEL_COUNT; enableCount++)
  9.     {
  10.         servo_EnableClock(servoPwmTimer2[enableCount].ServoPwmPort);
  11.  
  12.         GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
  13.         GPIO_InitStruct.Pull = GPIO_PULLUP;
  14.         GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
  15.         GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
  16.         GPIO_InitStruct.Pin = servoPwmTimer2[enableCount].ServoPwmPin;
  17.         HAL_GPIO_Init(servoPwmTimer2[enableCount].ServoPwmPort, &GPIO_InitStruct);
  18.     }
  19.  
  20.     /* Enable Pin For Timer */
  21.     TIM_HandleTypeDef TimHandle;
  22.     TIM_OC_InitTypeDef sConfig;
  23.  
  24.     __HAL_RCC_TIM2_CLK_ENABLE();
  25.  
  26.     TimHandle.Instance               = TIM2;
  27.     TimHandle.Init.Prescaler         = SERVO_TIM2_PRESCALE;
  28.     TimHandle.Init.Period            = SERVO_TIM2_PERIODE;
  29.     TimHandle.Init.ClockDivision     = TIM_CLOCKDIVISION_DIV1;
  30.     TimHandle.Init.CounterMode       = TIM_COUNTERMODE_UP;
  31.     TimHandle.Init.RepetitionCounter = 0;
  32.     HAL_TIM_PWM_Init(&TimHandle);
  33.  
  34.     for(enableCount=0; enableCount<SERVO_TIMER_2_EN_CHANEL_COUNT; enableCount++)
  35.     {
  36.         sConfig.OCMode       = TIM_OCMODE_PWM1;
  37.         sConfig.OCPolarity   = SERVO_TIM2_POLARITY;
  38.         sConfig.OCFastMode   = TIM_OCFAST_DISABLE;
  39.         sConfig.OCNPolarity  = TIM_OCNPOLARITY_HIGH;
  40.         sConfig.OCNIdleState = TIM_OCNIDLESTATE_RESET;
  41.         sConfig.OCIdleState  = TIM_OCIDLESTATE_RESET;
  42.         sConfig.Pulse = 0;
  43.  
  44.         if(servoPwmTimer2[enableCount].ServoPwmChannel==1)
  45.         {
  46.           HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, TIM_CHANNEL_1);
  47.           HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_1);
  48.         }
  49.         else if(servoPwmTimer2[enableCount].ServoPwmChannel==2)
  50.         {
  51.           HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, TIM_CHANNEL_2);
  52.           HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_2);
  53.         }
  54.         else if(servoPwmTimer2[enableCount].ServoPwmChannel==3)
  55.         {
  56.           HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, TIM_CHANNEL_3);
  57.           HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_3);
  58.         }
  59.         else if(servoPwmTimer2[enableCount].ServoPwmChannel==4)
  60.         {
  61.           HAL_TIM_PWM_ConfigChannel(&TimHandle, &sConfig, TIM_CHANNEL_4);
  62.           HAL_TIM_PWM_Start(&TimHandle, TIM_CHANNEL_4);
  63.         }
  64.     }
  65.     #endif
  66. }

Najpierw uruchamiany jest pin GPIO przypisany do danego kanału, następnie timera oraz PWM.

Ustawienie wartości PWM dla danego kanału oraz timera:

  1. static void servo_tim2_SetPWM(TimerPwmChannel_Typedef pwmChannel, uint16_t value)
  2. {
  3.     #if SERVO_TIMER_2_ENABLE == 1
  4.     if(value>SERVO_TIM2_PERIODE) { value=SERVO_TIM2_PERIODE; }
  5.  
  6.     if(pwmChannel==Servo_Channel_1) { TIM2->CCR1 = value; }
  7.     if(pwmChannel==Servo_Channel_2) { TIM2->CCR2 = value; }
  8.     if(pwmChannel==Servo_Channel_3) { TIM2->CCR3 = value; }
  9.     if(pwmChannel==Servo_Channel_4) { TIM2->CCR4 = value; }
  10.     #endif
  11. }

Główna funkcja uruchamiająca uruchamia timer oraz wprowadza wartość 0 dla generowanego PWM:

  1. void Servo_Enable(void)
  2. {
  3.     servo_enable_timer();
  4.     servo_setStartPWMValue();
  5. }

Ustawienie stopni przesunięcia:

  1. SetServoResult_Typedef Servo_SetRotDegrees(ServoTimerPwmSetting_Typedef* servoStr, float positToSet) {
  2.     if (positToSet < 0 || positToSet > 180) {
  3.         return SET_SERVO_ERROR;
  4.     }
  5.  
  6.     uint16_t microSeconds = 0;
  7.  
  8.     if(servoStr->selectedTim == TIM2)
  9.     {
  10.         microSeconds = (SERVO_MICSEK_MAX_TIM2_PULSE - SERVO_MICSEK_MIN_TIM2_PULSE) * positToSet / 180 + SERVO_MICSEK_MIN_TIM2_PULSE;
  11.     }
  12.     else if(servoStr->selectedTim == TIM3)
  13.     {
  14.         microSeconds = (SERVO_MICSEK_MAX_TIM3_PULSE - SERVO_MICSEK_MIN_TIM3_PULSE) * positToSet / 180 + SERVO_MICSEK_MIN_TIM3_PULSE;
  15.     }
  16.     else if(servoStr->selectedTim == TIM4)
  17.     {
  18.         microSeconds = (SERVO_MICSEK_MAX_TIM4_PULSE - SERVO_MICSEK_MIN_TIM4_PULSE) * positToSet / 180 + SERVO_MICSEK_MIN_TIM4_PULSE;
  19.     }
  20.     else if(servoStr->selectedTim == TIM5)
  21.     {
  22.         microSeconds = (SERVO_MICSEK_MAX_TIM5_PULSE - SERVO_MICSEK_MIN_TIM5_PULSE) * positToSet / 180 + SERVO_MICSEK_MIN_TIM5_PULSE;
  23.     }
  24.     else
  25.     {
  26.         return SET_SERVO_ERROR;
  27.     }
  28.  
  29.     /* Save current position into structures */
  30.     servoStr->servoMicros = microSeconds;
  31.     servoStr->servoPosition = positToSet;
  32.  
  33.     servo_SetPwm(servoStr, microSeconds);
  34.  
  35.     return SET_SERVO_OK;
  36. }

Na samym początku przerabiana jest wartość z stopni na mikrosekundy, po czym ustawiana jest wartość PWM.

  1. SetServoResult_Typedef Servo_SetPulseLengMicrosec(ServoTimerPwmSetting_Typedef* servoStr, uint16_t microSeconds)
  2. {
  3.     float setDegres = 0.0;
  4.  
  5.     if(servoStr->selectedTim == TIM2)
  6.     {
  7.         if (microSeconds < SERVO_MICSEK_MIN_TIM2_PULSE || microSeconds > SERVO_MICSEK_MAX_TIM2_PULSE) {
  8.             return SET_SERVO_ERROR;
  9.         }
  10.         setDegres = (float)(microSeconds - SERVO_MICSEK_MIN_TIM2_PULSE) * (float)180 / (float)(SERVO_MICSEK_MAX_TIM2_PULSE -SERVO_MICSEK_MIN_TIM2_PULSE);
  11.     }
  12.     else if(servoStr->selectedTim == TIM3)
  13.     {
  14.         if (microSeconds < SERVO_MICSEK_MIN_TIM3_PULSE || microSeconds > SERVO_MICSEK_MAX_TIM3_PULSE) {
  15.             return SET_SERVO_ERROR;
  16.         }
  17.         setDegres = (float)(microSeconds - SERVO_MICSEK_MIN_TIM3_PULSE) * (float)180 / (float)(SERVO_MICSEK_MAX_TIM3_PULSE -SERVO_MICSEK_MIN_TIM3_PULSE);
  18.     }
  19.     else if(servoStr->selectedTim == TIM4)
  20.     {
  21.         if (microSeconds < SERVO_MICSEK_MIN_TIM4_PULSE || microSeconds > SERVO_MICSEK_MAX_TIM4_PULSE) {
  22.             return SET_SERVO_ERROR;
  23.         }
  24.         setDegres = (float)(microSeconds - SERVO_MICSEK_MIN_TIM4_PULSE) * (float)180 / (float)(SERVO_MICSEK_MAX_TIM4_PULSE -SERVO_MICSEK_MIN_TIM4_PULSE);
  25.     }
  26.     else if(servoStr->selectedTim == TIM5)
  27.     {
  28.         if (microSeconds < SERVO_MICSEK_MIN_TIM5_PULSE || microSeconds > SERVO_MICSEK_MAX_TIM5_PULSE) {
  29.             return SET_SERVO_ERROR;
  30.         }
  31.         setDegres = (float)(microSeconds - SERVO_MICSEK_MIN_TIM5_PULSE) * (float)180 / (float)(SERVO_MICSEK_MAX_TIM5_PULSE -SERVO_MICSEK_MIN_TIM5_PULSE);
  32.     }
  33.     else
  34.     {
  35.         return SET_SERVO_ERROR;
  36.     }
  37.  
  38.     servoStr->servoMicros = microSeconds;
  39.     servoStr->servoPosition = setDegres;
  40.  
  41.     servo_SetPwm(servoStr, microSeconds);
  42.  
  43.     return SET_SERVO_OK;
  44. }

Funkcja podobna do poprzedniej z tą różnicą, że przerabiana jest wartość mikrosekund na stopnie.

Pozostałe funkcje można znaleźć w plikach na dysku Google pod tym linkiem.

Przebiegi z analizatora stanów logicznych:


TIM2:




TIM3:




TIM4:




TIM5:



Autor: o
Etykiety: ,