3PhaseEncoder_Template/3PEncoder.cpp at main · TNCT-Mechatech/3PhaseEncoder_Template · GitHub

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
#include "3PEncoder.hpp"
#include <iostream>
#include <thread>
#include <chrono>

std::atomic<int> AMT102VEncoder::pigpioInitCount(0);

AMT102VEncoder::AMT102VEncoder(int gpioA, int gpioB, int gpioC)
    : pinA(gpioA), pinB(gpioB), pinC(gpioC),
      position(0), lastDirection(0), revolutionFlag(false),
      cbIdA(-1), cbIdB(-1), cbIdC(-1)
{
    if (pigpioInitCount.fetch_add(1) == 0) {
        if (gpioInitialise() < 0) {
            throw std::runtime_error("pigpio initialization failed");
        }
    }

    gpioSetMode(pinA, PI_INPUT);
    gpioSetMode(pinB, PI_INPUT);
    gpioSetMode(pinC, PI_INPUT);
    gpioSetPullUpDown(pinA, PI_PUD_UP);
    gpioSetPullUpDown(pinB, PI_PUD_UP);
    gpioSetPullUpDown(pinC, PI_PUD_UP);

    cbIdA = gpioSetAlertFuncEx(pinA, encoderISR, this);
    cbIdB = gpioSetAlertFuncEx(pinB, encoderISR, this);
    cbIdC = gpioSetAlertFuncEx(pinC, cPhaseISR, this);
}

AMT102VEncoder::~AMT102VEncoder()
{
    // pigpioのコールバックはアンインストール不要
    if (pigpioInitCount.fetch_sub(1) == 1) {
        gpioTerminate();
    }
}

void AMT102VEncoder::start()
{
    // 必要ならここで状態初期化など（今回は特に何もしない）
}

void AMT102VEncoder::stop()
{
    // 必要ならここでコールバック解除など（今回は特に何もしない）
}

int AMT102VEncoder::getPosition() const
{
    return position.load();
}

int AMT102VEncoder::getDirection() const
{
    return lastDirection.load();
}

void AMT102VEncoder::encoderISR(int gpio, int level, uint32_t tick, void* userdata)
{
    AMT102VEncoder* self = reinterpret_cast<AMT102VEncoder*>(userdata);

    static int lastA = 0, lastB = 0;
    int A = gpioRead(self->pinA);
    int B = gpioRead(self->pinB);

    int delta = 0;
    if (gpio == self->pinA && level != PI_TIMEOUT) {
        if (A != lastA) {
            if (A == B) delta = +1;
            else       delta = -1;
            lastA = A;
        }
    } else if (gpio == self->pinB && level != PI_TIMEOUT) {
        if (B != lastB) {
            if (A != B) delta = +1;
            else       delta = -1;
            lastB = B;
        }
    }
    if (delta != 0) {
        self->position += delta;
        self->lastDirection = (delta > 0) ? 1 : -1;
    }
}

void AMT102VEncoder::cPhaseISR(int gpio, int level, uint32_t tick, void* userdata)
{
    AMT102VEncoder* self = reinterpret_cast<AMT102VEncoder*>(userdata);
    if (level == 1) { // 立ち上がり
        self->revolutionFlag = true;
    }
}

int AMT102VEncoder::measurePPR()
{
    std::cout << "AMT102V 分解能（PPR）自動測定を開始します。" << std::endl;
    std::cout << "エンコーダ軸を1回転させてください。" << std::endl;

    int start_count = 0, end_count = 0;
    bool measuring = false;
    int revolutions = 0;
    int prev_direction = 0;

    while (revolutions < 2) {
        int d = getDirection();
        if (revolutionFlag.exchange(false)) {
            revolutions++;
            if (!measuring) {
                start_count = getPosition();
                measuring = true;
                prev_direction = d;
                std::cout << "[START] C相検出: 計測開始" << std::endl;
            } else {
                end_count = getPosition();
                std::cout << "[END] C相検出: 計測終了" << std::endl;
            }
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    int diff = end_count - start_count;
    if (prev_direction < 0 && diff > 0) diff = -diff;
    int ppr = std::abs(diff);

    std::cout << "AMT102V 実測分解能（PPR）： " << ppr << " パルス/回転" << std::endl;
    std::cout << "DIPスイッチ設定と一致するかご確認ください。" << std::endl;

    return ppr;
}