GrblAxis.cpp

/*
** Copyright (C) 2025 CERN
**
** This software is provided 'as-is', without any express or implied
** warranty.  In no event will the authors be held liable for any damages
** arising from the use of this software.
**
** Permission is granted to anyone to use this software for any purpose,
** including commercial applications, and to alter it and redistribute it
** freely, subject to the following restrictions:
**
** 1. The origin of this software must not be misrepresented; you must not
**    claim that you wrote the original software. If you use this software
**    in a product, an acknowledgment in the product documentation would be
**    appreciated but is not required.
** 2. Altered source versions must be plainly marked as such, and must not be
**    misrepresented as being the original software.
** 3. This notice may not be removed or altered from any source distribution.
**
** Created on: 2025-04-16T15:43:07
** Author: Sylvain Fargier <sylvain.fargier@cern.ch>
*/
 
#include "GrblAxis.hpp"
 
#include <ccut/Regex.hpp>
#include <ccut/async.hpp>
 
#include "GrblPlatform.hpp"
#include "util/grbl/GrblDeviceBase.hpp"
#include "util/grbl/GrblGenerator.hpp"
 
using namespace logger;
using namespace smc;
using namespace smc::internal;
using namespace std::chrono;
using grbl::GrblParser;
 
using ImmediateCmd = GrblPlatform::ImmediateCmd;
using MotionCmd = GrblPlatform::MotionCmd;
 
GrblAxis::GrblAxis(const std::string &uid,
                   const std::shared_ptr<GrblPlatform> &grbl) :
    Axis{uid},
    m_grbl{grbl}
{}
 
std::future<void> GrblAxis::moveTo(const units::Value &userPosition) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = grbl->getAxis(uid());
        units::Value pos{userPosition};
        if (!convert(pos, units::unit_t::millimeters))
            throw Exception(ErrorCode::IncompatibleUnits,
                            "failed to convert in millimeters");
 
        grbl::Generator gen;
        gen << grbl::gen::MotionMode::Rapid << grbl::gen::DistanceMode::Absolute
            << grbl::gen::Position{grblAxis, grbl::grbl_float_t(pos.value)};
 
        MotionCmd cmd(
            [grbl](MotionCmd &cmd, GrblPlatform::Context &ctx) {
                cmd.line = ++ctx.line;
                std::string msg = cmd.message;
 
                if (ctx.triggeredMotion)
                {
                    uint8_t trigPin = grbl->getTrigger(ctx.prefix + ctx.id);
                    msg += grbl::gen::DigitalOut::on(trigPin, false).value;
                }
 
                msg += "N" + std::to_string(cmd.line);
                msg += "(MSG,Stepper:NewBlock)";
 
                grbl::ErrorCode code = ctx.device->send(msg);
                ctx.checkReply(code);
            },
            std::set<grbl::Axis>{grblAxis}, gen.str());
        return grbl->queue(std::move(cmd));
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}
 
std::future<void> GrblAxis::moveBy(const units::Value &userPosition) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = grbl->getAxis(uid());
        units::Value pos{userPosition};
        if (!convert(pos, units::unit_t::millimeters))
            throw Exception(ErrorCode::IncompatibleUnits,
                            "failed to convert in millimeters");
 
        grbl::Generator gen;
        gen << grbl::gen::MotionMode::Rapid << grbl::gen::DistanceMode::Relative
            << grbl::gen::Position{grblAxis, grbl::grbl_float_t(pos.value)};
 
        MotionCmd cmd(
            [grbl](MotionCmd &cmd, GrblPlatform::Context &ctx) {
                cmd.line = ++ctx.line;
                std::string msg = cmd.message;
 
                if (ctx.triggeredMotion)
                {
                    uint8_t trigPin = grbl->getTrigger(ctx.prefix + ctx.id);
                    msg += grbl::gen::DigitalOut::on(trigPin, false).value;
                }
 
                msg += "N" + std::to_string(cmd.line);
                msg += "(MSG,Stepper:NewBlock)";
 
                grbl::ErrorCode code = ctx.device->send(msg);
                ctx.checkReply(code);
            },
            std::set<grbl::Axis>{grblAxis}, gen.str());
        return grbl->queue(std::move(cmd));
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}
 
void GrblPlatform::Context::sendHold()
{
    this->checkReply(this->device->sendRealtime(grbl::gen::Realtime::FeedHold));
    const steady_clock::time_point deadline = steady_clock::now() + seconds{30};
 
    while (steady_clock::now() <= deadline)
    {
        this->checkReply(this->device->sendRealtime(
            grbl::gen::Realtime::StatusReport,
            [this](const std::string &message) {
                if (grbl::GrblParser::getLineType(message) ==
                    grbl::LineType::State)
                {
                    this->device->dataSignal(message);
                    throw grbl::ErrorCode{0};
                }
                return false;
            }));
        const grbl::GrblParser::State &st = this->state;
        if (st.runState == grbl::RunState::Hold &&
            static_cast<grbl::HoldStateArg>(st.runStateArg) ==
                grbl::HoldStateArg::Complete)
        {
            return;
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds{100});
        }
    }
    std::ostringstream oss;
    oss << "Failed to pause motion (state=" << grbl::to_string(state.runState)
        << ":" << state.runStateArg << ")";
    throw Exception(ErrorCode::Runtime, oss.str());
}
 
