iFBalance

iFBalance-코딩으로 뽀샤버리기

5강. DC 모터 바퀴의 홀센서 엔코더 제어하기

Posted by INDIFROG on 2021-03-05
Words 1.1k and Reading Time 6 Minutes
Viewed Times

5강. DC 모터 바퀴의 홀센서 엔코더 제어하기

아래 그림은 iFBalance 로봇에서 사용된 DC 모터의 샤프트 축에 부착된 마그네틱 홀센서로 구성된 엔코더의 모습입니다.

( 구매는 (주)인디프로그 쇼핑몰에서 가능합니다. : https://www.indifrog.co.kr/product/detail.html?product_no=100&cate_no=1&display_group=3 )

VSCODE_LIB

iFBalance에서 사용중인 기어모터는 일반적으로 컨텍터의 연결 방향이 위쪽 방향으로 되어 있다보니 바퀴가 바닥면과 닫는 부위의 높이가 높지않아서 컨넥터가 닿아 버리기 때문에 옆방향으로 컨넥터가 나오도록 실제로 특수 제작한 버전입니다. 이외에도 모터와 기어비 그리고 토크와의 상관관계 때문에 가능한 6V에서 최대의 스피드와 토크가 나오도록 주문제작한 버전입니다.

VSCODE_LIB

기존 iFLine에서 바퀴의 탭에 적용된 IR 엔코더 보다 모터의 샤프트 축에 바로 연동이 되어 회전하기 때문에 엔코더의 해상도가 최소 모터의 회전수 만큼 높다고 볼 수 있습니다.

차후 기어비 및 엔코더의 성능에 대해서 분석할 때 보다 자세히 다루도록 하겠습니다. DC 모터를 제어하기 위해 할당된 GPIO는 다음과 같다.

  Motor Control         GPIO

  LEFT_A                IO25
  LEFT_B                IO14
  RIGHT_A               IO13
  RIGHT_B               IO15

VSCODE_LIB

DC 모터에 대한 iFLine IR 엔코더 제어등에 관한 내용은 기존 잘 설명이 되어 있는 iFLine 강좌를 먼저 참조하시기 바랍니다.

2020-07-01-iFLine-개발강좌_09 ➾

iFBalance는 IR센서를 이용한 엔코더 대신 마그네틱 홀센서가 사용되므로 별도의 IR센서에 대한 튜닝이 필요하지 않을 뿐만 아니라 보다 빠르고 정교합니다. 다만, IREncoder와 동일한 이론이 적용됩니다.

VSCODE_LIB

소스코드에서 attachInterrupt 함수에서 CHANGE 를 사용하는데 이것은 마그네틱 홀센서의 펄스가 Rising이나 Falling 둘 다의 경우에 인터럽트가 발생하도록 합니다. 인터럽트가 발생하게 되면 인터럽트 핸들러(함수)가 실행되는데 이 때, 해당 A, B 펄스값을 digitalRead 함수로 읽어서 계산합니다. 따라서 왼쪽 모터, 오른쪽 모터 각각의 A, B 라인으로 부터 인터럽트를 발생하기 위해 위에서 설명한 GPIO에 연결이 되어 있습니다.

​

왼쪽 모터의 A, B라인에 대해서 예를 들어 설명을 해 보면, 하기 방정식과 같은 알고리즘으로 왼쪽 모터의 엔코더로 부터 출력되는 A, B 값으로 iFBalance의 방향을 계산할 수 있습니다.

countLeft += (lastLeftA ^ newLeftB) - (newLeftA ^ lastLeftB);
위 코드 공식을 적용해 보면 다음 표와 같이 계산이 되고 방향을 알 수 있게 된다.

VSCODE_LIB

또한 countLeft 변수에 누적해서 계산이 되므로 해당 엔코더로 부터 바퀴의 회전수를 알 수 있습니다.

​

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
/******************************************************************************
iFEncoder.h - iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#ifndef iFLineEncoder_h
#define iFLineEncoder_h

#include <Arduino.h> 

class iFEncoder
{
    private:
        static int8_t m_nLeA, m_nLeB, m_nReA, m_nReB;

        static volatile bool lastLeftA;
        static volatile bool lastLeftB;
        static volatile bool lastRightA;
        static volatile bool lastRightB;

        static volatile bool errorLeft;
        static volatile bool errorRight;

        static volatile int32_t countRight, lastCountR;
        static volatile int32_t countLeft, lastCountL;

        static int32_t prevSpeed;

    private:
        static void IRAM_ATTR leftISR();
        static void IRAM_ATTR rightISR();

    public:  
        static portMUX_TYPE muxl;
        static portMUX_TYPE muxr;

    public:  
        iFEncoder(){};
        void init(int8_t leA, int8_t leB, int8_t reA, int8_t reB);
        int32_t getCountLeft();
        int32_t getCountRight();

        static  int32_t getCountLeftReset();
        static  int32_t getCountRightReset();
        
        void resetCountLeft();
        void resetCountRight();
        void resetCounts();
    
        static void taskSpeedNotification(void *pvParameters);

};
#endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
/******************************************************************************
iFEncoder.cpp - iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#if ARDUINO >= 100
  #include "Arduino.h"
#else
  #include "WProgram.h"
#endif

#include "iFEncoder.h"

int8_t iFEncoder::m_nLeA = GPIO_NUM_13;
int8_t iFEncoder::m_nLeB = GPIO_NUM_15;
int8_t iFEncoder::m_nReA = GPIO_NUM_25;
int8_t iFEncoder::m_nReB = GPIO_NUM_14;

volatile bool iFEncoder::lastLeftA = false;
volatile bool iFEncoder::lastLeftB = false;
volatile bool iFEncoder::lastRightA = false;
volatile bool iFEncoder::lastRightB = false;

volatile bool iFEncoder::errorLeft = false;
volatile bool iFEncoder::errorRight = false;

volatile int32_t iFEncoder::countRight = 0;
volatile int32_t iFEncoder::lastCountR = 0;
volatile int32_t iFEncoder::countLeft = 0;
volatile int32_t iFEncoder::lastCountL = 0;

portMUX_TYPE iFEncoder::muxl = portMUX_INITIALIZER_UNLOCKED;
portMUX_TYPE iFEncoder::muxr = portMUX_INITIALIZER_UNLOCKED;

int32_t iFEncoder::prevSpeed = 0;

void IRAM_ATTR iFEncoder::leftISR()
{
    bool newLeftB, newLeftA;

    portENTER_CRITICAL(&muxl);
    newLeftB = digitalRead(m_nLeB);
    newLeftA = digitalRead(m_nLeA);// ^ lastLeftB;

    countLeft += (lastLeftA ^ newLeftB) - (newLeftA ^ lastLeftB);

    if ((lastLeftA ^ newLeftA) & (lastLeftB ^ newLeftB)) {
        errorLeft = true;
    }

    lastLeftA = newLeftA;
    lastLeftB = newLeftB;

    portEXIT_CRITICAL(&muxl);
}

void IRAM_ATTR iFEncoder::rightISR()
{
    bool newRightB, newRightA;

    portENTER_CRITICAL(&muxr);
    newRightB = digitalRead(m_nReB);
    newRightA = digitalRead(m_nReA);// ^ lastRightB;

    countRight +=  (newRightA ^ lastRightB) -(lastRightA ^ newRightB);
  //  countRight += (lastRightA ^ newRightB) - (newRightA ^ lastRightB);
    if ((lastRightA ^ newRightA) & (lastRightB ^ newRightB)) {
        errorRight = true;
    }
    lastRightA = newRightA;
    lastRightB = newRightB;

    portEXIT_CRITICAL(&muxr);
}

void iFEncoder::taskSpeedNotification(void *pvParameters)
{
  uint32_t speed;
  for (;;)
  {  
    speed = abs(getCountLeftReset()) + abs(getCountRightReset()) / 2;
    Serial.print("speed: ");
    Serial.println(String(speed));
    prevSpeed = speed;
    delay(1000);
  }
}


void iFEncoder::init(int8_t leA, int8_t leB, int8_t reA, int8_t reB)
{
    m_nLeA = leA;
    m_nLeB = leB;
    m_nReA = reA;
    m_nReB = reB;

    pinMode(m_nLeA, INPUT_PULLDOWN);
    pinMode(m_nLeB, INPUT_PULLDOWN);
    pinMode(m_nReA, INPUT_PULLDOWN);
    pinMode(m_nReB, INPUT_PULLDOWN);

    attachInterrupt(m_nLeA, leftISR, CHANGE);
    attachInterrupt(m_nLeB, leftISR, CHANGE);
    attachInterrupt(m_nReA, rightISR, CHANGE);
    attachInterrupt(m_nReB, rightISR, CHANGE);

    xTaskCreatePinnedToCore( taskSpeedNotification, "taskSpeedNotification", 
            2048, 
            NULL, 
            uxTaskPriorityGet(NULL), 
            NULL, 
            0);
}

int32_t iFEncoder::getCountLeft() {
    int32_t c = countLeft;
    return c;
}

int32_t iFEncoder::getCountRight() {
    int32_t c = countRight;
    return c;
}

void iFEncoder::resetCountLeft(){
  portENTER_CRITICAL(&muxl);
    countLeft = 0;
  portEXIT_CRITICAL(&muxl);
}
void iFEncoder::resetCountRight(){
  portENTER_CRITICAL(&muxr);
    countRight = 0;
  portEXIT_CRITICAL(&muxr);
}

int32_t iFEncoder::getCountLeftReset(){
  portENTER_CRITICAL(&muxl);
    int32_t c = countLeft;
    countLeft = 0;
  portEXIT_CRITICAL(&muxl);
  return c;
}

int32_t iFEncoder::getCountRightReset(){
  portENTER_CRITICAL(&muxr);
    int32_t c = countRight;  
    countRight = 0;
  portEXIT_CRITICAL(&muxr);
  return c;  
}

void iFEncoder::resetCounts(){
    resetCountLeft();
    resetCountRight();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
/******************************************************************************
main.cpp - iFLine, iFBalance Arduino Library 
hkkim@koreaits.com ( KITS Lab )
original creation date: April 3, 2020

Development environment specifics:
Arduino IDE 1.8.x
iFBalance
******************************************************************************/

#include <Arduino.h>
#include "iFLED.h"
#include "iFMotors.h"
#include "iFEncoder.h"

iFLED         ifLed;
iFMotors      ifMotors;
iFEncoder     ifEncoder;

// Encoder Count
int32_t      _nCurLeft = 0, _nCurRight = 0;

void getEncoderSpeed(){     
    portENTER_CRITICAL(&ifEncoder.muxr);  
    _nCurLeft = ifEncoder.getCountLeftReset();          
    _nCurRight = ifEncoder.getCountRightReset();          
    portEXIT_CRITICAL(&ifEncoder.muxr);
}

void setup()
{ 
  Serial.begin( 921600 );

  ///////////////////////////////////////////////////////
  // Color LED
  ifLed.init();  
  ifLed.setMode( BLINK );


  ///////////////////////////////////////////////////////
  // Motor
  ifMotors.init();  
  ifMotors.move( 450, 450);    

  ///////////////////////////////////////////////////////
  // Encoder : Cable is crossed !!!
  ifEncoder.init(25, 14, 13, 15);  
}

void loop() {
 /*
    getEncoderSpeed();
    uint32_t speed = abs(_nCurLeft) + abs(_nCurRight) / 2;
    Serial.print("speed: ");
    Serial.println(String(speed));

    delay(1000);
*/    
}

iFBalance의 iFEncoder 클래스의 사용법 역시 극단적으로 편리합니다.

iFEncoder 클래스 객체 내부에서 태스크를 실행하고 태스크 루프에서 매초마다 엔코더의 회전수를 계산하고 있는 코드입니다. 대략적으로 위 코드에서 PWM 값이 450 정도임에도 엔코더의 출력은 이 값의 10 배정도인 4500 이상이 출력이 됩니다.

모터를 회전시키기 위한 450 PWM 출력값에서 부하가 없는 공회전 상태에서 대략 2바퀴 정도 회전하므로 한 바퀴 회전당 2250 엔코더 출력이 발생한다고 계산할 수 있습니다. 바퀴의 지름이 43cm…. 바퀴 둘레의 길이는 ?
아주 미세한 검출 성능이죠. 대략 1cm 당 엔코더의 수는 얼마일까요? 16 탭 수 정도가 발생합니다. 추후 보다 자세히 분석을 해 보도록 합시다.

그런데 엔코더의 출력을 왜 계산하는 것일까요? 점점 복잡한 코드가 늘어나는데 모터의 회전수를 굳이 알 필요가 있을까요? 단지 밸런싱 로봇의 밸런싱을 제어하기 위해서 정말 엔코더의 숫자가 의미가 있을까요?

앞으로 탐구를 진행해 보도록 하죠.

이상으로 이번 강좌에서는 모터를 제어하기 위해 작성된 iFEncoder 클래스에 대해 살펴보았습니다.