machines/sheetpress/cassandra/firmware/firmware/LinearActuator.cpp

#include "LinearActuator.h"
#include "config.h"
#include "common/macros.h"

#ifdef HAS_STATES
#include <ArduinoJson.h>
#endif

#define HAS_LA_DEBUG

#ifdef HAS_LA_DEBUG
#define LA_DEBUG(A) Serial.println(A);
#else
#define LA_DEBUG(A)
#endif

#ifdef HAS_STATES
String LinearActuator::state()
{
    const int capacity = JSON_OBJECT_SIZE(3);
    StaticJsonDocument<capacity> doc;
    doc[0] = id;
    doc[1] = _state;
    doc[2] = pFlags;

#ifdef HAS_LA_DEBUG
    Serial.println("state : ");
    Serial.println(_state);
    Serial.print("pflags low : ");
    Serial.println(pFlags);
    Serial.print("limit high : ");
    Serial.println(u1.value);

    Serial.print("moving: ");
    Serial.println(TEST(pFlags, MOVING));
    Serial.print("retracting: ");
    Serial.println(TEST(pFlags, RETRACTING));
    Serial.print("freeing: ");
    Serial.println(TEST(pFlags, FREEING));
    Serial.print("done: ");
    Serial.println(TEST(pFlags, DONE));
    Serial.print("retracted: ");
    Serial.println(TEST(pFlags, RETRACTED));
    Serial.print("retracting on : ");
    Serial.println(retracting);
#endif

    /*
    Serial.println("pflags");
    Serial.print("moving: ");
    Serial.println(TEST(pFlags, MOVING));
    Serial.print("retracting: ");
    Serial.println(TEST(pFlags, RETRACTING));
    Serial.print("freeing: ");
    Serial.println(TEST(pFlags, FREEING));
    Serial.print("done: ");
    Serial.println(TEST(pFlags, DONE));
    Serial.print("retracted: ");
    Serial.println(TEST(pFlags, RETRACTED));
    Serial.print("retracting on : ");
    Serial.println(retracting);*/

    return doc.as<String>();
}
#endif

#define LA_NOTIFY(STATE)                       \
    if (owner && onChange)                     \
    {                                          \
        short ret = (owner->*onChange)(STATE); \
        return ret;                            \
    }

#define LA_ALARM_ABORT                                               \
    if (motorAlarmPin && analogRead(motorAlarmPin) > LA_ALARM_LEVEL) \
    {                                                                \
        LA_NOTIFY(ERROR);                                            \
        return ERROR;                                                \
    }

#define LA_LS_CHECK                                     \
    if (maxMoveTime && now - moveStartTS > maxMoveTime) \
    {                                                   \
        stop();                                         \
        _state = ERROR;                                 \
        LA_NOTIFY(ERROR_FATAL);                         \
    }

#define LA_ALARM_LEVEL 700

bool LinearActuator::pause()
{
    SBI(pFlags, PAUSED);
    digitalWrite(motorStepPin, LOW);
}
bool LinearActuator::resume()
{
    LA_ALARM_ABORT;

    CBI(pFlags, PAUSED);
    if (_state = PLUNGING && !u1.value && !l1.value)
    {
        move(0);
    }
    if (_state = HOMING && !u1.value && !l1.value)
    {
        move(1);
    }
}
short LinearActuator::move(short dir)
{
    LA_ALARM_ABORT;
    digitalWrite(motorDirPin, dir ? LOW : HIGH);
    digitalWrite(motorStepPin, HIGH);
    SBI(pFlags, MOVING);
    LA_NOTIFY(dir ? PLUNGING : HOMING);
}
bool LinearActuator::change(short newState)
{
    if (newState == _state)
    {
        return false;
    }
    _state = newState;
    return true;
}
short LinearActuator::setup()
{
    u1.setup();
    u1.loop();
    l1.setup();
    l1.loop();
    pFlags = 0;
    digitalWrite(motorStepPin, LOW);
    retracting = false;
}
void LinearActuator::debug(Stream *stream)
{
    *stream << name << " : " << u1.value << " : " << l1.value;
}

void LinearActuator::info(Stream *stream)
{
    *stream << name << " : " << SPACE("L-UP-PIN") << " : " << u1.pin << SPACE("L-DOWN-PIN") << " : " << l1.pin << '\n';
}

