엔코더 모터 제어 (2. 속도 계산)

https://kmong.com/gig/465716?selfMarketingCode=HskZcR53S1 

돈이 되는 인공지능의 시대 그 다음은 | 10000원부터 시작 가능한 총 평점 0점의 전자책, 교육 전자

관련글

---

엔코더 모터 제어 (0. 소개) (tistory.com)

엔코더 모터 제어 (1. 펄스및 위치 측정) (tistory.com)

엔코더 모터 제어 (3. PID 제어로 속도 제어 [PID 함수들 만들기 Part 01]) (tistory.com)

---

서론

---

엔코더 모터를 사용하는 이유 중 하나는 속도 측정을 하기 위함일 것입니다. 회전 속도를 제어하는 것은 로봇의 이동 방향, 행동을 정확하게 한다는 의미와도 같죠.

엔코더 모터의 회전 속도를 측정하는 방식은 크게 2가지 방식이 있습니다. M 방식과 T 방식이 있는데, 두 방식은 각각의 장단점이 일치하여 서로 같이 쓴다고도 합니다.

이번 포스트에서는 두 방식에 대한 간단한 설명과 속도 측정 공식, 그리고 M 방식을 사용했을 경우의 코드를 알아볼 예정입니다.

 

---

M 방식

T 방식

M 방식 속도 측정

   initEXTI0, 1

   initTIM2

   initRCC

   main

   EXTI0_IRQHandler

   EXTI1_IRQHandler

   TIM2_IRQHandler

최종 코드

시연 영상

---

참고 자료

---

 

STM32F103ZET6 Reference Manual

STM32F10xxx/20xxx/21xxx/L1xxxx Cortex®-M3 programming manual

RPM 계산 방법 M/T 방법(M/T Method) (tistory.com)

 

M 방식

---

M 방식은 쉽게 말하면 1초동안 몇 바퀴를 회전했는가로 RPM을 계산하는 방식입니다. 좀 더 일반적으로 말하자면 일정 시간(t) 동안 얼만큼의 신호가 들어 왔는가입니다.

M 방식의 장점은 고속에서 유리하다는 점입니다. 반대로 저속에서는 불리하죠. 

만약에 속도가 너무 느려서 2초마다 신호가 하나씩 들어온다고 가정합니다.

하지만 stm32에게 2초는 너무 느려서 1초마다씩만 속도 계산을 할 수 있다고 한다면,

처음 1초에는 신호가 들어 와서 RPM을 계산할 수 있지만, 

두 번째 1초에는 신호가 들어 오지 않아 RPM이 0으로 나올 것입니다.

실제로는 2초마다 하나씩 들어와서 RPM이 0이 아니지만요.

 

따라서 M 방식은 MCU 타이머의 한계로 저속에서는 불리한 방식이라고 할 수 있습니다.

 

PPR이라는 개념 기억 하시나요? Pulse Per Revolution의 약자로 엔코더를 한 바퀴 돌렸을 때 한 채널에서 발생시키는 신호(펄스)의 개수를 의미합니다. 제가 사용하고 있는 엔코더의 경우 11 PPR호 엔코더를 한 바퀴 돌렸을 때 한 채널에서 11개의 펄스가 발생하죠. A, B 두 채널이 있으니까 총 22개의 펄스가 발생하겠네요.

또한 전 포스트에서 언급한 체배의 개념도 알고 계셔야합니다.

 

M 방식으로 RPM을 구하는 공식은 다음과 같습니다.

RPM = (60 * edge) / (t * PPR * 체배)

여기서 60은 RPM은 minute 즉, 분 단위를 쓰고 시간 t는 second 초 단위를 사용하기 때문에 이를 변환하는 값입니다.

edge는 단위 시간 동안 들어온 신호의 개수 (rising / falling)입니다.

t는 단위 시간입니다.

PPR은 아실 거예요.

체배는 1체배 방식이면 1, 2체배 방식이면 2, 4면 4를 의미합니다.

 

저 공식을 일정 주기 t마다 실행시켜 RPM을 계산하면 되는 거죠.

 

 

T 방식

---

T 방식은 M 방식과는 다르게 신호가 들어온 시간 간격을 기준으로 RPM을 계산합니다.

쉽게 말하자면 첫 번째 신호가 들어 오고, 다음 신호가 몇 초 뒤에 들어 오는가를 기준으로 계산하는 거죠. 

T 방식은 저속에서 유리합니다. (설명은 참고 자료 사이트에 자세히 나와 있습니다.)

 

 

공식은 다음과 같습니다.

RPM = 60 / (PPR * 신호 사이의 시간 간격 * 체배)

 

 

 

 

M 방식 속도 측정

---

처음에 저는 T 방식으로 속도를 측정하려 했습니다. 어느정도 잘 측정 되다가 가끔씩 값이 튕기는 현상이 있어서 어쩔 수 없이 M 방식으로 방향을 바꾸게 되었습니다.

 

순서는 다음과 같습니다.

1. PG0와 PG1에 엔코더 A, B 채널을 연결합니다.

2. PG0와 PG1에 EXTI0, EXTI1을 연결한 후, 인터럽트를 활성화합니다.

