.

     

MPI Application Template
template.c
 

motion3.c -- Custom point to point S-curve motion with velocity specified using position.
/* motion3.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.
 */

/*

:Custom point to point S-curve motion with velocity specified using position.

This program demonstrates how to generate a point to point move using a custom
 velociy profile. The motion profile is specified by a structure called
 moveData[...] containing positions, velocities, and jerk percents.
 The motion is commanded using:

  1) mpiMotionStart(...) with the FINAL_VEL attribute, which loads the first
     point and begins motion.

  2) mpiMotionModify(...) with the FINAL_VEL and APPEND attribute, which adds
     the rest of the motion profiles to the end of the first profile.

The resultant motion profile will reach the "midVelocity" at the specified
 "midPosition" and the "endVelocity" at the specified "endPosition."
 MOVE_SEGMENTS is defined by the size of moveData[...].

The following equation was used to determine accelerations and decelerations.

  acceleration = (velocity1^2 - velocity0^2) / (2 * (position1 - position2))

There is a minimal error checking in this sample.

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 <math.h>

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

#define AXIS_NUMBER         (0)     /* Axis to be moved */
#define MOTION_NUMBER       (0)     /* Motion supervisor number */
#define JERK_PERCENT        (100.0)

long    axisNumber      = 0;
long    motionNumber    = 0;

struct {
    double  midposition;
    double  endPosition;
    double  midVelocity;
    double  endVelocity;
    double  jerkPercent;

} moveData[] =  {

/*  midposition,    endPosition,    midVel,     endVel, jerkPercent */
    {1200,          3000,           27000.0,    3000.0,     JERK_PERCENT,   },
    {4200,          6000,           15000.0,    3000.0,     JERK_PERCENT,   },
    {8900,          11300,          44000.0,    0.0,        JERK_PERCENT,   },

/*
    {6000,          0,              15000.0,    0.0,        JERK_PERCENT,   },
   Add additional points like the above line
*/

};



Arg argList[] = {
    {   NULL,       ArgTypeINVALID, NULL,   }
};

#define MOVE_SEGMENTS (sizeof(moveData)/sizeof(moveData[0]))

long    parseCommandLine(int                argc,
                         char               *argv[],
                         MPIControlType     *controlType,
                         MPIControlAddress  *controlAddress);

long    createObjects(MPIControl            *control,
                      MPIAxis               *axis,
                      MPIMotion             *motion,
                      MPIControlType        *controlType,
                      MPIControlAddress     *controlAddress,
                      long                  axisNumber);

long    deleteObjects(MPIControl    *control,
                      MPIAxis       *axis,
                      MPIMotion     *motion);