short LinearActuator::plunge(short force)
{
    LA_ALARM_ABORT;
    if (!change(PLUNGING))
    {
        if (!TEST(pFlags, DONE))
        {
            return TEST(pFlags, DONE);
        }
        else
        {
            if (force)
            {
                reset();
                _state = PLUNGING;
            }
            else
            {
                return TEST(pFlags, DONE);
            }
        }
    }
    else
    {
        reset();
        _state = PLUNGING;
        moveStartTS = now;
    }

    if (u1.value)
    {
        SBI(pFlags, FREEING);
        move(0);
        return TEST(pFlags, DONE);
    }
    else
    {
        SBI(pFlags, MOVING);
        move(0);
    }

    return TEST(pFlags, DONE);
}
short LinearActuator::retract()
{
    u1.loop();
    l1.loop();
    if (u1.value)
    {
        if (!retracting)
        {
            retracting = true;
            LA_DEBUG("PLUNGER : collision!");
        }
        SBI(pFlags, RETRACTING);
        return 1;
    }
    else
    {

        retracting = false;
        return 0;
    }
}
short LinearActuator::home(short force = false)
{
    if (!change(HOMING))
    {
        LA_DEBUG("PLUNGER already homing");
        if (!TEST(pFlags, DONE))
        {
            LA_DEBUG("PLUNGER already homing : not done yet");
            return TEST(pFlags, DONE);
        }
        else
        {
            if (force == false)
            {
                return TEST(pFlags, DONE);
            }
            else
            {
                LA_DEBUG("PLUNGER already homing : doing it again");
                reset();
                _state = HOMING;
            }
        }
    }
    else
    {
        reset();
        _state = HOMING;
        moveStartTS = now;
    }

    if (u1.value)
    {
        LA_DEBUG("PLUNGER : frooze up");
        SBI(pFlags, FREEING);
        return TEST(pFlags, DONE);
    }
    else
    {
        LA_DEBUG("PLUNGER : move");
        move(1);
    }
    return TEST(pFlags, DONE);
}
short LinearActuator::stop(short val)
{
    digitalWrite(motorStepPin, LOW);
    CBI(pFlags, MOVING);
    CBI(pFlags, FREEING);
}
short LinearActuator::reset(short val)
{
    pFlags = 0;
    _state = NONE;
    retracting = false;
}
short LinearActuator::loop()
{
    l1.loop();
    u1.loop();

    LA_ALARM_ABORT;

    // retract
    if (TEST(pFlags, MOVING))
    {
        if (u1.value || l1.value)
        {
            if (u1.value && !TEST(pFlags, RETRACTING))
            {
                LA_DEBUG("retract down");
                SBI(pFlags, RETRACTING);
                move(0);
            }

            if (l1.value && !TEST(pFlags, RETRACTING))
            {
                LA_DEBUG("retract up");
                SBI(pFlags, RETRACTING);
                move(1);
            }
        }
        else
        {
            if (TEST(pFlags, RETRACTING))
            {
                LA_DEBUG("retracting done");
                stop();
                SBI(pFlags, RETRACTED);
                CBI(pFlags, RETRACTING);
            }
        }
    }

    switch (_state)
    {
    case ERROR:
    case MANUAL:
    {
        break;
    }
    case STOPPED:
    {
        break;
    }
    case HOMING:
    {
        if (TEST(pFlags, DONE))
        {
            return;
        }
        if (TEST(pFlags, RETRACTED))
        {
            stop();
            SBI(pFlags, DONE);
            CBI(pFlags, RETRACTED);
            LA_DEBUG("homed");
            LA_NOTIFY(DONE);
        }
        else if (!TEST(pFlags, RETRACTING) && !TEST(pFlags, MOVING))
        {
            LA_DEBUG("homing : move on");
            move(1);
            LA_LS_CHECK;
        }
        break;
    }
    case PLUNGING:
    {

        if (TEST(pFlags, DONE))
        {
            return;
        }
        if (TEST(pFlags, RETRACTED))
        {
            stop();
            SBI(pFlags, DONE);
            CBI(pFlags, RETRACTED);
            LA_DEBUG("plunged");
            LA_LS_CHECK;
        }
        else if (!TEST(pFlags, RETRACTING) && !TEST(pFlags, MOVING))
        {
            LA_DEBUG("plunging : move on");
            move(0);
            LA_LS_CHECK;
        }
        break;
    }
    }
}