3. TIM2를 10 Hz로 맞춰 0.1s 마다 속도 값을 계산합니다.

4. EXTI0, 1 Interrupt handler는 M 방식에서 사용할 pulse값을 카운트 합니다.

5. TIM2 interrupt handler는 pulse 값과 위의 공식을 이용하여 RPM을 계산합니다. 그후 pulse값을 다시 0으로 초기화 합니다.

 

 

initEXTI0, 1

M 방식에서는 들어오는 신호의 개수가 많을 수록 정확해지기 때문에 4체배 방식을 이용했습니다.

void initEXTI0(void) {
  RCC -> APB2ENR |= ((1 << IOPGEN) | (1 << AFIOEN));
  GPIOG -> CRL |= (4 << MODE0);
  AFIO -> EXTICR1 |= (6 << 0);
  
  EXTI -> RTSR |= (1 << 0);
  EXTI -> FTSR |= (1 << 0);
  EXTI -> IMR |= (1 << 0);
  
  NVIC_ISER0 |= (1 << 6);
}
void initEXTI1(void) {
  RCC -> APB2ENR |= ((1 << IOPGEN) | (1 << AFIOEN));
  GPIOG -> CRL |= (4 << MODE1);
  AFIO -> EXTICR1 |= (6 << 4);
  
  EXTI -> RTSR |= (1 << 1);
  EXTI -> FTSR |= (1 << 1);
  EXTI -> IMR |= (1 << 1);
  
  NVIC_ISER0 |= (1 << 7);
}

RTSR, FTSR 둘 모두를 설정하여 Rising, Falling에서 Handler가 동작되도록 설정했습니다.

 

 

initTIM2

void initTIM2(void) {
  RCC -> APB1ENR |= (1 << 0);
  
  TIM2 -> PSC = 30000 - 1; // 4500 8,000Hz
  TIM2 -> ARR = 120;  // 1 1.25us, 8,000Hz
  TIM2 -> CNT = 0;
  TIM2 -> SR = 0;
  
  TIM2 -> DIER = (1 << 0);
  NVIC_ISER0 |= (1 << 28);
  
  TIM2 -> EGR |= (1 << 0);
  TIM2 -> CR1 |= (1 << 0);
}

TIM2에서는 PSC와 ARR 값을 적절히 조절하여 10 Hz마다 Interrupt가 발생되도록 설정했습니다.

 

 

initRCC

void initRCC(void) {
  RCC -> CFGR &= ~(7 << MCO);    // PLL clock / 2 is selected as MCO
  RCC -> CFGR |= (1 << USBPRE); // set USB prescaler to 1
  RCC -> CFGR |= (7 << PLLMUL);       // PLL x9
  RCC -> CFGR |= (1 << PLLXTPRE);      // HSE divided
  RCC -> CFGR |= (1 << PLLSRC); //HSE seleted as PLL input clock
  RCC -> CFGR |= (1 << ADCPRE); // prescaler to 4
  RCC -> CFGR &= ~(7 << PPRE1); // no prescaler
  RCC -> CFGR &= ~(7 << PPRE2); // no prescaler
	
  RCC -> CR |= (1 << HSEON);    // enable HSE
  while(!(RCC -> CR & (1 << HSERDY)));   // waiting for HSE ready

  RCC -> CR |= (1 << PLLON);
  while(!(RCC -> CR & (1 << PLLRDY)));  // waiting for PLL ready

  RCC -> CFGR |= (2 << SW);  
  while(((RCC -> CFGR >> SWS) & 0x03) != (2 << SW));
}

CFGR에 적절한 값들을 넣어 TIM2가 연결된 APB1 라인에 36 MHz가 공급하게 하여 TIM2에 36 MHz를 공급했습니다.

그 다음으로 CR을 설정하고, 기다리고를 반복하여 안전하게 RCC가 동작되도록 했습니다.

 

 

 

main

int main(void) {
	initRCC();
    initEXTI0();
    initEXTI1();
    initTIM2();
    
    while(1) {
    	// RPM을 출력할 수 있는 적절한 함수
	}  
}
   

 

 

EXTI0_IRQHandler

void EXTI0_IRQHandler(void) {
  EXTI -> PR |= (1 << 0);
  encPulseRPMCnt++;	// 엔코더 펄스가 얼만큼 들어 왔는지는 측정하기 위한 변수
  
  /*
  	회전 방향을 볼려고 한 것이 아니기 때문에 다른 계산은 없습니다.
  */
  
  
};

 

EXTI1_IRQHandler

void EXTI1_IRQHandler(void) {
  EXTI -> PR |= (1 << 1);
  encPulseRPMCnt++;	// 엔코더 펄스가 얼만큼 들어 왔는지는 측정하기 위한 변수
  
  /*
  	회전 방향을 볼려고 한 것이 아니기 때문에 다른 계산은 없습니다.
  */
  
  
};

 

 

TIM2_IRQHandler

void TIM2_IRQHandler(void) {
  TIM2 -> SR = 0;
  encRPM = 10 * encPulseRPMCnt / 22;	// 위의 공식에 기반한 식
  encPulseRPMCnt = 0;
};

