UpdateBufferPoints.cpp

#include <cassert>
#include "rsi.h"                                    // Import our RapidCode Library. 

using namespace RSI::RapidCode;

void PrintUpdateBufferErrors(RapidCodeObject *rsiClass)
{
    RsiError *err;
    bool hasError = rsiClass->ErrorLogCountGet() > 0;

    while (rsiClass->ErrorLogCountGet() > 0)
    {
        err = rsiClass->ErrorLogGet();

        printf("%s\n", err->text);
    }

    if (hasError)
    {
        exit(1);
    }
}

void PrintUpdateBufferNetworkErrors(MotionController *rsiClass)
{
    char *err;
    bool hasError = rsiClass->NetworkLogMessageCountGet() > 0;

    while (rsiClass->NetworkLogMessageCountGet() > 0)
    {
        err = rsiClass->NetworkLogMessageGet(0);

        printf("%s\n", err);
    }

    if (hasError)
    {
        exit(1);
    }
}

void updateBufferPointsMain()
{
    const double    TIME_SLICE = 0.001;                                // 0.001s = 1ms
    const int        AXIS_COUNT = 1;                                    // number of axes

    const int        REVS = 5;                                        // number of revolutions
    const int        RPS = 1;                                        // revs / sec
    const int        CPS = (int)std::powl(2, 20);                    // encoder counts per rev (set as appropiate)
    const int        TOTAL_POINTS = (int)(REVS / TIME_SLICE / RPS);    // total number of points
    const int        BUFFER_SZ = 100;                                // Number of points to send in a buffer
    const int        EMPTY_CT = 10;                                    // Number of points that remains in the beffer before an e-stop

    // Initizalize the controller from software w/ multiple axes
    MotionController *controller;
    MultiAxis *multiAxis;
    controller = MotionController::CreateFromSoftware();
    PrintUpdateBufferErrors(controller);
    try {

        controller->NetworkStart();
        PrintUpdateBufferNetworkErrors(controller);

        // add an additional axis for the multiaxis supervisor
        controller->MotionCountSet(AXIS_COUNT + 1);

        // create the multiaxis using the ID of the first free axis (0 indexed)
        multiAxis = controller->MultiAxisGet(AXIS_COUNT);
        PrintUpdateBufferErrors(multiAxis);

        // populate the multiaxis
        for (int i = 0; i < AXIS_COUNT; i++)
        {
            Axis *tempAxis;
            tempAxis = controller->AxisGet(i);

            PrintUpdateBufferErrors(tempAxis);

            tempAxis->EStopAbort();
            tempAxis->ClearFaults();
            tempAxis->PositionSet(0);

            tempAxis->UserUnitsSet(CPS);
            multiAxis->AxisAdd(tempAxis);
        }

        // populate the positions and times 
        std::vector<double> positions, times;
        for (int i = 0; i < TOTAL_POINTS; i += AXIS_COUNT)
        {
            positions.push_back(i * TIME_SLICE * RPS);
            times.push_back(TIME_SLICE);
        }


        // prepare the controller (and drive)
        multiAxis->Abort();
        multiAxis->ClearFaults();
        assert(multiAxis->StateGet() == RSIState::RSIStateIDLE);
        multiAxis->AmpEnableSet(true);

        // reset the motion ID to 0
        multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0], &times[0], 1, -1, false, true);
        multiAxis->MotionIdSet(0);


        // Establish how to keep track of what blocks have been sent
        int curMotionElementID = 0, curMotionID = 0, finalMotionID = 0;
        int numPointsToSend = BUFFER_SZ;
        int endOfLastSent = 0;

        bool exitCondition = false;


        // Set up a motion hold gate so we can start buffering blocks
        const int motionHoldGate = 3;
        controller->MotionHoldGateSet(motionHoldGate, true);
        multiAxis->MotionHoldGateSet(motionHoldGate);

        for (int i = 0; i < 2; ++i)
        {
            multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0] + endOfLastSent * AXIS_COUNT, &times[0] + endOfLastSent, numPointsToSend, EMPTY_CT, false, exitCondition);
            endOfLastSent += numPointsToSend;
            ++finalMotionID;
        }


        // Set up the interrupt frequency period
        controller->SyncInterruptPeriodSet(10); // this generates an interrupt every x cycles of a 1KHz sample rate
        // With our timeslices of 1ms, this is 10*1ms=10ms
        controller->SyncInterruptEnableSet(true);

        controller->MotionHoldGateSet(motionHoldGate, false); // release the hold gate to start moving

        while (!exitCondition)
        {
            int32 sampleRecieved = controller->SyncInterruptWait();    // see above for timing
            curMotionID = multiAxis->MotionIdExecutingGet();        // this takes an unspecified amount of time (non-rtos) so this is going to be called sometime after the interrupt gets handled.
            // There's an additional delay to retrieve the data as well.
            curMotionElementID = multiAxis->MotionElementIdExecutingGet();

            /*
            Each MovePT assigns a new MotionID for each call to a move update (with the bufferred points)
            Working under the assumption that each Buffer gets a new ID, send two (or several) smaller ones
            and when the change in IDs is greater than some number, send a new one
            */

            // change this logic to manage the number of buffered moves (and points) as appropiate
            // generate points as appropiate
            if (std::abs(finalMotionID - curMotionID) < 2)
            {
                // check end condition
                if (TOTAL_POINTS <= (endOfLastSent + BUFFER_SZ))
                {
                    numPointsToSend = TOTAL_POINTS - endOfLastSent; // send the remaining points
                    exitCondition = true;
                }
                multiAxis->MovePT(RSIMotionType::RSIMotionTypePT, &positions[0] + endOfLastSent * AXIS_COUNT, &times[0] + endOfLastSent, numPointsToSend, EMPTY_CT, false, exitCondition);

                printf("MotionID %d\nEnd of Last Sent %d\nElement ID %d\nNum to Send %d\nIs Done %s\n===========================================\n\n",
                    curMotionID, endOfLastSent, curMotionElementID, numPointsToSend, exitCondition ? "yes" : "no");

                endOfLastSent += numPointsToSend;
                ++finalMotionID;
            }
        }
        printf("Updates Done. Waiting to finish motion.\n");
        multiAxis->MotionDoneWait();
        printf("Motion Complete. Final Motion ID: %d\tFinal Element ID %d\n", multiAxis->MotionIdExecutingGet(), multiAxis->MotionElementIdExecutingGet());

        multiAxis->EStopAbort();
    }
    catch (RsiError const& err) {
        printf("\n%s\n", err.text);
    }
    controller->Delete();

}