/* motMod1.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/motmod1.c 4     7/23/01 2:36p Kevinh $";
#endif

/*

:Point to Point trapezoidal profile motion with end point modification.

This is a simple program to demonstrate how to start a trapezoidal profile
 motion on a single axis.  During the execution of the motion, the target
 position (end point), velocity, accel, decel are modified.  The controller
 automatically re-calculates the motion profile, using the modified
 trajectory parameters.

The mpiMotionModify(...) function modifies a presently executing motion
 profile.  If mpiMotionModify(...) is called when no motion is executing,
 the error code, MPIMotionMessageIDLE will be returned, and no motion
 will be commanded.  This error code let's the application know that there
 was no motion to be modified.

Warning!  This is a sample program to assist in the integration 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 "stdmpi.h"
#include "stdmei.h"

#include "apputil.h"

#if defined(ARG_MAIN_RENAME)
#define main    motMod1Main

argMainRENAME(main, motMod1)
#endif

#define MOTION_NUMBER       (0)
#define AXIS_NUMBER         (0)

/* Motion Start Parameters */
#define TARGET_POSITION     (10000.0)
#define VEL                 (10000.0)
#define ACCEL               (100000.0)
#define DECEL               (100000.0)

/* Motion Modify Parameters */
#define MODIFY_POSITION     (-1000.0)   /* new target position */
#define MODIFY_VEL          (5000.0)    /* new velocity */
#define MODIFY_DECEL        (50000.0)   /* new deceleration */

#define WAIT_TIME           (500)       /* delay to modify move, (msec) */


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

    long    returnValue;

    MPIMotionParams params;     /* motion parameters */
    MPITrajectory   trajectory; /* motion trajectory */
    double          position;   /* final target position */
    MPIControlType      controlType;
    MPIControlAddress   controlAddress;

    long    argIndex;

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

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


    /* Create motion controller object*/
    control =
        mpiControlCreate(MPIControlTypeDEFAULT,
                         NULL);
    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));

    /* Set up motion parameters */
    trajectory.velocity     = VEL;          /* counts per sec */
    trajectory.acceleration = ACCEL;        /* counts per sec * sec */
    trajectory.deceleration = DECEL;

    position = TARGET_POSITION;     /* target position (counts) */

    params.trapezoidal.trajectory   = &trajectory;
    params.trapezoidal.position     = &position;

    printf("\nMotionStart...\n");

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

    meiPlatformSleep(WAIT_TIME);    /* delay */

    /* Set up new motion parameters */
    trajectory.velocity     = MODIFY_VEL;       /* counts per sec */
    trajectory.acceleration = ACCEL;            /* use same acceleration */
    trajectory.deceleration = MODIFY_DECEL;     /* counts per sec * sec */

    /* New target position (counts) */
    position = MODIFY_POSITION;

    printf("\nMotionModify...\n");

    /* Modify the executing motion profile */
    returnValue =
        mpiMotionModify(motion,
                        MPIMotionTypeTRAPEZOIDAL,
                        &params);
    msgCHECK(returnValue);

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

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

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

    return ((int)returnValue);
}