.

     

MPI Application Template
template.c
 

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);

   meiPlatformSleep(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 */
   meiAssert(*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,
                       &params);
    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);
   MEIPlatform          platform;
   MPIRecorderConfig    config;
   MEIRecorderConfig    configXMP;
   MEIProbeInfo         probeInfo;
   MEIXmpData           *firmware;
   long              returnValue;
   long              *pointList[MEIXmpMaxRecSize];
   long              **point;
   long              initialPattern;

   /* Get pointer to XMP firmware */
   returnValue = mpiControlMemory(control,
                      &firmware,
                      NULL);

   if (returnValue == MPIMessageOK)
   {
      platform = meiControlPlatform(control);
      returnValue = meiPlatformValidate(platform);
   }

   if (returnValue == MPIMessageOK)
   {
      returnValue = meiProbeInfo(probe, &probeInfo);
   }

   if (returnValue == MPIMessageOK)
   {
      returnValue = meiPlatformMemoryGet(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[MEIRecorderTriggerIndexSTART].type = MEIRecorderTriggerTypeUSER;
      configXMP.trigger[MEIRecorderTriggerIndexSTART].attributes.user.addr = probeInfo.address.status;
      configXMP.trigger[MEIRecorderTriggerIndexSTART].attributes.user.condition = MEIRecorderTriggerConditionCHANGE;
      configXMP.trigger[MEIRecorderTriggerIndexSTART].attributes.user.count = 2;
      configXMP.trigger[MEIRecorderTriggerIndexSTART].attributes.user.mask    = 0xFFFF0000;
      configXMP.trigger[MEIRecorderTriggerIndexSTART].attributes.user.pattern = initialPattern & 0xFFFF0000;

      configXMP.trigger[MEIRecorderTriggerIndexSTOP].type = MEIRecorderTriggerTypeDISABLED;
      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,
            &notify,
            &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;
}


 
      
       Legal Notice  |  Tech Email  |  Feedback
      
Copyright ©
2001-2009 Motion Engineering