MotorEncoder Library

example.ino

#include <Arduino.h>
#include "MotorEncoder.h"

const int motorPinA  = 14;
const int motorPinB  = 12;
const int encoderPin = 4;

const int maxSteps = 1000;

MotorEncoder me(motorPinA, motorPinB, encoderPin);

void setup() {
    Serial.begin(115200);
    me.begin();
}

void loop() {
    if (me.positionReached()) {
        Serial.println("Position reached");
        delay(2000);
        int newPosition = random(maxSteps);
        Serial.printf("newPosition = %d\r\n", newPosition);
        me.setPosition(newPosition);
    }
}

MotorEncoder.cpp

#include <Arduino.h>
#include "MotorEncoder.h"

MotorEncoder::MotorEncoder(int motorPinA, int MotorPinB, int encoderPin)
    : motorPinA(motorPinA)
    , motorPinB(motorPinB)
    , encoderPin(encoderPin) {
    pinMode(motorPinA, OUTPUT);
    pinMode(motorPinB, OUTPUT);
    pinMode(encoderPin, INPUT);
}

MotorEncoder::~MotorEncoder() {}

void MotorEncoder::begin() {
    attachInterrupt(encoderPin, std::bind(&MotorEncoder::encoderISR, this), RISING);
}

void MotorEncoder::stopMotor() {
    digitalWrite(motorPinA, LOW);
    digitalWrite(motorPinB, LOW);
}

void MotorEncoder::rotateRight() {
    digitalWrite(motorPinA, HIGH);
    digitalWrite(motorPinB, LOW);
}

void MotorEncoder::rotateLeft() {
    digitalWrite(motorPinA, LOW);
    digitalWrite(motorPinB, HIGH);
}

void MotorEncoder::encoderISR() {
    if (position < newPosition) {
        position++;
    }
    if (position > newPosition) {
        position--;
    }
    if (position == newPosition) {
        stopMotor();
    }
}

void MotorEncoder::setPosition(int newPosition) {
    this->newPosition = newPosition;
    if (newPosition < position) rotateLeft();
    if (newPosition > position) rotateRight();
}

int MotorEncoder::getPosition() const {
    return position;
}

bool MotorEncoder::positionReached() const {
    return position == newPosition;
}

MotorEncoder.h

#pragma once
#include <Arduino.h>
#include <FunctionalInterrupt.h>

class MotorEncoder {
  public:
    MotorEncoder(int motorPinA, int MotorPinB, int encoderPin);
    ~MotorEncoder();
    void begin();
    void loop();
    void setPosition(int newPosition);
    int  getPosition() const;
    bool positionReached() const;

  protected:
    int motorPinA;
    int motorPinB;
    int encoderPin;

    int volatile position         = 0;
    int newPosition               = 0;

    void stopMotor();
    void rotateLeft();
    void rotateRight();

  protected:
    void encoderISR();
};