UpdateBufferPoints.cpp

 
#include <cassert>
#include "rsi.h" // Import our RapidCode Library. 
#include <cmath>
 
using namespace RSI::RapidCode;
 
void PrintUpdateBufferErrors(RapidCodeObject *rsiClass)
{
    bool hasError = rsiClass->ErrorLogCountGet() > 0;
 
    while (rsiClass->ErrorLogCountGet() > 0)
    {
        const RsiError* err = rsiClass->ErrorLogGet();
 
        printf("%s\n", err->text);
    }
 
    if (hasError)
    {
        exit(1);
    }
}
 
void PrintUpdateBufferNetworkErrors(MotionController *rsiClass)
{
    bool hasError = rsiClass->NetworkLogMessageCountGet() > 0;
 
    while (rsiClass->NetworkLogMessageCountGet() > 0)
    {
        const char *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_t 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();
}