comp.c -- Configure Axis Compensation Tables
/* comp.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/comp.c 10 7/23/01 2:36p Kevinh $";
#endif
/*
:Configure Axis Compensation Tables
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 <string.h>
#include <ctype.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#if defined(ARG_MAIN_RENAME)
#define main compMain
argMainRENAME(main, comp)
#endif
#define COMP_FILENAME "comp.txt"
MEIXmpCompensator Comp;
long CompTable[MEIXmpCompTableSize];
long loadCompTable(MPIControl control,
long index,
MEIXmpCompensator *comp);
int
main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
char *fileName;
FILE *fp;
long compIndex;
long compIndexOld;
long controlIndex;
long tableIndex;
long tableCount = 0;
char buffer[256];
long returnValue;
MPIControlType controlType;
MPIControlAddress controlAddress;
long argIndex;
/* Parse command line for Control type and address */
argIndex =
argControl(argc,
argv,
&controlType,
&controlAddress);
fileName =
(argIndex >= argc)
? COMP_FILENAME
: argv[argIndex++];
if (argIndex < argc) {
meiPlatformConsole("usage: %s %s\n"
"\t\t[fileName (%s)]",
argv[0],
ArgUSAGE,
COMP_FILENAME);
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,
"%s: file not found.\n",
fileName);
exit(2);
}
compIndex = 0;
compIndexOld = -1;
while (fgets(buffer, sizeof(buffer), fp) != NULL) {
long inAxis;
long outAxis;
long minPosition;
long deltaPosition;
long compPoints;
long dimension;
char *ptr;
if (compIndex > compIndexOld) {
Comp.Dimension = -1;
Comp.OutAxis = -1;
Comp.Control[0].InAxis = -1;
Comp.Control[0].DeltaPosition = -1;
Comp.Control[0].CompPoints = -1;
Comp.Control[1].InAxis = -1;
Comp.Control[1].DeltaPosition = -1;
Comp.Control[1].CompPoints = -1;
tableCount = 0;
memset(CompTable, 0, sizeof(CompTable));
Comp.CompTable = CompTable;
compIndexOld = compIndex;
}
for (ptr = buffer; *ptr != '\0'; ptr++) {
char byte = *ptr;
if (islower(byte)) {
*ptr = (char)toupper(byte);
}
}
if (sscanf(buffer, "#DIMENSION = %d", &dimension) == 1) {
Comp.Dimension = dimension;
continue;
}
if (sscanf(buffer, "#INAXIS[%d] = %d",&controlIndex, &inAxis) == 2) {
Comp.Control[controlIndex].InAxis = inAxis;
continue;
}
if (sscanf(buffer, "#OUTAXIS = %d", &outAxis) == 1) {
Comp.OutAxis = outAxis;
continue;
}
if (sscanf(buffer, "#MINPOSITION[%d] = %d",&controlIndex, &minPosition) == 2) {
Comp.Control[controlIndex].MinPosition = minPosition;
continue;
}
if (sscanf(buffer, "#DELTAPOSITION[%d] = %d",&controlIndex, &deltaPosition) == 2) {
Comp.Control[controlIndex].DeltaPosition = deltaPosition;
continue;
}
if (sscanf(buffer, "#COMPPOINTS[%d] = %d",&controlIndex, &compPoints) == 2) {
Comp.Control[controlIndex].CompPoints = compPoints;
continue;
}
if (sscanf(buffer, "#POSITION[%d]", &tableIndex) == 1) {
long index;
long calc_x;
if (Comp.Dimension == 1) {
Comp.Control[1].CompPoints = 1;
Comp.Control[1].InAxis = 0;
Comp.Control[1].DeltaPosition = 0;
}
/* Do some error checking. */
if (compIndex >= MEIXmpMAX_Compensators) {
fprintf(stderr,
"Too many compensators. Max = %d.\n",
MEIXmpMAX_Compensators);
break;
}
if (Comp.OutAxis < 0) {
fprintf(stderr,
"No #OutAxis statement.\n");
break;
}
if (Comp.Dimension < 0) {
fprintf(stderr,
"No #Dimension statement.\n");
break;
}
for (controlIndex = 0; controlIndex < Comp.Dimension; controlIndex++) {
if (Comp.Control[controlIndex].InAxis < 0) {
fprintf(stderr,
"No #InAxis[%d] statement.\n",
controlIndex);
break;
}
if (Comp.Control[controlIndex].CompPoints < 0) {
fprintf(stderr,
"No #CompPoints[%d] statement.\n",
controlIndex);
break;
}
if (Comp.Control[controlIndex].DeltaPosition < 0) {
fprintf(stderr,
"Delta Position[%d] (%d) is invalid.\n",
controlIndex,
Comp.Control[controlIndex].DeltaPosition);
break;
}
}
if ((Comp.Control[0].CompPoints * Comp.Control[1].CompPoints)
> MEIXmpCompTableSize) {
fprintf(stderr,
"Too many compensation points. Max = %d.\n",
MEIXmpCompTableSize);
break;
}
if (tableIndex >= Comp.Control[1].CompPoints) {
fprintf(stderr,
"Compensation table number (%d) out of range [0-%d].\n",
tableIndex,
Comp.Control[1].CompPoints);
break;
}
calc_x = Comp.Control[0].MinPosition;
index = 0;
while (index < Comp.Control[0].CompPoints) {
long x;
long y;
long tableOffset;
if (fgets(buffer, sizeof(buffer), fp) == NULL) {
fprintf(stderr,
"Unexpected end-of-file\n");
break;
}
if (sscanf(buffer, "%d %d", &x, &y) == 2) {
if (x != calc_x) {
fprintf(stderr,
"Table out of order, expected position = %d, read %d\n",
calc_x,
x);
break;
}
tableOffset = index + tableIndex * Comp.Control[0].CompPoints;
Comp.CompTable[tableOffset] = y;
calc_x += Comp.Control[0].DeltaPosition;
index++;
}
}
if (index < Comp.Control[0].CompPoints) {
fprintf(stderr,
"Compensation table number (%d) too small.\n",
tableIndex);
break;
}
else {
tableCount++;
fprintf(stderr,
"Compensation Table %d read.\n",
tableIndex);
}
if (tableCount >= Comp.Control[1].CompPoints) {
returnValue =
loadCompTable(control,
compIndex,
&Comp);
fprintf(stderr,
"Compensator %d is %sconfigured.\n",
compIndex,
(returnValue == MPIMessageOK)
? ""
: "NOT ");
compIndex++;
}
}
}
fclose(fp);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
long loadCompTable(MPIControl control,
long index,
MEIXmpCompensator *comp)
{
MPIControlConfig controlConfig;
MEIControlConfig controlConfigXmp;
long returnValue;
returnValue =
mpiControlConfigGet(control,
&controlConfig,
&controlConfigXmp);
if (returnValue == MPIMessageOK) {
if (controlConfigXmp.Compensator[index].CompTable != NULL) {
fprintf(stderr,
"Warning: controlConfigXmp.Compensator[%d] is already in use\n",
index);
}
controlConfigXmp.compensatorCount = index + 1;
controlConfigXmp.Compensator[index] = *comp;
if (index > 0) {
long *prev_dest;
long prev_size;
prev_dest = controlConfigXmp.Compensator[index - 1].CompTable;
prev_size = controlConfigXmp.Compensator[index - 1].Control[0].CompPoints *
controlConfigXmp.Compensator[index-1].Control[1].CompPoints;
controlConfigXmp.Compensator[index].CompTable = &prev_dest[prev_size];
}
else {
controlConfigXmp.Compensator[index].CompTable =
controlConfigXmp.CompensationTable;
}
meiASSERT(&controlConfigXmp.CompensationTable[MEIXmpCompTableSize] >=
&controlConfigXmp.Compensator[index].CompTable[
controlConfigXmp.Compensator[index].Control[0].CompPoints *
controlConfigXmp.Compensator[index].Control[1].CompPoints]);
memcpy(controlConfigXmp.Compensator[index].CompTable,
comp->CompTable,
controlConfigXmp.Compensator[index].Control[0].CompPoints *
controlConfigXmp.Compensator[index].Control[1].CompPoints *
sizeof(long));
returnValue =
mpiControlConfigSet(control,
&controlConfig,
&controlConfigXmp);
}
return (returnValue);
}