EV3Motor.h

#pragma once
#include <stdint.h>
#include <EV3Peripheral.h>



 
 
#define ev3Motor_EncoderGlobal(motor) void ev3Motor_STATIC_##motor##__EncoderWrapper() {motor._CountPulse();}
 
#define ev3Motor_SetupEncoder(motor) attachInterrupt(digitalPinToInterrupt(motor.Ports().EncoderA), ev3Motor_STATIC_##motor##__EncoderWrapper, RISING);
 
namespace ev3 {

struct MotorPortConfig {
    uint8_t Enable, In1, In2, EncoderA, EncoderB;
};
 
enum MotorDirection {
    FORWARD, REVERSE
};

class Motor : public _::Peripheral<MotorPortConfig> {
    MotorPortConfig ports;
    volatile unsigned long encoder = 0;
    bool encoding = false;
    bool runningToPosition = false;
    unsigned long target = 0;
 
    void checkDistance();
public:
    EV3_PERIPHERAL_CONSTRUCT(Motor)
 
    void SetPower(uint8_t power); 
    void SetDirection(MotorDirection direction); 
    void StartEncoder(); 
    void StopEncoder(); 
    void ResetEncoder(); 
    void RunToPosition(uint8_t power, unsigned long position); 
 
    void _CountPulse(); 
 
    virtual void Setup(); 
    virtual void Loop(); 
};
}