motor.h

#pragma once

#define M4_MMREV  0.7
#define M5_MMREV  0.8


#if CORE_MOCK
class AccelStepper
{
public:
    enum { DRIVER, FULL4WIRE, };
    AccelStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true) {}
    unsigned long computeNewSpeed () { return 10; }
    float maxSpeed () { return 10; }
    float acceleration () { return 5; }
    long distanceToGo () { return 0; }
};
#else
#include <AccelStepper.h>
#endif

class ISRStepper: public AccelStepper
{
    // usefulness: make computeNewSpeed() public to be called outside from interrupt
public:
    ISRStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true):
        AccelStepper(interface, pin1, pin2, pin3, pin4, enable) { }
    // make it public
    inline unsigned long computeNewSpeed () { return AccelStepper::computeNewSpeed(); }
};

class Motor
{
public:

    using MotorHardware_t = ISRStepper;

    Motor (MotorHardware_t& stepper): stepper(stepper)
    {
        setSpeedMmPerSec(1);
        setAccelMmPerSecPerSec(0.5); //  <----- this is not yet configured by user
    }

    void setMmPerRev (float mmPerRevolution)
    {
        _mmPerRevolution = mmPerRevolution;
    }

    float getMmPerRev () const
    {
        return _mmPerRevolution;
    }

    void setPhysical (int stepsPerRevolution, float mmPerRevolution, bool forwardClockwise)
    {
        _stepsPerRevolution = stepsPerRevolution;
        setMmPerRev(mmPerRevolution);
        setForwardClockwise(forwardClockwise);
    }

    void setForwardClockwise (bool clockwise)
    {
        _forwardClockwise = clockwise;
    }

    bool getForwardClockwise ()
    {
        return _forwardClockwise;
    }

    void setSpeedMmPerSec (float mmPerSec)
    {
#if !CORE_MOCK
        stepper.setMaxSpeed(mmToStep(mmPerSec));
#endif
    }

    void setAccelMmPerSecPerSec (float mmPerSecPerSec)
    {
        if (mmPerSecPerSec == 0)
            mmPerSecPerSec = 1000;
        _retainAccel = mmPerSecPerSec;
#if !CORE_MOCK
        stepper.setAcceleration(mmToStep(mmPerSecPerSec));
#endif
    }

    float getAccelMmPerSecPerSec () const
    {
        return _retainAccel;
    }

    float mmToStep (float mm) const
    {
        return mm * _stepsPerRevolution / _mmPerRevolution;
    }

    float stepToMm (int step) const
    {
        return step * _mmPerRevolution / _stepsPerRevolution;
    }

    int whereStep ()
    {
#if CORE_MOCK
        return 0;
#else
        return stepper.currentPosition() * (_forwardClockwise? 1: -1);
#endif
    }

    void moveToMm (float mm)
    {
        Serial.printf("# moving to %g mm / %g steps\n",
            mm,
            mmToStep(mm));
        moveToStep(mmToStep(mm));
    }

    void moveToStep (int step)
    {
#if !CORE_MOCK
        cli();
        setAccelMmPerSecPerSec(_retainAccel);
        if (_mmPerRevolution > 0 && _stepsPerRevolution > 0)
            stepper.moveTo(_forwardClockwise? step: -step);
        sei();
#endif
        Serial.printf("#     move to: %d step    %g mm\n", step, stepToMm(step));
    }

    void stop ()
    {
#if !CORE_MOCK
        cli();
        stepper.stop();
        sei();
#endif
    }

	void resetPosition()
	{
#if !CORE_MOCK
        stop();
        cli();
	    stepper.setCurrentPosition(0);
	    sei();
#endif
	}

	void stayHere ()
	{
#if !CORE_MOCK
        cli();
        stepper.setAcceleration(1e20);
	    stepper.moveTo(stepper.currentPosition()); // change target to here
        setAccelMmPerSecPerSec(_retainAccel);
	    sei();
#endif
	}

    void showConfiguration ()
    {
        Serial.printf("#     %g mm/rev\n"
                      "#     %d steps/rev\n"
                      "#     %g steps/mm\n"
                      "#     max speed: %g steps/s   %g mm/s\n"
                      "#     accel: %g steps/s/s   %g mm/s/s\n",
                      _mmPerRevolution,
                      _stepsPerRevolution,
                      _stepsPerRevolution / _mmPerRevolution,
                      stepper.maxSpeed(),
                      stepToMm(stepper.maxSpeed()),
                      stepper.acceleration(),
                      stepToMm(stepper.acceleration())
                  );
    }

protected:

    int _stepsPerRevolution = -1;
    float _mmPerRevolution = -1;
    float _retainAccel = -1;
    bool _forwardClockwise;

    MotorHardware_t& stepper;
};