motion2.c -- Two-axis motion, with synchronized and coordinated S-Curve profiles.
/* motion2.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/motion2.c 18 7/23/01 2:36p Kevinh $";
#endif
/*
: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. Simple point to point motion is commanded
using S-Curve (or Trapezoidal) profile.
When motion is commanded to point 0, using the MPIMotionAttrMaskSYNC_START
attribute, the controller starts the motion profiles for both axes at the
same time. Each axis uses it's own MPITrajectory, independently reaching
their target positions at different times.
When motion is commanded to point 1, (no attributes), the controller
starts and completes the motion profiles for the axes at the same time.
Each axis shares a single MPITrajectory, by breaking the vector trajectory
values into component trajectory values based on the ratio of the axis
distances. The result is point to point linear coordinated move for the
axes, specified by a target "position" for each axis and a single vector
"trajectory" for the axes.
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.
If an error condition occurs during motion, the program clears the error
using mpiMotionAction(motion, MPIActionRESET).
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 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 motion2Main
argMainRENAME(main, motion2)
#endif
#define MOTION_COUNT (2)
#define AXIS_COUNT (2)
/* Command line arguments and defaults */
long axisNumber[AXIS_COUNT] = { 0, 1, };
long motionNumber = 0;
MPIMotionType motionType = MPIMotionTypeS_CURVE;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ "-type", ArgTypeLONG, &motionType, },
{ NULL, ArgTypeINVALID, NULL, }
};
/* Motion Parameters */
double position[MOTION_COUNT][AXIS_COUNT] = {
{ 2000.0, 20000.0, },
{ 0.0, 0.0, },
};
MPITrajectory trajectory[MOTION_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[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
};
MPIMotionTrapezoidal trapezoidal[MOTION_COUNT] = {
{ &trajectory[0][0], &position[0][0], },
{ &trajectory[1][0], &position[1][0], },
};
MPIMotionVelocity velocity[MOTION_COUNT] = {
{ &trajectory[0][0], },
{ &trajectory[1][0], },
};
long
motionIdle(MPIMotion motion,
MPIStatus *status);
int
main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
MPIAxis axisX; /* X axis */
MPIAxis axisY; /* Y axis */
MPIMotion motion; /* motion object */
long returnValue; /* return value from library */
long index;
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[0] > (MEIXmpMAX_Axes - AXIS_COUNT)) ||
(motionNumber >= MEIXmpMAX_MSs) ||
(motionType < MPIMotionTypeFIRST) ||
(motionType >= MEIMotionTypeLAST)) {
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 - AXIS_COUNT,
MEIXmpMAX_MSs - 1,
MEIMotionTypeLAST - 1);
exit(MPIMessageARG_INVALID);
}
switch (motionType) {
case MPIMotionTypeS_CURVE:
case MPIMotionTypeTRAPEZOIDAL:
case MPIMotionTypeVELOCITY: {
break;
}
default: {
meiPlatformConsole("%s: %d: motion type not available\n",
argv[0],
motionType);
exit(MPIMessageUNSUPPORTED);
break;
}
}
axisNumber[1] = axisNumber[0] + 1;
/* Create motion controller object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize motion controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
/* Create X axis object using axis number 0 on controller*/
axisX =
mpiAxisCreate(control,
axisNumber[0]);
msgCHECK(mpiAxisValidate(axisX));
/* Create Y axis object using axis number 1 on controller */
axisY =
mpiAxisCreate(control,
axisNumber[1]);
msgCHECK(mpiAxisValidate(axisY));
/* Create motion object */
/* Append X axis to motion */
motion =
mpiMotionCreate(control,
motionNumber,
axisX);
msgCHECK(mpiMotionValidate(motion));
/* Append Y axis to motion */
returnValue =
mpiMotionAxisAppend(motion,
axisY);
msgCHECK(returnValue);
/* Loop repeatedly */
index = 0;
while (meiPlatformKey(MPIWaitPOLL) <= 0) {
long motionDone;
if (returnValue == MPIMessageOK) {
MPIMotionType type;
MPIMotionParams motionParams;
type = motionType;
switch (motionType) {
case MPIMotionTypeS_CURVE: {
if (index == 0) {
type = (MPIMotionType)(type | MPIMotionAttrMaskSYNC_START);
}
motionParams.sCurve = sCurve[index];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
if (index == 0) {
type = (MPIMotionType)(type | MPIMotionAttrMaskSYNC_START);
}
motionParams.trapezoidal = trapezoidal[index];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[index];
break;
}
default: {
meiASSERT(FALSE);
break;
}
}
printf("\nMotionStart...");
returnValue =
mpiMotionStart(motion,
type,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion,
type,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
}
/* Poll status until motion done */
motionDone = FALSE;
while (motionDone == FALSE) {
MPIStatus status;
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state) {
case MPIStateIDLE: {
motionDone = TRUE;
motionIdle(motion,
&status);
/* Wait for the motor to settle */
meiPlatformSleep(300); /* msec */
break;
}
case MPIStateERROR: {
motionDone = TRUE;
/* Clear any error condition(s) */
returnValue =
mpiMotionAction(motion,
MPIActionRESET);
msgCHECK(returnValue);
/* Wait for reset to take effect */
meiPlatformSleep(100); /* msec */
break;
}
default: {
break;
}
}
}
if (++index >= MOTION_COUNT) {
index = 0;
}
}
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisY);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisX);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
long motionIdle(MPIMotion motion,
MPIStatus *status)
{
long returnValue;
double actual[AXIS_COUNT];
double command[AXIS_COUNT];
long index;
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 < AXIS_COUNT; index++) {
printf("\taxis[%d] position: command %11.3lf\tactual %11.3lf\n",
index,
command[index],
actual[index]);
}
return (returnValue);
}