/* motion1.c */

/* Copyright(c) 1991-2002 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.
 */

#if defined(MEI_RCS)
static const char MEIAppRCS[] =
    "$Header: /MainTree/XMPLib/XMP/app/motion1.c 12    7/18/01 9:32a Kevinh $";
#endif

#if defined(ARG_MAIN_RENAME)
#define main    motion1Main

argMainRENAME(main, motion1)
#endif

/*

:Perform point to point trapezoidal profile motion.

This is a simple program to demonstrate how to start a trapezoidal profile
 motion on a single axis.  There is a minimal error checking in this sample.

Warning!  This is a sample program to assist in the longegration of the
 XMP 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 MOTION_NUMBER   (0)
#define AXIS_NUMBER     (0)

#define GOAL_POSITION   (50000)
#define VELOCITY        (5000)
#define ACCELERATION    (10000)
#define DECELERATION    (20000)


/* Perform basic command line parsing. (-control -server -port -trace) */
void basicParsing(int                    argc,
                  char                  *argv[],
                  MPIControlType        *controlType,
                  MPIControlAddress     *controlAddress)
{
    long argIndex;

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

    /* Check for unknown/invalid command line arguments */
    if (argIndex < argc) {
        fprintf(stderr,"usage: %s %s\n", argv[0], ArgUSAGE);
        exit(MPIMessageARG_INVALID);
    }
}


/* Create and initialize MPI objects */
void programInit(MPIControl         *control,
                 MPIControlType      controlType,
                 MPIControlAddress  *controlAddress,
                 MPIMotion          *motion,
                 long                motionNumber,
                 MPIAxis            *axis,
                 long                axisNumber)
{
    long            returnValue;


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

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

    /* Create axis object */
    *axis =
        mpiAxisCreate(*control,
                      axisNumber);
    msgCHECK(mpiAxisValidate(*axis));

    /* Create motion supervisor object with axis */
    *motion =
        mpiMotionCreate(*control,
                        motionNumber,       /* motion supervisor number */
                        *axis);             /* axis object handle */
    msgCHECK(mpiMotionValidate(*motion));
}


/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl      *control,
                    MPIMotion       *motion,
                    MPIAxis         *axis)
{
    long    returnValue;


    /* Delete motion supervisor object */
    returnValue =
        mpiMotionDelete(*motion);
    msgCHECK(returnValue);

    /* Delete axis object */
    returnValue =
        mpiAxisDelete(*axis);
    msgCHECK(returnValue);

    /* Delete motion controller object */
    returnValue =
        mpiControlDelete(*control);
    msgCHECK(returnValue);
}


/* Command simple trapezoidal motion */
long simpleTrapMove(MPIMotion   motion,
                    double      goalPosition,
                    double      velocity,
                    double      acceleration,
                    double      deceleration)
{
    MPIMotionParams     params;     /* Motion parameters      */
    MPITrajectory       trajectory; /* Trajectory information */

    long returnValue;               /* MPI library return value */


    /* Setup trajectory structure */
    trajectory.velocity     = velocity;
    trajectory.acceleration = acceleration;
    trajectory.deceleration = deceleration;

    /* Setup parameters structure */
    params.trapezoidal.trajectory   = &trajectory;
    params.trapezoidal.position     = &goalPosition;

    /* Start motion */
    returnValue =
        mpiMotionStart(motion,
                       MPIMotionTypeTRAPEZOIDAL,
                       &params);
    msgCHECK(returnValue);

    return returnValue;
}


int main(int    argc,
         char   *argv[])
{
    MPIControl          control;    /* motion controller object handle */
    MPIControlType      controlType;
    MPIControlAddress   controlAddress;
    MPIMotion           motion;     /* motion object handle */
    MPIAxis             axis;       /* axis object handle */


    /* Perform basic command line parsing. (-control -server -port -trace) */
    basicParsing(argc,
                 argv,
                 &controlType,
                 &controlAddress);

    /* Create and initialize MPI objects */
    programInit(&control,
                controlType,
                &controlAddress,
                &motion,
                MOTION_NUMBER,
                &axis,
                AXIS_NUMBER);

    /* Command simple motion */
    simpleTrapMove(motion,
                   GOAL_POSITION,
                   VELOCITY,
                   ACCELERATION,
                   DECELERATION);

    /* Perform certain cleanup actions and delete MPI objects */
    programCleanup(&control,
                   &motion,
                   &axis);

    return MPIMessageOK;
}