.

     

MPI Application Template
template.c
 

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

   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);   /* default sleep (msec) */
   meiAssert(*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;
   MEIPlatform          platform;
   MPIRecorderConfig    config;
   MEIRecorderConfig    configXMP;
   MEIProbeInfo         probeInfo;

   MEIXmpData           *firmware;

   long                 returnValue;

   long                 *pointList[MEIXmpMaxRecSize];
   long                 **point;

   long                 initialPattern;


   returnValue = mpiControlValidate(control);

   if (returnValue == MPIMessageOK) {
      /* Get platform handle */
      platform = meiControlPlatform(control);

      returnValue = meiPlatformValidate(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 = meiProbeInfo(probe, &probeInfo);
   }

   if (returnValue == MPIMessageOK) {
      returnValue = meiPlatformMemoryGet(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[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);
   }
   /* 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,
                     &params);

   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,
                     &params);

   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,
                &notify,
                &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;
}
 
      
       Legal Notice  |  Tech Email  |  Feedback
      
Copyright ©
2001-2009 Motion Engineering