.

     

MPI Application Template
template.c
 

shape.c -- Configure Trajectory Shaping Filters
/* shape.c */

/* Copyright(c) 1991-2006 by Motion Engineering, Inc.  All rights reserved.
 *
 * This software  contains proprietary and  confidential information  of
 * Motion Engineering Inc., and its suppliers.  Except as may be set forth
 * in the license agreement under which  this software is supplied, use,
 * disclosure, or  reproduction is prohibited without the prior express
 * written consent of Motion Engineering, Inc.
 */

/*

:Configure Trajectory Shaping Filters

Warning!  This is a sample program to assist in the integration of an
 MEI motion controller with your application.  It may not contain all
 of the logic and safety features that your application requires.

*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <ctype.h>

#include "stdmpi.h"
#include "stdmei.h"

#include "apputil.h"

#if defined(ARG_MAIN_RENAME)
#define main    shapeMain

argMainRENAME(main, shape)
#endif

typedef struct SHAPER {
    long    Index;
    long    Axis;
    long    Length;
    long    MaxDelay;
    long    MaxAmp;
    double  TimeStep;
    long    Time[MEIPreFilterCoeffsMAX];
    long    Amp[MEIPreFilterCoeffsMAX];
} SHAPER;

long
    loadShaper(MPIControl   control,
               SHAPER       *shaper);

int
    main(int    argc,
         char   *argv[])
{
    MPIControl          control;    /* motion controller handle */
    MPIControlConfig    config;

    SHAPER      shaper;

    char    *fileName;
    FILE    *fp;
    char    buffer[256];
    long    shaperCount;
    long    shaperCountOld;

    long    returnValue;

    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    long    argIndex;

    argIndex =
        argControl(argc,
                   argv,
                   &controlType,
                   &controlAddress);

    fileName =
        (argIndex >= argc)
                ? "shape.txt"
                : argv[argIndex++];

    if (argIndex < argc) {
        meiPlatformConsole("usage: %s %s\n"
                           "\t\t[fileName (%s)]\n",
                           argv[0],
                           ArgUSAGE,
                           "shape.txt");
        exit(0);
    }

    /* Create motion controller object */
    control =
        mpiControlCreate(controlType,
                         &controlAddress);
    msgCHECK(mpiControlValidate(control));

    /* Initialize motion controller */
    returnValue = mpiControlInit(control);
    msgCHECK(returnValue);

    fp = fopen(fileName, "r");

    if (fp == NULL) {
        fprintf(stderr,
                "shape.txt not found.\n");
        exit(2);
    }

    returnValue =
        mpiControlConfigGet(control,
                            &config,
                            NULL);
    msgCHECK(returnValue);

    shaperCount = 0;
    shaperCountOld = -1;

    while (fgets(buffer, sizeof(buffer), fp) != NULL) {
        long    axis;
        long    length;
        long    maxAmp;
        long    maxDelay;

        char    *ptr;

        if (shaperCount > shaperCountOld) {
            shaper.Axis     = -1;   /* Make sure all params are read from file */
            shaper.MaxAmp   = -1;
            shaper.Length   =  0;
            shaper.MaxDelay = -1;
            shaper.TimeStep = 1.0 / config.sampleRate;

            shaperCountOld = shaperCount;
        }

        for (ptr = buffer; *ptr != '\0'; ptr++) {
            char    byte = *ptr;

            if (isupper(byte)) {
                *ptr = (char)tolower(byte);
            }
        }

        if (sscanf(buffer, "#axis  = %d", &axis) == 1) {
            shaper.Axis = axis;
            continue;
        }

        if (sscanf(buffer, "#impulses = %d", &length) == 1) {
            shaper.Length = length;
            continue;
        }

        if (sscanf(buffer, "#ampsum = %d", &maxAmp) == 1) {
            shaper.MaxAmp = maxAmp;
            continue;
        }

        if (sscanf(buffer, "#steps = %d", &maxDelay) == 1) {
            shaper.MaxDelay = maxDelay;
            continue;
        }

        if (strncmp(buffer, "#time", 5) == 0) {
            long    sum;
            long    maxTime;
            long    index;

            sum = 0;
            maxTime = 0;

            for (index = 0; index < shaper.Length; index++) {
                long    time;
                long    amp;

                if (fgets(buffer, sizeof(buffer), fp) == NULL) {
                    fprintf(stderr,
                            "Unexpected end-of-file\n");
                    return ((int)returnValue);

                }

                if (sscanf(buffer, "%d %d", &time, &amp) == 2) {
                    shaper.Time[index]  = time;
                    shaper.Amp[index]   = amp;
                    sum += amp;

                    if (time > maxTime) {
                        maxTime = time;
                    }
                }
            }

            if (sum != shaper.MaxAmp) {
                fprintf(stderr,
                        "Sum of amplitudes (%d) does not equal AmpSum (%d).\n",
                        sum,
                        shaper.MaxAmp);
                return ((int)returnValue);
            }

            if (maxTime != shaper.MaxDelay) {
                fprintf(stderr,
                        "Maximum time (%d) does not equal Steps (%d).\n",
                        maxTime,
                        shaper.MaxDelay);
                return (returnValue);
            }

            if (axis < 0) {
                fprintf(stderr,
                        "No #Axis statement.\n");
                return (returnValue);
            }
            else {
                shaper.Index = shaperCount;

                if (shaperCount >= MEIPreFilterCountMAX) {
                    fprintf(stderr,
                            "Too many shapers.\n");
                    break;
                }
                else {
                    double  scale;

                    /* Rescale for XMP filter normalization */
                    scale = (double)MEIXmpFILTER_SCALE / (double)shaper.MaxAmp;

                    sum = 0;

                    for (index = 0; index < shaper.Length - 1; index++) {
                        shaper.Amp[index] = (long)(shaper.Amp[index] * scale);
                        sum += shaper.Amp[index];
                    }
                    shaper.Amp[shaper.Length - 1] = MEIXmpFILTER_SCALE - sum;

                    loadShaper(control,
                               &shaper);

                    fprintf(stderr,
                            "Axis %d shaper loaded.\n",
                            axis);

                    shaperCount++;
                }
            }
        }
    }
    fclose(fp);

    returnValue = mpiControlDelete(control);
    msgCHECK(returnValue);

    return ((int)returnValue);
}

