shape.c -- Configure Trajectory Shaping Filters
/* shape.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.
*/
/*
:Configure Trajectory Shaping Filters
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 <string.h>
#include <ctype.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#if defined(ARG_MAIN_RENAME)
#define main shapeMain
argMainRENAME(main, shape)
#endif
typedef struct SHAPER {
long Index;
long Axis;
long Length;
long MaxDelay;
long MaxAmp;
double TimeStep;
long Time[MPIPreFilterCoeffsMAX];
long Amp[MPIPreFilterCoeffsMAX];
} SHAPER;
long
loadShaper(MPIControl control,
SHAPER *shaper);
int
main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
MPIControlConfig config;
SHAPER shaper;
char *fileName;
FILE *fp;
char buffer[256];
long shaperCount;
long shaperCountOld;
long returnValue;
MPIControlType controlType;
MPIControlAddress controlAddress;
long argIndex;
argIndex =
argControl(argc,
argv,
&controlType,
&controlAddress);
fileName =
(argIndex >= argc)
? "shape.txt"
: argv[argIndex++];
if (argIndex < argc) {
mpiPlatformConsole("usage: %s %s\n"
"\t\t[fileName (%s)]\n",
argv[0],
ArgUSAGE,
"shape.txt");
exit(0);
}
/* Create motion controller object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize motion controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
fp = fopen(fileName, "r");
if (fp == NULL) {
fprintf(stderr,
"shape.txt not found.\n");
exit(2);
}
returnValue =
mpiControlConfigGet(control,
&config,
NULL);
msgCHECK(returnValue);
shaperCount = 0;
shaperCountOld = -1;
while (fgets(buffer, sizeof(buffer), fp) != NULL) {
long axis;
long length;
long maxAmp;
long maxDelay;
char *ptr;
if (shaperCount > shaperCountOld) {
shaper.Axis = -1; /* Make sure all params are read from file */
shaper.MaxAmp = -1;
shaper.Length = 0;
shaper.MaxDelay = -1;
shaper.TimeStep = 1.0 / config.sampleRate;
shaperCountOld = shaperCount;
}
for (ptr = buffer; *ptr != '\0'; ptr++) {
char byte = *ptr;
if (isupper(byte)) {
*ptr = (char)tolower(byte);
}
}
if (sscanf(buffer, "#axis = %d", &axis) == 1) {
shaper.Axis = axis;
continue;
}
if (sscanf(buffer, "#impulses = %d", &length) == 1) {
shaper.Length = length;
continue;
}
if (sscanf(buffer, "#ampsum = %d", &maxAmp) == 1) {
shaper.MaxAmp = maxAmp;
continue;
}
if (sscanf(buffer, "#steps = %d", &maxDelay) == 1) {
shaper.MaxDelay = maxDelay;
continue;
}
if (strncmp(buffer, "#time", 5) == 0) {
long sum;
long maxTime;
long index;
sum = 0;
maxTime = 0;
for (index = 0; index < shaper.Length; index++) {
long time;
long amp;
if (fgets(buffer, sizeof(buffer), fp) == NULL) {
fprintf(stderr,
"Unexpected end-of-file\n");
return ((int)returnValue);
}
if (sscanf(buffer, "%d %d", &time, &) == 2) {
shaper.Time[index] = time;
shaper.Amp[index] = amp;
sum += amp;
if (time > maxTime) {
maxTime = time;
}
}
}
if (sum != shaper.MaxAmp) {
fprintf(stderr,
"Sum of amplitudes (%d) does not equal AmpSum (%d).\n",
sum,
shaper.MaxAmp);
return ((int)returnValue);
}
if (maxTime != shaper.MaxDelay) {
fprintf(stderr,
"Maximum time (%d) does not equal Steps (%d).\n",
maxTime,
shaper.MaxDelay);
return (returnValue);
}
if (axis < 0) {
fprintf(stderr,
"No #Axis statement.\n");
return (returnValue);
}
else {
shaper.Index = shaperCount;
if (shaperCount >= MPIPreFilterCountMAX) {
fprintf(stderr,
"Too many shapers.\n");
break;
}
else {
double scale;
/* Rescale for XMP filter normalization */
scale = (double)MPIXmpFILTER_SCALE / (double)shaper.MaxAmp;
sum = 0;
for (index = 0; index < shaper.Length - 1; index++) {
shaper.Amp[index] = (long)(shaper.Amp[index] * scale);
sum += shaper.Amp[index];
}
shaper.Amp[shaper.Length - 1] = MPIXmpFILTER_SCALE - sum;
loadShaper(control,
&shaper);
fprintf(stderr,
"Axis %d shaper loaded.\n",
axis);
shaperCount++;
}
}
}
}
fclose(fp);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
long
loadShaper(MPIControl control,
SHAPER *shaper)
{
MPIXmpData *firmware;
MPIXmpBufferData *external;
MPIAxis axis;
MPIControlConfig controlConfig;
MPIControlConfig controlConfigXmp;
MPIAxisConfig axisConfig;
MPIAxisConfig axisConfigXmp;
long returnValue;
long index;
long axisNumber;
long length;
/* Get pointer to XMP firmware */
returnValue =
mpiControlMemory(control,
(void **)&firmware,
(void **)&external);
msgCHECK(returnValue);
index = shaper->Index;
axisNumber = shaper->Axis;
axis =
mpiAxisCreate(control,
axisNumber);
msgCHECK(mpiAxisValidate(axis));
returnValue =
mpiAxisConfigGet(axis,
&axisConfig,
&axisConfigXmp);
msgCHECK(returnValue);
/* Set command position to Filter input (disable filter) */
axisConfigXmp.PreFilter.PositionDelta = &firmware->Axis[axisNumber].TC.PositionDelta;
axisConfigXmp.PreFilter.Velocity = &firmware->Axis[axisNumber].TC.Velocity;
axisConfigXmp.PreFilter.Delay = 0;
returnValue =
mpiAxisConfigSet(axis,
&axisConfig,
&axisConfigXmp);
msgCHECK(returnValue);
returnValue =
mpiControlConfigGet(control,
&controlConfig,
&controlConfigXmp);
msgCHECK(returnValue);
controlConfigXmp.preFilter[index].axisNumber = axisNumber;
controlConfigXmp.preFilter[index].form = MPIPreFilterFormSHAPING;
controlConfigXmp.preFilter[index].length = shaper->Length;
for (length = 0; length < shaper->Length; length++) {
controlConfigXmp.preFilter[index].coeff[2 * length] =
shaper->Time[length];
controlConfigXmp.preFilter[index].coeff[2 * length + 1] =
shaper->Amp[length];
}
if (controlConfigXmp.preFilterCount < (index + 1)) {
controlConfigXmp.preFilterCount = (index + 1);
}
returnValue =
mpiControlConfigSet(control,
&controlConfig,
&controlConfigXmp);
msgCHECK(returnValue);
/* Wait for shaper to stabilize */
mpiControlSampleWait(control, shaper->MaxDelay + 2);
returnValue =
mpiAxisConfigGet(axis,
&axisConfig,
&axisConfigXmp);
msgCHECK(returnValue);
/* Set command position pointer to Output of shaper */
axisConfigXmp.PreFilter.PositionDelta = &external->PreFilter[index].OutputDelta;
axisConfigXmp.PreFilter.Velocity = &external->PreFilter[index].VOutput;
axisConfigXmp.PreFilter.Delay = shaper->MaxDelay;
returnValue =
mpiAxisConfigSet(axis,
&axisConfig,
&axisConfigXmp);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis);
msgCHECK(returnValue);
return (returnValue);
}
|