위의 공식에 따르면 M 방식을 이용할 경우

RPM = (60 * edge) / (t * PPR * 체배) 입니다.

여기서 edge는 encPulseRPMCnt라는 변수로 넣었습니다.

t는 TIM2 자체가 0.1s 주기로 동작하기 때문에 0.1을 넣었습니다.

PPR은 30 * 11이죠

11은 엔코더 자체의 PPR이고

30은 모터가 한 바퀴 돌 때 엔코더가 30 바퀴 돌기 때문입니다. (흔히 엔코더 모터의 ratio라고 하죠)

즉 모터가 한 바퀴 돌 때 엔코더 펄스는 330개가 발생하기 때문에 PPR에 30 * 11을 넣습니다.

저는 4체배 방식을 이용하기 때문에 체배에는 4를 넣고요.

 

따라서 식은 다음과 같이 쓸 수 있습니다.

RPM = (60 * encPulseRPMCnt) / (0.1 * (11 * 30) * 4)

이를 정리하면 10 * encPulseRPMCnt / 22가 나옵니다.

 

 

 

최종 코드

---

#include "gpio.h"
#include "exti.h"
#include "rcc.h"
#include "afio.h"
#include "tim.h"
#include "nvic.h"

// ===============================================================

volatile unsigned int encPulseRPMCnt = 0;

// ===============================================================

void EXTI0_IRQHandler(void) {
  EXTI -> PR |= (1 << 0);
  encPulseRPMCnt++;
};

void EXTI1_IRQHandler(void) {
  EXTI -> PR |= (1 << 1);
  encPulseRPMCnt++;
};

void TIM2_IRQHandler(void) {
  TIM2 -> SR = 0;
  encRPM = 10 * encPulseRPMCnt / 22;
  encPulseRPMCnt = 0;
};

// ===============================================================








int main(void) {
  initRCC();
  initEXTI0();
  initEXTI1();
  initTIM2();
    
  while(1) {
    // RPM을 출력할 수 있는 적절한 함수
  }  
}








// ===============================================================

void initEXTI0(void) {
  RCC -> APB2ENR |= ((1 << IOPGEN) | (1 << AFIOEN));
  GPIOG -> CRL |= (4 << MODE0);
  AFIO -> EXTICR1 |= (6 << 0);
  
  EXTI -> RTSR |= (1 << 0);
  EXTI -> FTSR |= (1 << 0);
  EXTI -> IMR |= (1 << 0);
  
  NVIC_ISER0 |= (1 << 6);
}

void initEXTI1(void) {
  RCC -> APB2ENR |= ((1 << IOPGEN) | (1 << AFIOEN));
  GPIOG -> CRL |= (4 << MODE1);
  AFIO -> EXTICR1 |= (6 << 4);
  
  EXTI -> RTSR |= (1 << 1);
  EXTI -> FTSR |= (1 << 1);
  EXTI -> IMR |= (1 << 1);
  
  NVIC_ISER0 |= (1 << 7);
}

void initTIM2(void) {
  RCC -> APB1ENR |= (1 << 0);
  
  TIM2 -> PSC = 30000 - 1; // 4500 8,000Hz
  TIM2 -> ARR = 120;  // 1 1.25us, 8,000Hz
  TIM2 -> CNT = 0;
  TIM2 -> SR = 0;
  
  TIM2 -> DIER = (1 << 0);
  NVIC_ISER0 |= (1 << 28);
  
  TIM2 -> EGR |= (1 << 0);
  TIM2 -> CR1 |= (1 << 0);
}

void initRCC(void) {
  RCC -> CFGR &= ~(7 << MCO);    // PLL clock / 2 is selected as MCO
  RCC -> CFGR |= (1 << USBPRE); // set USB prescaler to 1
  RCC -> CFGR |= (7 << PLLMUL);       // PLL x9
  RCC -> CFGR |= (1 << PLLXTPRE);      // HSE divided
  RCC -> CFGR |= (1 << PLLSRC); //HSE seleted as PLL input clock
  RCC -> CFGR |= (1 << ADCPRE); // prescaler to 4
  RCC -> CFGR &= ~(7 << PPRE1); // no prescaler
  RCC -> CFGR &= ~(7 << PPRE2); // no prescaler
	
  RCC -> CR |= (1 << HSEON);    // enable HSE
  while(!(RCC -> CR & (1 << HSERDY)));   // waiting for HSE ready

  RCC -> CR |= (1 << PLLON);
  while(!(RCC -> CR & (1 << PLLRDY)));  // waiting for PLL ready

  RCC -> CFGR |= (2 << SW);  
  while(((RCC -> CFGR >> SWS) & 0x03) != (2 << SW));
}
   

 

 

시연 영상

---

저는 USART1을 이용하여 RPM 값을 컴퓨터 화면에 띄웠습니다.

 

 

---

 

Continue

---

 

 

 

https://kmong.com/gig/465716?selfMarketingCode=HskZcR53S1 

돈이 되는 인공지능의 시대 그 다음은 | 10000원부터 시작 가능한 총 평점 0점의 전자책, 교육 전자