probe1.c -- Position captures using Probe in Time mode.
/* probe1.c */
/* Copyright(c) 1991-2007 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 Time mode.
This program demonstrates how to configure and use the probe object in time
mode. The probe is configured to latch four time stamps in each servo cycle,
providing a sampling rate of 8kHz(4 x 2kHz default controller sampling rate).
The probe object can be configured to trigger on the same motor or on a different
motor when a high speed input HSIN32 board is used.
In this example, probe is setup to latch the probe FPGA clock when the index
pulse on axis 0 is triggerred. 2 recorders are setup to store this time value
as well as the motor position at each probe trigger. The motor is set to move
50 revolutions at 2000 counts/rev. This will generate 100 triggers, one for
each state transition. The interpolated position is then calculated and stored
to a text file "probe1.txt."
Note in order to have this program execute properly, the controller must have
2 recorders and 1 capture objects enabled. This can be set in the controller
summary window in Motion Console. If the program is returning invalid handle
error, please run recorderInUse.c to check for any stalled recorders.
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"
#if defined(ARG_MAIN_RENAME)
#define main probe1Main
argMainRENAME(main, probe1)
#endif
#define OUT_FILE "probe1.txt"
#define AXIS (0) /* Axis number to take motor position data on */
#define MOTOR_PROBE (0) /* Motor number to trigger probe */
#define PROBE_TYPE (MPIProbeTypeMOTOR) /* Probe type, motor or IO */
/* Other settings: MPIProbeTypeIO */
#define PROBE_SOURCE (MPIProbeSourceINDEX) /* Probe trigger Source */
/* Other settings: MPIProbeSourceMOTOR_IO_0..., MPIProbeSourceINDEX, MPIProbeSourceHOME */
#define RECORD_COUNT (2000) /* Recorder size */
#define START_POSITION (0)
#define END_POSITION (2000 * 50) /* 100 Triggers (2 per rev.) */
#define MOVE_VEL (5.0E4) /* Velocity */
#define MOVE_ACCEL (5.0E5) /* Acceleration */
#define MOVE_DECEL (5.0E5) /* Deceleration */
#define MOVE_JERK_PERCENT (66.0) /* Jerk percent, 66% gives the smoothest motion */
MPIRecorderRecord Record[RECORD_COUNT];
/* User defined data structure */
typedef struct ProbeRecord {
long sample;
short io;
short index;
short data[4];
long time;
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,
MPIMotion *motion,
MPINotify *notify,
MPIEventMgr *eventMgr,
Service *service)
{
MPIEventMask eventMask;
long returnValue;
/* Obtain a control handle */
*control =
mpiControlCreate(controlType, controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize the controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
*axis =
mpiAxisCreate(*control,
AXIS);
msgCHECK(mpiAxisValidate(*axis));
/* Create motion supervisor object using MS number 0 */
*motion =
mpiMotionCreate(*control,
AXIS,
MPIHandleVOID);
msgCHECK(mpiMotionValidate(*motion));
/* Create 1-axis motion coordinate system */
returnValue =
mpiMotionAxisListSet(*motion,
1,
axis);
msgCHECK(returnValue);
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); /* default sleep (msec) */
mpiAssert(*service != NULL);
}
/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl *control,
MPIRecorder recorder,
Service service,
MPIEventMgr eventMgr,
MPINotify notify,
MPIMotion motion,
MPIAxis axis)
{
long returnValue;
/* Delete control handle */
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);
*control = MPIHandleVOID;
}
/* ConfigureRecorder(...) sets the recorder object to record controller sample
counter, probe status, probe, axis position, and velocity on each probe
trigger.
*/
MPIRecorder ConfigureRecorder(MPIControl control,
MPIProbe probe,
long recordCount,
long axisNumber)
{
MPIRecorder recorder = MPIHandleVOID;
MPIPlatform platform;
MPIRecorderConfig config;
MPIRecorderConfig configXMP;
MPIProbeInfo probeInfo;
MPIXmpData *firmware;
long returnValue;
long *pointList[MPIXmpMaxRecSize];
long **point;
long initialPattern;
returnValue = mpiControlValidate(control);
if (returnValue == MPIMessageOK) {
/* Get platform handle */
platform = mpiControlPlatform(control);
returnValue = mpiPlatformValidate(platform);
}
if (returnValue == MPIMessageOK) {
/* Create data recorder */
recorder = mpiRecorderCreate(control, -1);
returnValue = mpiRecorderValidate(recorder);
}
if (returnValue == MPIMessageOK) {
/* Get pointer to XMP firmware */
returnValue =
mpiControlMemory(control,
&firmware,
NULL);
}
if (returnValue == MPIMessageOK) {
returnValue = mpiProbeInfo(probe, &probeInfo);
}
if (returnValue == MPIMessageOK) {
returnValue = mpiPlatformMemoryGet(platform,
&initialPattern,
probeInfo.address.status,
sizeof(initialPattern));
}
/* Get recorder configuration handle */
if (returnValue == MPIMessageOK) {
returnValue = mpiRecorderConfigGet(recorder,
&config,
&configXMP);
}
/* Configure recorder trigger so data is recorded upon probe trigger */
if (returnValue == MPIMessageOK) {
config.addressCount = 1;
config.highCount = 0;
config.period = 0;
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);
}
/* Configure recorder record */
if (returnValue == MPIMessageOK) {
point = pointList;
*point++ = &firmware->SystemData.SampleCounter;
*point++ = probeInfo.address.status;
*point++ = (long *)&probeInfo.address.data[0]; /* two 16 bit words per long */
*point++ = (long *)&probeInfo.address.data[2];
*point++ = probeInfo.address.time;
*point++ = &firmware->Axis[axisNumber].ActPosition;
*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 (recorder);
}
/* ProcessRecords(...) processes the recorder data and writes results to a file.
The interploated position is calculated using the equation below:
Position Interpolated = (Pos2 - Pos1)*(TimeProbe - Time1)/(Time2 - Time1) + Pos1
*/
void ProcessRecords(MPIRecorderRecord *Record, long recordCount, long initialIndex)
{
/* Write to file only if data is present */
if (recordCount > 0) {
FILE *outfile;
ProbeRecord *record;
ProbeRecord *next_record;
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;
record = (ProbeRecord *)Record;
old_index = initialIndex;
for (recordIndex = 0; recordIndex < recordCount; recordIndex++) {
long index;
long new_captures;
short feedback_time;
double time_delta;
double position_delta;
record = (ProbeRecord *)&Record[recordIndex];
next_record = (ProbeRecord *)&Record[recordIndex + 1];
new_captures = record->index - old_index;
if(new_captures < 0) new_captures += 256;
if(new_captures > 4) printf("too many captures: ");
if(new_captures)
{
feedback_time = (short)(record->time >> 2);
time_delta = (next_record->time - record->time) >> 2;
position_delta = next_record->position - record->position;
for(index = 0; index < new_captures; index++)
{
long data_index;
long io_mask;
short captured_time;
short time_diff;
double captured_position;
double offset;
long io_state;
/* Calculate interpolated position */
data_index = (old_index + index) & 3;
captured_time = record->data[data_index];
io_mask = 1 << data_index;
io_state = record->io & io_mask;
time_diff = captured_time - feedback_time;
offset = (position_delta * (double)time_diff) /
time_delta;
captured_position = (double)record->position + offset;
/* Write data to file */
fprintf(outfile,"%ld",record->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");
printf("%ld",record->sample);
printf("\t%d",io_state ? 1 : 0);
printf("\t%ld",old_index + index);
printf("\t%.0lf\n",captured_position);
captures++;
}
}
old_index = record->index;
}
fprintf(outfile,"%d captures\n", captures);
fclose(outfile);
}
}
}
/* configureProbe(...) sets up the probe object.
*/
void configureProbe(MPIControl control,
MPIProbe *probe,
long *initialIndex)
{
MPIProbeStatus probeStatus;
MPIProbeParams probeParams;
MPIProbeConfig probeConfig;
long returnValue;
long index;
probeParams.type = PROBE_TYPE;
probeParams.number.motor = MOTOR_PROBE;
probeParams.probeIndex = 0;
/* Create probe */
*probe = mpiProbeCreate(control,
&probeParams);
msgCHECK(mpiProbeValidate(*probe));
/* Get probe configuration handle */
returnValue = mpiProbeConfigGet(*probe, &probeConfig, NULL);
msgCHECK(returnValue);
probeConfig.enable = TRUE;
probeConfig.source = PROBE_SOURCE;
probeConfig.data = MPIProbeDataTIME; /* Time mode */
probeConfig.inputFilter = FALSE;
/* Set probe configuration */
returnValue = mpiProbeConfigSet(*probe, &probeConfig, NULL);
msgCHECK(returnValue);
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;
}
/* commandMotion(...) sets up trajectory and commands motion on the specified
motion object. Probe time stamps, position values, etc. are recorded on
each probe trigger. When motion completes, recorders are stopped. Motor is
returned to start position.
*/
void commandMotion(MPIControl control, MPINotify notify, MPIMotion motion, MPIRecorder recorder, MPIProbe probe, long initialIndex)
{
MPITrajectory trajectory;
MPIMotionParams params; /* motion parameters */
MPIRecorderRecord *record;
long recordCount;
long returnValue;
double endPosition;
/* Setup motion parameters */
endPosition = END_POSITION;
trajectory.velocity = MOVE_VEL;
trajectory.acceleration = MOVE_ACCEL;
trajectory.deceleration = MOVE_DECEL;
trajectory.jerkPercent = MOVE_JERK_PERCENT;
params.sCurve.position = &endPosition;
params.sCurve.trajectory = &trajectory;
/* Start data recording */
record = Record;
recordCount = 0;
/* Start motion */
returnValue =
mpiMotionStart(motion,
MPIMotionTypeS_CURVE,
¶ms);
fprintf(stderr,
"mpiMotionStart returns 0x%x: %s\n",
returnValue,
mpiMessage(returnValue, NULL));
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,
MPIWaitPOLL);
if (returnValue != MPIMessageOK)
{
if(returnValue != MPIMessageTIMEOUT)
{
msgCHECK(returnValue);
}
}
else
{
fprintf(stderr,
"mpiNotifyEventWait() returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
/* Exit while loop when motion is done */
if (eventStatus.type == MPIEventTypeMOTION_DONE)
{
printf("Motion Done\n");
break;
}
}
}
if (mpiRecorderStop(recorder) != MPIMessageOK) {
fprintf(stderr,
"mpiRecorderStop returns 0x%x: %s\n",
returnValue,
mpiMessage(returnValue, NULL));
if (returnValue == MPIRecorderMessageSTOPPED) {
returnValue = MPIMessageOK;
}
}
/* Calculate interpolated position and stores recorded data to file */
ProcessRecords(Record, recordCount, initialIndex);
trajectory.velocity = MOVE_VEL;
endPosition = START_POSITION;
/* Return motor to starting position */
returnValue =
mpiMotionStart(motion,
MPIMotionTypeS_CURVE,
¶ms);
while (TRUE) {
MPIEventStatus eventStatus;
/* Collect motion events */
returnValue =
mpiNotifyEventWait(notify,
&eventStatus,
MPIWaitPOLL);
if (returnValue != MPIMessageOK)
{
if(returnValue != MPIMessageTIMEOUT)
{
msgCHECK(returnValue);
}
}
else
{
fprintf(stderr,
"mpiNotifyEventWait() returns 0x%x\n"
"\teventStatus: type %d source 0x%x info 0x%x\n",
returnValue,
eventStatus.type,
eventStatus.source,
eventStatus.info[0]);
/* Exit while loop when motion is done */
if (eventStatus.type == MPIEventTypeMOTION_DONE)
{
printf("Motion Done\n");
break;
}
}
}
}
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 */
Service service; /* service handle */
MPIProbe probe;
MPIRecorder recorder; /* data recorder handle */
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,
&motion,
¬ify,
&eventMgr,
&service);
/* Set up probe object */
configureProbe(control,
&probe,
&initialIndex);
/* Set up recorder object to record data on each probe trigger */
recorder =
ConfigureRecorder(control,
probe,
RECORD_COUNT, /* maximum number of records */
AXIS);
msgCHECK(mpiRecorderValidate(recorder));
/* Start motion and record probe latched position. ProccessRecords is
called within this function to calculate the interpolated position
for each trigger. Values are then stored in "probe1.text." Motor is
returned to start position in the end.
*/
commandMotion(control,
notify,
motion,
recorder,
probe,
initialIndex);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(&control,
recorder,
service,
eventMgr,
notify,
motion,
axis);
return MPIMessageOK;
}
|
