probe2.c -- Position captures using Probe in Position mode.
/* probe2.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.
*/
/*
:Position captures using Probe in Position mode by making a position move
The probe objects enables you to get high accuracy position latching more
than one time per sample (4 max in standard firmware). When probe is used
in position modes (as opposed to time mode), the positin latch trigger
must be on the same node as the encoder that supplies the position. Keep
in mind that serial encoders and other encoders that send positin regularly
will not work well with position mode probe. This is because there is no
more timely position than once per servo sample. When using probe with serial
encoders, use time based probe.
This program does the following:
1. Creates and sets up objects
2. Configures the probe object
3. Configures the recorder
4. Commands a move so index pulses are latched by probe
5. Collects the recorder data and waits for a motion done
6. Decodes the probe information that was colelcted by the recorder
7. Makes a move back the starting position for convenience's sake
8. Waits for the motion to finish
9. Deletes objects
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.
The msgCHECK(...) macros used in the following sample code are intended
to convey our strong belief that ALL error return codes should be checked.
Actual application code should use specific error handling techniques (other
than msgCHECKs) best suited to your internal error recovery methods.
*/
#include <stdlib.h>
#include <stdio.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#define OUT_FILE "probe2.txt"
#define AXIS (0)
#define RECORD_COUNT (2000) /* max number of edges that can be recorder (sets array size) */
#define END_POSITION (8192 * 2) /* should be 20 edges (2 per rev.) */
#define MOVE_VEL (100000)
#define MOVE_ACCEL (3.0E5)
#define MOVE_DECEL (3.0E5)
#define MOVE_JERK_PERCENT (20.0)
/*
The order of data in this struct MUST match the order data is recorded
for this program to work.
*/
typedef struct ProbeRecord
{
long sample;
short io;
short index;
short data[4];
long encoder;
long position;
float velocity;
} ProbeRecord;
/* Perform basic command line parsing. (-control -server -port -trace) */
void basicParsing(int argc,
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress)
{
long argIndex;
/* Parse command line for Control type and address */
argIndex = argControl(argc, argv, controlType, controlAddress);
/* Check for unknown/invalid command line arguments */
if (argIndex < argc)
{
fprintf(stderr,"usage: %s %s\n", argv[0], ArgUSAGE);
exit(MPIMessageARG_INVALID);
}
}
/* Create and initialize MPI objects */
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIAxis *axis,
long axisNum,
MPIMotion *motion,
long motionNum,
MPINotify *notify,
MPIEventMgr *eventMgr,
Service *service,
MPIProbe *probe,
MPIRecorder *recorder)
{
MPIEventMask eventMask;
MPIProbeParams probeParams;
long returnValue;
/* Obtain a control handle */
*control =
mpiControlCreate(controlType, controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize the controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
*axis =
mpiAxisCreate(*control,
axisNum);
msgCHECK(mpiAxisValidate(*axis));
/* Create motion supervisor object using MS number 0 */
*motion =
mpiMotionCreate(*control,
motionNum,
*axis);
msgCHECK(mpiMotionValidate(*motion));
returnValue = mpiMotionAction(*motion, MPIActionRESET);
msgCHECK(returnValue);
mpiPlatformSleep(10);
/* Request notification of all events from motion */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskALL(eventMask);
returnValue =
mpiMotionEventNotifySet(*motion,
eventMask,
NULL);
msgCHECK(returnValue);
/* Create event notification object for motion */
*notify =
mpiNotifyCreate(eventMask,
*motion);
msgCHECK(mpiNotifyValidate(*notify));
/* Create event manager object */
*eventMgr = mpiEventMgrCreate(*control);
msgCHECK(mpiEventMgrValidate(*eventMgr));
/* Add notify to event manager's list */
returnValue =
mpiEventMgrNotifyAppend(*eventMgr,
*notify);
msgCHECK(returnValue);
/* Create service thread */
*service =
serviceCreate(*eventMgr,
-1, /* default (max) priority */
-1); /* use events */
mpiAssert(*service != NULL);
probeParams.type = MPIProbeTypeMOTOR;
probeParams.number.motor = AXIS;
probeParams.probeIndex = 0;
*probe = mpiProbeCreate(*control,
&probeParams);
msgCHECK(mpiProbeValidate(*probe));
/* Create data recorder */
*recorder = mpiRecorderCreate(*control, -1);
returnValue = mpiRecorderValidate(*recorder);
msgCHECK(returnValue);
}
/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIRecorder recorder,
Service service,
MPIEventMgr eventMgr,
MPINotify notify,
MPIMotion motion,
MPIAxis axis,
MPIControl control)
{
long returnValue;
returnValue = mpiRecorderDelete(recorder);
msgCHECK(returnValue);
returnValue = serviceDelete(service);
msgCHECK(returnValue);
returnValue = mpiEventMgrDelete(eventMgr);
msgCHECK(returnValue);
returnValue = mpiNotifyDelete(notify);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis);
msgCHECK(returnValue);
returnValue =
mpiControlDelete(control);
msgCHECK(returnValue);
}
/* Command simple s-curve motion */
void simpleScurveMove(MPIMotion motion,
double goalPosition,
double velocity,
double acceleration,
double deceleration,
double jerkPercent)
{
MPIMotionParams params; /* Motion parameters */
MPITrajectory trajectory; /* Trajectory information */
long returnValue; /* MPI library return value */
/* Setup trajectory structure */
trajectory.velocity = velocity;
trajectory.acceleration = acceleration;
trajectory.deceleration = deceleration;
trajectory.jerkPercent = jerkPercent;
/* Setup parameters structure */
params.sCurve.trajectory = &trajectory;
params.sCurve.position = &goalPosition;
/* Start motion */
returnValue =
mpiMotionStart(motion,
MPIMotionTypeS_CURVE,
¶ms);
msgCHECK(returnValue);
}
/*
Configure the Data Recorder. This functions set up the recorder to use
triggers to record only the needed data. Be sure to set the recorded
data order to be consistant with the ProbeRecord struct at the top of
the program. The data recorded is sent to ProcessRecords to determine
the latched positions.
*/
long ConfigureRecorder(MPIRecorder recorder,
MPIProbe probe,
long recordCount,
long axisNumber)
{
MPIControl control = mpiRecorderControl(recorder);
MPIPlatform platform;
MPIRecorderConfig config;
MPIRecorderConfig configXMP;
MPIProbeInfo probeInfo;
MPIXmpData *firmware;
long returnValue;
long *pointList[MPIXmpMaxRecSize];
long **point;
long initialPattern;
/* Get pointer to XMP firmware */
returnValue = mpiControlMemory(control,
&firmware,
NULL);
if (returnValue == MPIMessageOK)
{
platform = mpiControlPlatform(control);
returnValue = mpiPlatformValidate(platform);
}
if (returnValue == MPIMessageOK)
{
returnValue = mpiProbeInfo(probe, &probeInfo);
}
if (returnValue == MPIMessageOK)
{
returnValue = mpiPlatformMemoryGet(platform, &initialPattern, probeInfo.address.status, sizeof(initialPattern));
}
if (returnValue == MPIMessageOK)
{
returnValue = mpiRecorderConfigGet(recorder, &config, &configXMP);
}
if (returnValue == MPIMessageOK)
{
config.addressCount = 1;
config.highCount = 0;
config.period = 0;
/*
This configures the recorder to collect data only when it sees probe status change.
It is not strictly neccessary to use recorder triggers (could just record a lot of
data, instead). Recorder triggers just keep the amount of recorded data more reasonable.
*/
configXMP.trigger[MPIRecorderTriggerIndexSTART].type = MPIRecorderTriggerTypeUSER;
configXMP.trigger[MPIRecorderTriggerIndexSTART].attributes.user.addr = probeInfo.address.status;
configXMP.trigger[MPIRecorderTriggerIndexSTART].attributes.user.condition = MPIRecorderTriggerConditionCHANGE;
configXMP.trigger[MPIRecorderTriggerIndexSTART].attributes.user.count = 2;
configXMP.trigger[MPIRecorderTriggerIndexSTART].attributes.user.mask = 0xFFFF0000;
configXMP.trigger[MPIRecorderTriggerIndexSTART].attributes.user.pattern = initialPattern & 0xFFFF0000;
configXMP.trigger[MPIRecorderTriggerIndexSTOP].type = MPIRecorderTriggerTypeDISABLED;
returnValue = mpiRecorderConfigSet(recorder, &config, &configXMP);
}
if (returnValue == MPIMessageOK)
{
point = pointList;
/*
From the description at the top of the program:
typedef struct ProbeRecord
{
long sample;
short io;
short index;
short data[4];
long encoder;
long position;
float velocity;
} ProbeRecord;
Must be recorded in this order to match ProbeRecord.
*/
*point++ = &firmware->SystemData.SampleCounter;
/* 16 lsb are io, 16 msb are index (probe latch counter) */
*point++ = probeInfo.address.status;
/*
The last 16 bits from each probe latched position are packed in
the following data registers. The 16 bits are the 16 lsb from the
FPGA encoder position. Keep in mind that the FPGA encoder position
does not consider the origin.
The first probe latch is data[0] (16 lsb)
The second probe latch is data[1] (16 msb)
The third probe latch is data[2] (16 lsb)
The fourth probe latch is data[3] (16 msb)
*/
*point++ = (long *)&probeInfo.address.data[0];
*point++ = (long *)&probeInfo.address.data[2];
*point++ = probeInfo.address.primaryPosition;
*point++ = &firmware->Axis[axisNumber].ActPosition.l[1];
/* Velocity only used in time based probe. Not used in this position based sample app. */
*point++ = (long *)&firmware->Axis[axisNumber].CommandVelocity;
/* Configure data recorder records */
returnValue =
mpiRecorderRecordConfig(recorder,
MPIRecorderRecordTypePOINT,
point - pointList, /* count */
pointList);
}
if (returnValue != MPIMessageOK)
{
mpiRecorderDelete(recorder);
recorder = MPIHandleVOID;
}
return (returnValue);
}
/* Process the recorder data and write results to a file. */
void ProcessRecords(MPIRecorderRecord *Record, long recordCount, long initialIndex)
{
if (recordCount > 0)
{
FILE *outfile;
ProbeRecord *probeRecord;
outfile = fopen(OUT_FILE, "w");
if (outfile == NULL)
{
fprintf(stderr,
"Could not open output file %s.\n",
OUT_FILE);
}
else
{
long recordIndex;
long old_index;
long captures;
fprintf(outfile,"%d records\n", recordCount);
fprintf(outfile,
"Sample"
"\tIO State"
"\tIndex"
"\tcaptured pos"
"\n");
captures = 0;
probeRecord = (ProbeRecord *)Record;
old_index = initialIndex;
for (recordIndex = 0; recordIndex < recordCount; recordIndex++)
{
long index;
long new_captures;
short feedback_position;
probeRecord = (ProbeRecord *)&Record[recordIndex];
new_captures = probeRecord->index - old_index;
if(new_captures < 0) new_captures += 256;
if(new_captures > 4) printf("too many captures: ");
if(new_captures)
{
feedback_position = (short)(probeRecord->encoder);
for(index = 0; index < new_captures; index++)
{
long data_index;
long io_mask;
short position_diff;
double captured_position;
long io_state;
/*
Get the 2 lsb of old_index + index.
Remember that there are only 4 probe locations. The old_index + index
will tell us which data index to look in. This is equivalent to
doing a modulo 4 (for the four locations). Keep in mind that at probe
index 0, the probe writes to probeRecord->data[i = 0]. It will increment
i until it reaches 4, where it will wrap around back to 0 (hence the
modulo 4).
*/
data_index = (old_index + index) & 3;
/*
probeRecord->data[data_index] is the last 16 bits of the motor object
latched position. Keep in mind that the axis origin is not considered
in the latched position.
*/
io_mask = 1 << data_index;
/*
*/
io_state = probeRecord->io & io_mask;
/*
position_diff is the distance from the sample following the latched position to the
latched position. This value can vary from 0 to -actual velocity in counts /
sample. probeRecord->data[data_index] is not origin adjusted. Both
probeRecord->data[data_index] and feedback_position are shorts (16 bits).
*/
position_diff = probeRecord->data[data_index] - feedback_position;
/*
probeRecord->Position is the origin adjusted position at the sample following the latch
feedback_position is the non-origin adjusted position at the sample following the latch
This means that feedback_position - probeRecord->Position = origin
The reason that the position_diff uses the non-origin based position is that
the latched position is returned with a non-origin based position.
*/
captured_position = (double)probeRecord->position + position_diff;
fprintf(outfile,"%ld",probeRecord->sample);
fprintf(outfile,"\t%d",io_state ? 1 : 0);
fprintf(outfile,"\t%ld",old_index + index);
fprintf(outfile,"\t%.0lf",captured_position);
fprintf(outfile,"\n");
captures++;
}
}
old_index = probeRecord->index;
}
fprintf(outfile,"%d captures\n", captures);
fclose(outfile);
}
}
}
/*
getRecords gets the records from the recorder and checks for motion done.
This function is event driven, so it just waits until it gets events.
This means that if you only record a small number of records, then you
will not get an event from the recorder to get your data because the
recorder will not need to be emptied. To get around this, we check for
records one last time when the motionDone event is recieved, before exiting
the function.
*/
void getRecords(MPIRecorder recorder, MPIRecorderRecord* record, MPINotify notify, long *recordCount)
{
long returnValue;
while (*recordCount < RECORD_COUNT)
{
MPIEventStatus eventStatus;
long countMax;
long count;
countMax = RECORD_COUNT - *recordCount;
if (countMax > 0)
{
returnValue =
mpiRecorderRecordGet(recorder,
countMax,
record,
&count);
if (returnValue == MPIMessageOK)
{
record += count;
*recordCount += count;
}
else {
break;
}
}
/* Collect motion events */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
if (returnValue != MPIMessageOK)
{
if(returnValue != MPIMessageTIMEOUT)
{
msgCHECK(returnValue);
}
}
else
{
fprintf(stderr,
"mpiNotifyEventWait() eventStatus: type %d source 0x%x info 0x%x\n",
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
if (eventStatus.type == MPIEventTypeMOTION_DONE)
{
printf("Motion Done\n");
/*
One last recorder read. This is needed for any events that didn't
get read because they didn't fill the recorder buffer. Without
this read, the while loop will exit on the next break without
getting all the records.
*/
returnValue = mpiRecorderRecordGet(recorder,
countMax,
record,
&count);
if (returnValue == MPIMessageOK)
{
record += count;
*recordCount += count;
}
else
{
break;
}
break;
}
}
}
returnValue = mpiRecorderStop(recorder);
if (returnValue == MPIRecorderMessageSTOPPED)
{
returnValue = MPIMessageOK;
}
msgCHECK(returnValue);
}
/* Simple function that waits for a motionDone event before returning */
void waitForMotionDone(MPINotify notify)
{
long returnValue;
while (TRUE)
{
MPIEventStatus eventStatus;
/* Collect motion events */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitFOREVER);
if (returnValue != MPIMessageOK)
{
if(returnValue != MPIMessageTIMEOUT)
{
msgCHECK(returnValue);
}
}
else
{
fprintf(stderr,
"mpiNotifyEventWait() eventStatus: type %d source 0x%x info 0x%x\n",
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
if (eventStatus.type == MPIEventTypeMOTION_DONE)
{
printf("Motion Done\n");
break;
}
}
}
}
/*
Sets up the probe object. The source has several possible values:
typedef enum MPIProbeSource {
MPIProbeSourceHOME,
MPIProbeSourceINDEX,
MPIProbeSourceLIMIT_HW_NEG,
MPIProbeSourceLIMIT_HW_POS,
MPIProbeSourceINDEX_SECONDARY,
MPIProbeSourceMOTOR_IO_0,
MPIProbeSourceMOTOR_IO_1,
MPIProbeSourceMOTOR_IO_2,
MPIProbeSourceMOTOR_IO_3,
MPIProbeSourceMOTOR_IO_4,
MPIProbeSourceMOTOR_IO_5,
MPIProbeSourceMOTOR_IO_6,
MPIProbeSourceMOTOR_IO_7,
MPIProbeSourceMOTOR_IO_8,
MPIProbeSourceMOTOR_IO_9,
MPIProbeSourceMOTOR_IO_10,
MPIProbeSourceMOTOR_IO_11,
MPIProbeSourceMOTOR_IO_12,
MPIProbeSourceMOTOR_IO_13,
MPIProbeSourceMOTOR_IO_14,
MPIProbeSourceMOTOR_IO_15,
} MPIProbeSource;
The data can be the primary encoder, secondary encoder, or time.
typedef enum MPIProbeData {
MPIProbeDataPOSITION_PRIMARY,
MPIProbeDataPOSITION_SECONDARY,
MPIProbeDataTIME,
} MPIProbeData;
Since probe2.c is a position based probe demo, we will use the primary encoder.
*/
long probeConfig(MPIProbe probe, MPIProbeSource source, MPIProbeData data)
{
MPIProbeConfig probeConfig;
MPIProbeStatus probeStatus;
long initialIndex;
long returnValue;
long index;
returnValue = mpiProbeStatus(probe, &probeStatus, NULL);
msgCHECK(returnValue);
printf("Probe Status:\n\tindex: %d\n",probeStatus.index);
printf("\tio: 0x%8.8lX\n",probeStatus.io);
printf("\tmaxRegisters: %d\n",probeStatus.maxRegisters);
for(index = 0; index < probeStatus.maxRegisters; index++)
{
printf("\tregs[%d]: 0x%8.8lX\n", index, probeStatus.regs[index]);
}
initialIndex = probeStatus.index;
returnValue = mpiProbeConfigGet(probe, &probeConfig, NULL);
msgCHECK(returnValue);
probeConfig.enable = TRUE;
probeConfig.source = source;
probeConfig.data = data;
probeConfig.inputFilter = FALSE;
returnValue = mpiProbeConfigSet(probe, &probeConfig, NULL);
msgCHECK(returnValue);
return initialIndex;
}
int main(int argc,
char *argv[])
{
MPIControl control;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIAxis axis; /* axis handle(s) */
MPIMotion motion; /* motion handle */
MPINotify notify; /* event notification handle */
MPIEventMgr eventMgr; /* event manager handle */
MPIProbe probe;
MPIRecorder recorder; /* data recorder handle */
MPIRecorderRecord record[RECORD_COUNT];
Service service; /* service handle */
long returnValue; /* return value from library */
long recordCount = 0;
long initialIndex;
/* Perform basic command line parsing. (-control -server -port -trace) */
basicParsing(argc,
argv,
&controlType,
&controlAddress);
/* Create and initialize MPI objects */
programInit(&control,
controlType,
&controlAddress,
&axis,
AXIS,
&motion,
AXIS,
¬ify,
&eventMgr,
&service,
&probe,
&recorder);
/* configure the source and encoder (primary / secondary) to use and return probe index */
initialIndex = probeConfig(probe, MPIProbeSourceINDEX, MPIProbeDataPOSITION_PRIMARY);
/* Start data recording */
returnValue = ConfigureRecorder(recorder,
probe,
RECORD_COUNT, /* maximum number of records */
AXIS);
msgCHECK(returnValue);
/* make a move to cause probe triggers -- not strictly neccesary, just need probe triggers */
simpleScurveMove(motion, END_POSITION, MOVE_VEL, MOVE_ACCEL, MOVE_DECEL, MOVE_JERK_PERCENT);
getRecords(recorder, record, notify, &recordCount);
ProcessRecords(record, recordCount, initialIndex);
/* move back to zero for convenience's sake -- easier to try the program multiple times */
simpleScurveMove(motion, 0, MOVE_VEL, MOVE_ACCEL, MOVE_DECEL, MOVE_JERK_PERCENT);
/* wait for the end of the motion before cleaning up and exiting */
waitForMotionDone(notify);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(recorder,
service,
eventMgr,
notify,
motion,
axis,
control);
return MPIMessageOK;
}
|
