motion2.c -- Two-axis motion, with synchronized and coordinated S-Curve
profiles.
/* motion2.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.
*/
/*
:Two-axis motion, with synchronized and coordinated S-Curve profiles.
This sample demonstrates how to create a two-axis motion system, using
a single motion object. Multiple point to point motion is commanded
using S-Curve profile until any key is pressed.
The MPIMotionAttrMaskSYNC_START and MPIMotionAttrMaskSYNC_END attribute is used.
The controller starts the motion profiles for both axes at the same time and
reaches their target positions at the same time.
During motion, the motion status is polled from the controller. When both
axes complete their motions (state = IDLE), the command and actual positions,
and motion status information is displayed.
Note: When multiple axes are associated with a motion supervisor, the
controller automatically combines the individual axis and motor status
into the motion status. Thus, if a Stop, E-Stop or Abort action occurs
on one axis, the event will be propogated automatically to the other axes.
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 "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
/* Defaults */
#define POINT_COUNT (2)
#define AXIS_COUNT (2)
#define MOTION_NUMBER (0)
#define AXIS_X_NUMBER (0)
/* Perform basic command line parsing. (-control -server -port -trace) */
void parseCommandLine(int argc,
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress,
long axisCount,
long *axisX_Number,
long *motionNumber)
{
long argIndex;
Arg argList[] =
{
/* Determining axisX, then axisY is axisX + 1 */
{ "-axis", ArgTypeLONG, axisX_Number, },
/* Determining motion supervisor number for the axisX and axisY to relate */
{ "-motion", ArgTypeLONG, motionNumber, },
/* End of argument list (null terminator) */
{ NULL, ArgTypeINVALID, NULL, }
};
/* 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) ||
(*axisX_Number > (MPIXmpMAX_Axes - axisCount)) ||
(*motionNumber >= MPIXmpMAX_MSs))
{
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
/*Max.#of axis = #of motors - 2*/
MPIXmpMAX_Axes - axisCount,
MPIMotionTypeLAST - 1);
exit(MPIMessageARG_INVALID);
}
}
/* Perform program initialization i.e. MPI object creation */
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIMotion *motion,
long motionNumber,
MPIAxis *axisX,
long axisNumberX,
MPIAxis *axisY,
long axisNumberY)
{
long returnValue;
/* Create motion controller object */
*control =
mpiControlCreate(controlType,
controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
/* Create axis object */
*axisX =
mpiAxisCreate(*control,
axisNumberX);
msgCHECK(mpiAxisValidate(*axisX));
*axisY =
mpiAxisCreate(*control,
axisNumberY);
msgCHECK(mpiAxisValidate(*axisY));
/* Create motion supervisor object and append axisX to it */
*motion =
mpiMotionCreate(*control,
motionNumber,
*axisX);
msgCHECK(mpiMotionValidate(*motion));
/* Append axisY to motion */
returnValue =
mpiMotionAxisAppend(*motion,
*axisY);
msgCHECK(returnValue);
}
/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl *control,
MPIMotion *motion,
MPIAxis *axisX,
MPIAxis *axisY)
{
long returnValue;
/* Delete motion supervisor object */
returnValue =
mpiMotionDelete(*motion);
msgCHECK(returnValue);
/* Delete axisX object */
returnValue =
mpiAxisDelete(*axisX);
msgCHECK(returnValue);
/* Delete axisY object */
returnValue =
mpiAxisDelete(*axisY);
msgCHECK(returnValue);
/* Delete motion controller object */
returnValue =
mpiControlDelete(*control);
msgCHECK(returnValue);
}
/* Display actual position for each axis */
void displayMotionPosition(MPIMotion motion,
MPIStatus *status,
long axisCount)
{
long returnValue;
long index;
double *actual = (double *) malloc(axisCount * sizeof(double));
double *command = (double *) malloc(axisCount * sizeof(double));
printf("MotionDone: status: state %d action %d eventMask 0x%x\n"
"\tatTarget %d settled %d %s\n",
status->state,
status->action,
status->eventMask,
status->atTarget,
status->settled,
(status->settled == FALSE)
? "=== NOT SETTLED ==="
: "");
returnValue =
mpiMotionPositionGet(motion,
actual,
command);
msgCHECK(returnValue);
/* Display axis positions */
for (index = 0; index < axisCount; index++)
{
fprintf(stderr,"\taxis[%d] position: command %11.3lf\tactual %11.3lf\n",
index,
command[index],
actual[index]);
}
free(actual);
free(command);
}
/* Wait and check for motion status */
void waitForMotionDone(MPIMotion motion,
long axisCount)
{
MPIStatus status;
long motionDone = FALSE;
long returnValue = MPIMessageOK;
/* Poll status until motion done */
while (motionDone == FALSE)
{
/* Get the motion supervisor status */
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state)
{
case MPIStateSTOPPING:
case MPIStateMOVING:
{
/* Sleep for 10ms and give up control to other threads */
mpiPlatformSleep(10);
break;
}
case MPIStateIDLE:
case MPIStateSTOPPED:
motionDone = TRUE;
/* Display axis position */
displayMotionPosition(motion,
&status,
axisCount);
/* Wait for the motor to settle */
mpiPlatformSleep(300); /* msec */
break;
case MPIStateERROR:
case MPIStateSTOPPING_ERROR:
{
fprintf(stderr, "ERROR: Axis in error state. Cannot continue motion\n");
exit(-1);
}
default:
{
/* Unknown State */
fprintf(stderr, "Unknown state from mpiMotionStatus.\n");
msgCHECK(MPIMessageFATAL_ERROR);
break;
}
}
}
}
/* Command multiple SCurve motion until a key is pressed*/
long doMultipleSCurveMove(MPIMotion motion,
long pointCount,
long axisCount)
{
long index = 0;
long returnValue; /* MPI library return value */
MPIMotionParams params; /* Motion parameters */
MPIMotionType motionType = MPIMotionTypeS_CURVE | MPIMotionAttrMaskSYNC_START | MPIMotionAttrMaskSYNC_END;
double position[POINT_COUNT][AXIS_COUNT] =
{
{ 2000.0, 20000.0, },
{ 0.0, 0.0, },
};
MPITrajectory trajectory[POINT_COUNT][AXIS_COUNT] =
{
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
{ /* velocity accel decel jerkPercent */
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
{ 10000.0, 1000000.0, 1000000.0, 0.0, },
},
};
MPIMotionSCurve sCurve[POINT_COUNT] =
{
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
};
/* Return immediately when an input character is available */
while (mpiPlatformKey(MPIWaitPOLL) <= 0)
{
params.sCurve = sCurve[index];
/* Start motion */
returnValue =
mpiMotionStart(motion,
motionType,
¶ms);
msgCHECK(returnValue);
/* Wait until the motion is completed */
waitForMotionDone(motion, axisCount);
if (++index >= pointCount)
{
index = 0;
}
}
return returnValue;
}
int main(int argc,
char *argv[])
{
MPIControl control; /* motion controller object handle */
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIMotion motion; /* motion object handle */
MPIAxis axisX; /* axisX object handle */
MPIAxis axisY; /* axisY object handle */
long motionNumber = MOTION_NUMBER;
long axisX_Number = AXIS_X_NUMBER;
long axisY_Number;
/* Perform basic command line parsing. (-control -server -port -trace) */
parseCommandLine(argc,
argv,
&controlType,
&controlAddress,
AXIS_COUNT,
&axisX_Number,
&motionNumber);
axisY_Number = axisX_Number +1;
/* Create and initialize MPI objects */
programInit(&control,
controlType,
&controlAddress,
&motion,
motionNumber,
&axisX,
axisX_Number,
&axisY,
axisY_Number);
/* Command simple SCurve motion */
doMultipleSCurveMove(motion,
POINT_COUNT,
AXIS_COUNT);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(&control,
&motion,
&axisX,
&axisY);
return MPIMessageOK;
}
|
