record4.c -- Perform a diagonal two-axis motion and record command
velocity and status.
/* record4.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.
*/
/*
:Perform a diagonal two-axis motion and record command velocity and status.
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"
#if defined(ARG_MAIN_RENAME)
#define main record4Main
argMainRENAME(main, record4)
#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 = MPIMotionTypeTRAPEZOIDAL;
long period = 0; /* every sample */
long recordCount = 100;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber[0], },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ "-type", ArgTypeLONG, &motionType, },
{ "-period", ArgTypeLONG, &period, },
{ "-records", ArgTypeLONG, &recordCount, },
{ NULL, ArgTypeINVALID, NULL, }
};
double position[MOTION_COUNT][AXIS_COUNT] = {
{ 200000.0, 200000.0, },
{ 0.0, 0.0, },
};
MPITrajectory trajectory[MOTION_COUNT][AXIS_COUNT] = {
{ /* velocity accel decel jerkPercent */
{ 100000.0, 1000000.0, 1000000.0, 50.0, },
{ 100000.0, 1000000.0, 1000000.0, 50.0, },
},
{ /* velocity accel decel jerkPercent */
{ 100000.0, 1000000.0, 1000000.0, 50.0, },
{ 100000.0, 1000000.0, 1000000.0, 50.0, },
},
};
/* motion parameters */
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], },
};
int
main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
MPIRecorder recorder; /* data recorder handle */
MPIAxis axisX; /* X axis */
MPIAxis axisY; /* Y axis */
MPIMotion motion; /* motion object */
long returnValue; /* return value from library */
long index;
MPIXmpData *firmware;
long axisCount;
long pointCount;
void *point[MPIXmpMaxRecSize];
MPIRecorderRecord *record;
register MPIRecorderRecord *recordPtr;
long recordCountRemaining;
long recordIndex;
long recordsRead;
MPIControlType controlType;
MPIControlAddress controlAddress;
long argIndex;
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] > (MPIXmpMAX_Axes - AXIS_COUNT)) ||
(motionNumber >= MPIXmpMAX_MSs) ||
(motionType < MPIMotionTypeFIRST) ||
(motionType >= MPIMotionTypeLAST)) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n"
"\t\t[-type # (0 .. %d)]\n"
"\t\t[-period (%d)]\n"
"\t\t[-records (%d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - AXIS_COUNT,
MPIXmpMAX_MSs - 1,
MPIMotionTypeLAST - 1,
period,
recordCount);
exit(MPIMessageARG_INVALID);
}
switch (motionType) {
case MPIMotionTypeS_CURVE:
case MPIMotionTypeTRAPEZOIDAL:
case MPIMotionTypeVELOCITY: {
break;
}
default: {
mpiPlatformConsole("%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);
returnValue =
mpiControlMemory(control,
(void **)&firmware,
NULL);
msgCHECK(returnValue);
/* Create X axis object using axisNumber on controller*/
axisX =
mpiAxisCreate(control,
axisNumber[0]);
msgCHECK(mpiAxisValidate(axisX));
/* Create Y axis object using axisNumber + 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);
axisCount = AXIS_COUNT;
pointCount = 0;
point[pointCount++] = &firmware->SystemData.SampleCounter;
for (index = 0; index < axisCount; index++) {
point[pointCount++] = &firmware->Axis[index].Status;
point[pointCount++] = &firmware->Axis[index].TC.Velocity;
mpiAssert(pointCount <= MPIXmpMaxRecSize);
}
record =
(MPIRecorderRecord *)mpiPlatformAlloc(sizeof(*record) *
recordCount);
mpiAssert(record != NULL);
recorder =
mpiRecorderCreate(control, -1);
msgCHECK(mpiRecorderValidate(recorder));
returnValue =
mpiRecorderRecordConfig(recorder,
MPIRecorderRecordTypePOINT,
pointCount,
point);
msgCHECK(returnValue);
recordPtr = record;
recordCountRemaining = recordCount;
returnValue =
mpiRecorderStart(recorder,
recordCount);
msgCHECK(returnValue);
/* Loop repeatedly */
index = 0;
{
long motionDone;
if (returnValue == MPIMessageOK) {
MPIMotionParams motionParams;
switch (motionType) {
case MPIMotionTypeS_CURVE: {
motionParams.sCurve = sCurve[index];
break;
}
case MPIMotionTypeTRAPEZOIDAL: {
motionParams.trapezoidal = trapezoidal[index];
break;
}
case MPIMotionTypeVELOCITY: {
motionParams.velocity = velocity[index];
break;
}
default: {
mpiAssert(FALSE);
break;
}
}
returnValue =
mpiMotionStart(motion,
motionType,
&motionParams);
if (returnValue != MPIMessageOK) {
printf("mpiMotionStart(0x%x, %d, 0x%x) returns 0x%x: %s\n",
motion,
motionType,
&motionParams,
returnValue,
mpiMessage(returnValue, NULL));
}
}
while (recordCountRemaining > 0)
{
long countGet;
returnValue =
mpiRecorderRecordGet(recorder,
recordCountRemaining,
recordPtr,
&countGet);
msgCHECK(returnValue);
recordCountRemaining -= countGet;
recordPtr += countGet;
}
/* Poll status until motion done */
motionDone = FALSE;
while (motionDone == FALSE)
{
MPIStatus status;
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state)
{
case MPIStateMOVING:
{
break;
}
case MPIStateIDLE:
case MPIStateSTOPPED:
{
motionDone = TRUE;
break;
}
case MPIStateERROR:
{
motionDone = TRUE;
returnValue =
mpiMotionAction(motion,
MPIActionRESET);
msgCHECK(returnValue);
/* Wait for reset to take effect */
mpiPlatformSleep(100);
break;
}
default:
{
mpiAssert(FALSE);
break;
}
}
}
if (++index >= MOTION_COUNT)
{
index = 0;
}
}
recordsRead = recordPtr - record;
printf("Record\tSample\tStat 0\tVel 0\tStat 1\tVel 1\n");
for (recordIndex = 0,
recordPtr = record;
recordIndex < recordsRead;
recordIndex++,
recordPtr++)
{
printf("%d\t%d\t0X%X\t%f\t0X%X\t%f\n",
recordIndex,
recordPtr->point[0],
recordPtr->point[1],
(float)*((float *)&recordPtr->point[2]),
recordPtr->point[3],
(float)*((float *)&recordPtr->point[4]));
}
returnValue = mpiRecorderDelete(recorder);
msgCHECK(returnValue);
returnValue =
mpiPlatformFree(record,
(sizeof(*record) * recordCount));
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisY);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axisX);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
|
