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 */
MPIMotionAttributes 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 |
MPIMotionAttrMaskFINAL_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,
¶ms);
msgCHECK(returnValue);
}
else{ /* Not first point */
/* Modify motion */
returnValue =
mpiMotionModify(motion,
MPIMotionTypeS_CURVE | attrMask,
¶ms);
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 > (MPIXmpMAX_Axes)) ||
(motionNumber >= MPIXmpMAX_MSs)) {
mpiPlatformConsole("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,
MPIXmpMAX_Axes,
MPIXmpMAX_MSs - 1,
MPIMotionTypeLAST - 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;
}
|