int main(int    argc,
         char   *argv[])
{

    MPIControl          control;        /* Motion controller object handle */
    MPIMotion           motion;         /* Motion object handle */
    MPIAxis             axis;           /* Axis object handle */
    MPIMotionParams     params;         /* Motion parameters */
    MPITrajectory       trajectory;     /* Motion trajectory */
    MEIMotionAttributes attributes;     /* Motion attributes */
    MPIMotionAttrMask   attrMask;       /* Motion attributes mask */
    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    long    returnValue;
    long    segmentCount;
    double  endPosition;

    parseCommandLine(argc,
                     argv,
                     &controlType,
                     &controlAddress);

    createObjects(&control,
                  &axis,
                  &motion,
                  &controlType,
                  &controlAddress,
                  axisNumber);

    segmentCount = 0;
    moveData[-1].endVelocity = 0;
    moveData[-1].endPosition = 0;

    while (segmentCount < MOVE_SEGMENTS){
        /* Setup new motion parameters */
        attrMask =
            MPIMotionAttrMaskAPPEND |
            MEIMotionAttrMaskFINAL_VEL;

        endPosition              =  moveData[segmentCount].endPosition;
        trajectory.velocity      =  moveData[segmentCount].midVelocity;
        attributes.finalVelocity = &moveData[segmentCount].endVelocity;

    /*
       Acceleration = (velocity1^2 - velocity0^2) / (2 * (position1 - position2))
       Point 0 is 'endPosition' of the previous move. Point 1 is 'midposition'.
    */

        trajectory.acceleration  =
            fabs(((pow(moveData[segmentCount].midVelocity, 2) -
            pow(moveData[segmentCount - 1].endVelocity, 2)) /
            (2 * (moveData[segmentCount].midposition -
            moveData[segmentCount - 1].endPosition))));

    /*
       Acceleration = (velocity1^2 - velocity0^2) / (2 * (position1 - position2))
       Point 0 is 'midposition'. Point 1 is 'endPosition'.
    */

        trajectory.deceleration  =
            fabs(((pow(moveData[segmentCount].endVelocity, 2) -
            pow(moveData[segmentCount].midVelocity, 2)) /
            (2 * (moveData[segmentCount].endPosition -
            moveData[segmentCount].midposition))));

        trajectory.jerkPercent   =  moveData[segmentCount].jerkPercent;

        if (!segmentCount){
            /* First point */
            params.sCurve.position   = &endPosition;
            params.sCurve.trajectory = &trajectory;
            params.external = &attributes;
            /* Start motion */
            returnValue =
                mpiMotionStart(motion,
                               MPIMotionTypeS_CURVE | attrMask,
                               &params);
            msgCHECK(returnValue);
        }
        else{   /* Not first point */
            /* Modify motion */
            returnValue =
                mpiMotionModify(motion,
                                MPIMotionTypeS_CURVE | attrMask,
                                &params);
            msgCHECK(returnValue);
        }
        segmentCount++;
    }

    deleteObjects(&control,
                  &axis,
                  &motion);

    return (returnValue);
}


long    parseCommandLine(int                argc,
                         char               *argv[],
                         MPIControlType     *controlType,
                         MPIControlAddress  *controlAddress)
{
    long    argIndex;

    /* Parse command line for Control type and address */
    argIndex =
        argControl(argc,
                   argv,
                   controlType,
                   controlAddress);

    /* Parse command line for application-specific arguments */
    while (argIndex < argc) {
        long    argIndexNew;

        argIndexNew = argSet(argList, argIndex, argc, argv);

        if (argIndexNew <= argIndex) {
            argIndex = argIndexNew;
            break;
        }
        else {
            argIndex = argIndexNew;
        }
    }

    /* Check for unknown/invalid command line arguments */
    if ((argIndex < argc) ||
        (axisNumber > (MEIXmpMAX_Axes)) ||
        (motionNumber  >= MEIXmpMAX_MSs)) {
        meiPlatformConsole("usage: %s %s\n"
                           "\t\t[-axis # (0 .. %d)]\n"
                           "\t\t[-motion # (0 .. %d)]\n"
                           "\t\t[-type # (0 .. %d)]\n",
                            argv[0],
                            ArgUSAGE,
                            MEIXmpMAX_Axes,
                            MEIXmpMAX_MSs - 1,
                            MEIMotionTypeLAST - 1);
        exit(MPIMessageARG_INVALID);
    }

    return 0;
}


long    createObjects(MPIControl            *control,
                      MPIAxis               *axis,
                      MPIMotion             *motion,
                      MPIControlType        *controlType,
                      MPIControlAddress     *controlAddress,
                      long                  axisNumber)
{

    long    returnValue;

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

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

    /* Create axis object*/
    *axis =
        mpiAxisCreate(*control,
                      AXIS_NUMBER);     /* Axis number */
    msgCHECK(mpiAxisValidate(*axis));

    /* Create motion object, append axis */
    *motion =
        mpiMotionCreate(*control,
                        MOTION_NUMBER,  /* Motion supervisor number */
                        *axis);         /* Axis object handle */
    msgCHECK(mpiMotionValidate(*motion));

    returnValue =
        mpiMotionAxisListSet(*motion,
                             1,
                             axis);
    msgCHECK(returnValue);

    return returnValue;
}


long    deleteObjects(MPIControl    *control,
                      MPIAxis       *axis,
                      MPIMotion     *motion)
{

    long returnValue;

    /* Object clean-up */
    returnValue = mpiMotionDelete(*motion);
    msgCHECK(returnValue);

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

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

    return returnValue;
}


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