STM32f103 Enkoder, stabilizator prędkości

[Isilio]

Witam, mam 2 silniczki pololu z enkoderami magnetycznymi link poniżej i potrzebuję do mojego projektu aby uzyskać stałą prędkość niezależnie od obciążenia czy naładowania akumulatora. Silniki sterowane PWM. Mam taki program odczytuję tylko sygnał z enkodera jako PWM, ale wyniki trochę od siebie odbiegają niby przy tej samej prędkości nawet po przepuszczeniu przez inverter. Wiem ze stm posiada Encoder Interface Mode czy on lepiej naddaje się do odczytu sygnału z enkodera? Jak napisać stabilizator prędkości?

Silnik DC z enkoderem

void TIM1_CC_IRQHandler(void){

TIM_ClearITPendingBit(TIM1,TIM_IT_CC1);
IC1 = TIM_GetCapture1(TIM1);

if(IC1 != 0)
{
	wsp_wyp = (TIM_GetCapture2(TIM1)*100) / IC1;
	czestotliwosc = 72000000 / IC1;
//	xxx = (wsp_wyp / 100) * czestotliwosc;

	if (IC1 == 958){
		GPIO_ResetBits(GPIOA, GPIO_Pin_6);
		TIM_Cmd(TIM3, DISABLE);

	}
}
else
{
	wsp_wyp = 0;
	czestotliwosc = 0;
}
}




int main (void){

RCC_conf();
NVIC_conf();
UART_conf();
TIME_conf();
GPIO_conf();

  	while (1)
  		{

  		printf("f = %.3f\n", czestotliwosc);

  		printf("wspolczynnik = %.3f\n", wsp_wyp);

  		delay(400);
  		}

}

void TIME_conf (void){
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	  TIM_OCInitTypeDef  TIM_OCInitStructure;
	  TIM_ICInitTypeDef  TIM_ICInitStructure;

	  //conf TIM3 do generowania PWM
	  TIM_TimeBaseStructure.TIM_Period = 3200;
	  TIM_TimeBaseStructure.TIM_Prescaler = 0;
	  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

	  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
	  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
	  TIM_OCInitStructure.TIM_Pulse = 1400;
	  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
	  TIM_OC1Init(TIM3, &TIM_OCInitStructure); 						// PA6 PWM out
	  TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
	  TIM_ARRPreloadConfig(TIM3, ENABLE);

	  TIM_Cmd(TIM3, ENABLE);

	  //conf TIM1 do pomiaru PWM
	  	TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;				// PA8 PWM read
	  	TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
	  	TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
	  	TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
	  	TIM_ICInitStructure.TIM_ICFilter = 0x0;


	  	TIM_PWMIConfig(TIM1, &TIM_ICInitStructure);
	  	TIM_SelectInputTrigger(TIM1, TIM_TS_TI1FP1);
	  	TIM_SelectSlaveMode(TIM1, TIM_SlaveMode_Reset);
	  	TIM_SelectMasterSlaveMode(TIM1, TIM_MasterSlaveMode_Enable);

	  	TIM_Cmd(TIM1, ENABLE);


	  	TIM_ITConfig(TIM1, TIM_IT_CC2, ENABLE);
}

void NVIC_conf (void){

	NVIC_InitTypeDef NVIC_InitStructure;

	NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
}

[Wojcik98]

Cześć,
na tej stronie z silniczkiem masz podany przykładowy kod, niby na Arduino, ale ogólny algorytm powinieneś ogarnąć.

Po pierwsze, sygnał z enkoderów nie jest w postaci PWM, tylko wyjść kwadraturowych. Nie jestem pewny, czy Twój program mierzy wypełnienie PWM, czy częstotliwość częstotliwość sygnału wejściowego, ale oba podejścia są złe. Sygnał kwadraturowy przy stałej prędkości będzie miał stałe "wypełnienie" 50%, a częstotliwość można uzyskać bardzo dużą drgając kołem tak, żeby czujnik magnetyczny ciągle zmieniał sygnał, a koło będzie praktycznie stać w miejscu. Żeby mierzyć prawidłowo prędkość musisz skorzystać z obu wyjść enkodera i musisz liczyć zarówno częstotliwość sygnału, jak i przesunięcie jednego sygnału względem drugiego. Jak dostajesz impulsy na zmianę, to wszystko spoko, jak dostajesz dwa razy impuls z jednego wejścia, to zmienił się kierunek obrotu. Jeśli stm ma sprzętowe wsparcie enkoderow, to lepiej go używać, ale dla ćwiczenia możesz samemu napisać obsługę.