long
    loadShaper(MPIControl   control,
               SHAPER       *shaper)
{
    MEIXmpData          *firmware;
    MEIXmpBufferData    *external;

    MPIAxis axis;

    MPIControlConfig    controlConfig;
    MEIControlConfig    controlConfigXmp;
    MPIAxisConfig       axisConfig;
    MEIAxisConfig       axisConfigXmp;

    long    returnValue;

    long    index;
    long    axisNumber;
    long    length;

    /* Get pointer to XMP firmware */
    returnValue =
        mpiControlMemory(control,
                         (void **)&firmware,
                         (void **)&external);
    msgCHECK(returnValue);

    index      = shaper->Index;
    axisNumber = shaper->Axis;

    axis =
        mpiAxisCreate(control,
                      axisNumber);
    msgCHECK(mpiAxisValidate(axis));

    returnValue =
        mpiAxisConfigGet(axis,
                         &axisConfig,
                         &axisConfigXmp);
    msgCHECK(returnValue);

    /* Set command position to Filter input (disable filter) */
    axisConfigXmp.PreFilter.PositionDelta = &firmware->Axis[axisNumber].TC.PositionDelta;
    axisConfigXmp.PreFilter.Velocity = &firmware->Axis[axisNumber].TC.Velocity;
   axisConfigXmp.PreFilter.Delay = 0;

    returnValue =
        mpiAxisConfigSet(axis,
                         &axisConfig,
                         &axisConfigXmp);
    msgCHECK(returnValue);

    returnValue =
        mpiControlConfigGet(control,
                            &controlConfig,
                            &controlConfigXmp);
    msgCHECK(returnValue);

    controlConfigXmp.preFilter[index].axisNumber   = axisNumber;
   controlConfigXmp.preFilter[index].form       = MEIPreFilterFormSHAPING;
    controlConfigXmp.preFilter[index].length    = shaper->Length;

    for (length = 0; length < shaper->Length; length++) {
        controlConfigXmp.preFilter[index].coeff[2 * length] =
            shaper->Time[length];
        controlConfigXmp.preFilter[index].coeff[2 * length + 1] =
            shaper->Amp[length];
    }

    if (controlConfigXmp.preFilterCount < (index + 1)) {
        controlConfigXmp.preFilterCount = (index + 1);
    }

    returnValue =
        mpiControlConfigSet(control,
                            &controlConfig,
                            &controlConfigXmp);
    msgCHECK(returnValue);

    /* Wait for shaper to stabilize */

    meiControlSampleWait(control, shaper->MaxDelay + 2);

    returnValue =
        mpiAxisConfigGet(axis,
                         &axisConfig,
                         &axisConfigXmp);
    msgCHECK(returnValue);

    /* Set command position pointer to Output of shaper */
    axisConfigXmp.PreFilter.PositionDelta = &external->PreFilter[index].OutputDelta;
    axisConfigXmp.PreFilter.Velocity = &external->PreFilter[index].VOutput;
    axisConfigXmp.PreFilter.Delay = shaper->MaxDelay;

    returnValue =
        mpiAxisConfigSet(axis,
                         &axisConfig,
                         &axisConfigXmp);
    msgCHECK(returnValue);

    returnValue = mpiAxisDelete(axis);
    msgCHECK(returnValue);

    return (returnValue);
}


 
      
       Legal Notice  |  Tech Email  |  Feedback
      
Copyright ©
2001-2009 Motion Engineering