std::future<void> GrblAxis::stop() const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        std::future<void> f = grbl->run<void>(
            [grbl](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                ctx.sendHold();
                grbl->clearQueues("stopped", false);
                ctx.sendReset();
                cmd.prom->get<void>().set_value();
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
std::future<void> GrblAxis::pause() const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        std::future<void> f = grbl->run<void>(
            [](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                ctx.sendHold();
                cmd.prom->get<void>().set_value();
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
std::future<void> GrblAxis::resume() const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        std::future<void> f = grbl->run<void>(
            [](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                ctx.checkReply(
                    ctx.device->sendRealtime(grbl::gen::Realtime::CycleStart));
                cmd.prom->get<void>().set_value();
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
std::future<units::value_t> GrblAxis::getPosition(units::unit_t unit) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        std::shared_ptr<const Axis> obj{
            std::static_pointer_cast<const Axis>(shared_from_this())};
 
        std::future<units::value_t> f = grbl->run<units::value_t>(
            [unit, obj](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                ctx.device->sendRealtime(
                    grbl::gen::Realtime::StatusReport,
                    [&ctx](const std::string &message) {
                        if (grbl::GrblParser::getLineType(message) ==
                            grbl::LineType::State)
                        {
                            ctx.device->dataSignal(message);
                            throw grbl::ErrorCode{0};
                        }
                        return false;
                    });
                cmd.prom->get<units::value_t>().set_value(
                    obj->lastPosition(unit));
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<units::value_t>(ex);
    }
}
 
std::future<void> GrblAxis::setActualPosition(
    const units::Value &userPosition) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = grbl->getAxis(uid());
        units::Value pos{userPosition};
 
        if (!convert(pos, units::unit_t::millimeters))
            throw Exception(ErrorCode::IncompatibleUnits,
                            "failed to convert in millimeters");
 
        std::future<void> f = grbl->run<void>(
            [grblAxis, pos](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                grbl::Generator gen;
                gen << grbl::gen::NonModal::SetCoordinate(-1, true)
                    << grbl::gen::Position{grblAxis,
                                           grbl::grbl_float_t(pos.value)};
                grbl::ErrorCode code = ctx.device->send(gen.str());
                ctx.checkReply(code);
 
                cmd.prom->get<void>().set_value();
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}
 
units::value_t GrblAxis::lastPosition(units::unit_t unit) const
{
    units::value_t ret;
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        ret = grbl->getLastAxisPosition(uid());
    }
    if (ret.unit != unit)
    {
        if (!convert(ret, unit))
            throw Exception(ErrorCode::IncompatibleUnits,
                            "failed to convert to " + to_string(unit));
    }
    return ret;
}
 
static const ccut::Regex alarmCodeRegex{"ALARM:([[:digit:]]+)"};
 
std::future<void> GrblAxis::takeReference(
    const std::chrono::milliseconds &timeout) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        // create an immediate command
        std::future<void> f = grbl->run<void>(
            [grbl, timeout](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                // send the homing command
                grbl::ErrorCode code = ctx.device->send(
                    grbl::gen::System::Home,
                    [&ctx](const std::string &line) {
                        std::vector<std::string> matches;
                        if (alarmCodeRegex.match(line, matches))
                        {
                            int code = std::stoi(matches[1].c_str());
                            std::string alarm_msg = ctx.getAlarmMessage(
                                static_cast<grbl::AlarmCode>(code));
                            error(GrblPlatform::s_loggerCat) << alarm_msg;
                            throw grbl::CustomErrorCode::InternalError;
                        }
                        return true;
                    },
                    timeout);
                // check the reply
                if (code == grbl::CustomErrorCode::Timeout)
                {
                    error(GrblPlatform::s_loggerCat)
                        << "homing request timeout";
                    grbl->clearQueues("homing failed", false);
                    ctx.sendReset();
                }
                ctx.checkReply(code);
                // set the promise value
                cmd.prom->get<void>().set_value();
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<void>(ex);
    }
}
 
static Axis::State grblStateToAxisState(const grbl::RunState &state)
{
    switch (state)
    {
    case grbl::RunState::Idle: return Axis::State::IDLE;
    case grbl::RunState::Run:
    case grbl::RunState::Hold:
    case grbl::RunState::Jog:
    case grbl::RunState::Home: return Axis::State::RUNNING;
    case grbl::RunState::Alarm: return Axis::State::ERROR;
    default: return Axis::State::UNKNOWN;
    }
}
 
std::future<Axis::State> GrblAxis::getState() const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        std::future<Axis::State> f = grbl->run<Axis::State>(
            [](ImmediateCmd &cmd, const GrblPlatform::Context &ctx) {
                grbl::RunState grblState = ctx.state.runState;
                Axis::State runState = grblStateToAxisState(grblState);
                cmd.prom->get<Axis::State>().set_value(runState);
            });
        return f;
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<Axis::State>(ex);
    }
}
 
std::future<std::set<std::string>> GrblAxis::listConfig() const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        grbl::Axis grblAxis = grbl->getAxis(uid());
        return grbl->run<std::set<std::string>>(
            [grblAxis](ImmediateCmd &cmd, GrblPlatform::Context &ctx) {
                cmd.prom->get<std::set<std::string>>().set_value(
                    ctx.listConfig(grblAxis));
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<std::set<std::string>>(ex);
    }
}
 
std::future<std::string> GrblAxis::getConfig(const std::string &name) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string uid = this->uid();
 
        grbl::Axis grblAxis = grbl->getAxis(uid);
        return grbl->run<std::string>([name, grblAxis, uid,
                                       grbl](ImmediateCmd &cmd,
                                             GrblPlatform::Context &ctx) {
            grbl::SettingId id = ctx.findSetting(name, grblAxis).id;
            std::string ret;
            grbl::ErrorCode rc = ctx.device->send(
                grbl::gen::System::getSetting(id),
                [id, uid, &ret, grbl](const std::string &line) {
                    if (GrblParser::getLineType(line) == grbl::LineType::Setting)
                    {
                        GrblParser::Setting setting;
                        if ((GrblParser::parseSetting(line, setting)) &&
                            (setting.first == id))
                        {
                            ret = setting.second;
                            grbl->updateAxisSetting(uid, setting.first,
                                                    setting.second);
                            return true;
                        }
                    }
                    return false;
                });
            ctx.checkReply(rc);
 
            cmd.prom->get<std::string>().set_value(ret);
        });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error<std::string>(ex);
    }
}
 
std::future<void> GrblAxis::setConfig(const std::string &name,
                                      const std::string &value) const
{
    try
    {
        std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
        if (!grbl)
            throw Exception(ErrorCode::Canceled, "platform stopped (deleted)");
 
        const std::string uid = this->uid();
        grbl::Axis grblAxis = grbl->getAxis(uid);
        return grbl->run<void>(
            [name, value, grblAxis, uid, grbl](ImmediateCmd &cmd,
                                               GrblPlatform::Context &ctx) {
                grbl::SettingId id = ctx.findSetting(name, grblAxis).id;
                grbl::ErrorCode rc = ctx.device->send(
                    grbl::gen::System::setSetting(id, value));
                ctx.checkReply(rc);
                grbl->updateAxisSetting(uid, id, value);
                cmd.prom->get<void>().set_value();
            });
    }
    catch (const Exception &ex)
    {
        return ccut::make_future_error(ex);
    }
}
 
bool GrblAxis::canConvert(const units::Value &value, units::unit_t unit) const
{
    units::value_t temp{value};
    return convert(temp, unit);
}
 
bool GrblAxis::convert(units::Value &value, units::unit_t unit) const
{
    std::shared_ptr<GrblPlatform> grbl(m_grbl.lock());
 
    if (unit != value.unit && grbl)
    {
        switch (unit)
        {
        case units::STEPS:
            if (value.unit == units::MILLIMETERS)
            {
                units::Value resolution = grbl->getLastAxisResolution(uid());
                if (resolution.value <= 0)
                {
                    logger::warning(GrblPlatform::s_loggerCat)
                        << "axis resolution not set";
                    return false;
                }
                value = resolution * value;
                return true;
            }
            break;
        case units::MILLIMETERS:
            if (value.unit == units::STEPS)
            {
                units::Value resolution = grbl->getLastAxisResolution(uid());
                if (resolution.value <= 0)
                {
                    logger::warning(GrblPlatform::s_loggerCat)
                        << "axis resolution not set";
                    return false;
                }
                value = value / resolution;
                return true;
            }
            break;
        default: break;
        }
        try
        {
            units::Value resolution = grbl->getLastAxisResolution(uid());
            if (resolution.value <= 0)
            {
                logger::warning(GrblPlatform::s_loggerCat)
                    << "axis resolution not set";
                return false;
            }
            switch (value.unit)
            {
            case units::STEPS: value = value * resolution; return true;
            case units::MILLIMETERS: value = resolution / value; return true;
            default: return false;
            }
        }
        catch (const Exception &ex)
        {
            logger::warning(GrblPlatform::s_loggerCat)
                << "failed to convert value: " << ex.what();
            return false;
        }
    }
    return DeviceBase::convert(value, unit);
}