W tym linku masz wytłumaczoną nieco obsługę enkoderow (na stmf4, ale nie powinno się rożnić bardzo):

http://www.micromouseonline.com/2013/02/16/quadrature-encoders-with-the-stm32f4/

Co do stabilizatora prędkości, to poczytaj o regulatorze PID. W skrocie: odpalasz regulator co jakiś ustalony czas (np. 10ms) i mierzysz, ile dostałeś impulsow z enkodera w tym czasie (prędkość rzeczywista), porownujesz z docelową prędkością i odpowiednio zmieniasz PWM na silniku.

[Isilio]

Okej mam teraz coś w tym stylu (kod poniżej), enc_cnt liczy mi impulsy z enkodera rozdzielczość mojego enkodera to 16imp\obr po przekładni 120:1 to 1920imp\obr. Po około tych 1920imp\obr silnik rzeczywiście robi jeden pełen obrót.

Czy idę w dobrym kierunku? Teraz mam napisać funkcję jak powiedział wcześniej Wojcik98 zliczającą liczbę tych impulsów przez np. 10ms aby uzyskać prędkość rzeczywistą? Pomoże ktoś napisać tą funkcję?

Następnie mam zająć się regulatorem PID uzależnić jakoś prędkość rzeczywistą od sygnału PWM sterującego silnikiem? Ma ktoś jakiś pomysł albo materiały?

void EXTI9_5_IRQHandler(void)
{
   if (EXTI_GetITStatus(EXTI_Line6) | EXTI_GetITStatus(EXTI_Line5))
   {
       rotary_encoder_update();

       // Clear interrupt flag
       EXTI_ClearITPendingBit(EXTI_Line6);
       EXTI_ClearITPendingBit(EXTI_Line5);
   }
}

int main(void)
{
UART_conf ();
PWMIN ();
       init_rotary_encoder();



   while (1)
   {

       printf("enc = %i\n", enc_cnt);
       printf("dist = %.3f cm\n", dist);

   	    delay(150);
   }
}



void init_rotary_encoder()
{
   GPIO_InitTypeDef GPIO_InitStruct;
   EXTI_InitTypeDef EXTI_InitStruct;
   NVIC_InitTypeDef NVIC_InitStruct;

   // Step 1: Initialize GPIO as input for rotary encoder
   // PB6 (encoder pin A), PB5 (encoder pin B)
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
   GPIO_InitStruct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_5;
   GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING;
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
   GPIO_Init(GPIOB, &GPIO_InitStruct);

   // Step 2: Initialize EXTI for PB6 and PB5
   RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
   GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource6);
   GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource5);
   EXTI_InitStruct.EXTI_Line = EXTI_Line6 | EXTI_Line5;
   EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
   EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
   EXTI_InitStruct.EXTI_LineCmd = ENABLE;
   EXTI_Init(&EXTI_InitStruct);

   // Step 3: Initialize NVIC for EXTI9_5 IRQ channel
   NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn;
   NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x00;
   NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00;
   NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
   NVIC_Init(&NVIC_InitStruct);
}


void rotary_encoder_update()
{
   uint8_t enc_val_a, enc_val_b;
   uint8_t enc_inc, enc_dec;

   // Read pin A and pin B every rising and falling edge interrupt
   // from both pin (4x resolution)
   enc_val_a = (uint8_t) ((GPIOB->IDR & GPIO_Pin_6) >> 6);
   enc_val_b = (uint8_t) ((GPIOB->IDR & GPIO_Pin_5) >> 5);

   // Read encoder direction using xor logic
   enc_inc = enc_val_a ^ last_enc_val_b;
   enc_dec = enc_val_b ^ last_enc_val_a;

   // Decrement or increment counter
   if(enc_inc)
   {
       enc_cnt++;
   }
   if(enc_dec)
   {
       enc_cnt--;
   }

   // Store encoder value for next reading
   last_enc_val_a = enc_val_a;
   last_enc_val_b = enc_val_b;

   dist = (enc_cnt / 1920) * 20.735;

}