home1.c -- Simple Homing routine that captures the hardware position, sets the origin
/* home1.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/home1.c 9 7/18/01 9:32a Kevinh $"
#endif
#if defined(ARG_MAIN_RENAME)
#define main home1Main
argMainRENAME(main, home1)
#endif
/*
:Simple Homing routine that captures the hardware position, sets the origin
and moves back to home.
home1.c allows the user to trigger his home off an input or index
pulse, capture the hardware position, set the origin and then move back
to that home position.
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 "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#define CapturesPerMotor (2) /* Default Capture configuration */
#define ACTIVE_FALLING_EDGE (0)
#define ACTIVE_RAISING_EDGE (1)
/* MPI Object numbers */
#define MOTION_NUMBER (0)
#define AXIS_NUMBER (0)
#define MOTOR_NUMBER (0)
/* Motion Parameters */
#define VELOCITY (5000) /* Move velocity */
#define ACCELERATION (10000) /* Move acceleration */
#define DECELERATION (10000) /* Move deceleration */
/* Capture Parameters */
#define CAPTURE_EDGE (ACTIVE_FALLING_EDGE) /* Capture on falling edge */
/* CAPTURE_TRIGGER can be MEIMotorInputHOME or MEIMotorInputINDEX */
#define CAPTURE_TRIGGER (MEIMotorInputHOME) /* Capture on home pulse */
/* 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,
MPIMotion *motion,
long motionNumber,
MPIAxis *axis,
long axisNumber,
MPIMotor *motor,
long motorNumber,
MPICapture *capture,
long captureNumber)
{
long returnValue;
/* Create motion controller object */
*control =
mpiControlCreate(controlType,
controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
/* Create axis object */
*axis =
mpiAxisCreate(*control,
axisNumber);
msgCHECK(mpiAxisValidate(*axis));
/* Create motion supervisor object with axis */
*motion =
mpiMotionCreate(*control,
motionNumber,
*axis);
msgCHECK(mpiMotionValidate(*motion));
/* Create motor object */
*motor =
mpiMotorCreate(*control,
motorNumber);
msgCHECK(mpiMotorValidate(*motor));
/* Create capture object */
*capture =
mpiCaptureCreate(*control,
captureNumber);
msgCHECK(mpiCaptureValidate(*capture));
}
/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl *control,
MPIMotion *motion,
MPIAxis *axis,
MPIMotor *motor,
MPICapture *capture)
{
long returnValue;
/* Delete capture object */
returnValue =
mpiCaptureDelete(*capture);
msgCHECK(returnValue);
/* Delete motor object */
returnValue =
mpiMotorDelete(*motor);
msgCHECK(returnValue);
/* Delete motion supervisor object */
returnValue =
mpiMotionDelete(*motion);
msgCHECK(returnValue);
/* Delete axis object */
returnValue =
mpiAxisDelete(*axis);
msgCHECK(returnValue);
/* Delete motion controller object */
returnValue =
mpiControlDelete(*control);
msgCHECK(returnValue);
}
/* Calculate default capture number for axisNumber */
long captureNumber(long motorNumber)
{
return ((motorNumber/MEIXmpMotorsPerBlock) * MEIXmpMaxLatches) +
((motorNumber % MEIXmpMotorsPerBlock) * CapturesPerMotor);
}
/*
Configure caputure object
edge: 0 for falling edge, 1 for rising edge
*/
void configureCapture(MPICapture capture,
MEIMotorInput trigger,
long edge)
{
MPICaptureConfig captureConfig;
long returnValue;
/* Disable capture */
returnValue =
mpiCaptureArm(capture,
FALSE);
msgCHECK(returnValue);
/* Read caputre configuration */
returnValue =
mpiCaptureConfigGet(capture,
&captureConfig,
NULL);
msgCHECK(returnValue);
/* Set capture parameters */
captureConfig.trigger.mask = trigger;
captureConfig.trigger.pattern = edge ? trigger : 0;
/* Write caputre configuration */
returnValue =
mpiCaptureConfigSet(capture,
&captureConfig,
NULL);
msgCHECK(returnValue);
}
/* Configure Home Event action */
void configureHomeAction(MPIMotor motor,
MPIAction action,
long activeHigh)
{
MPIMotorEventConfig eventConfig;
MPIEventMask eventMask;
long returnValue;
/* Read home event configuration */
returnValue =
mpiMotorEventConfigGet(motor,
MPIEventTypeHOME,
&eventConfig,
NULL);
msgCHECK(returnValue);
/* Configure Home Event action */
eventConfig.action = action;
eventConfig.trigger.polarity = activeHigh;
/* Write home event configuration */
returnValue =
mpiMotorEventConfigSet(motor,
MPIEventTypeHOME,
&eventConfig,
NULL);
msgCHECK(returnValue);
/* Reset Home Event */
mpiEventMaskCLEAR(eventMask);
mpiEventMaskSET(eventMask, MPIEventTypeHOME);
returnValue =
mpiMotorEventReset(motor,
eventMask);
msgCHECK(returnValue);
}
/* Command simple trapezoidal motion */
void simpleVelocityMove(MPIMotion motion,
double velocity,
double acceleration,
double deceleration)
{
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 = 0.0; /* Trapezoidal profile */
/* Setup parameters structure */
params.velocity.trajectory = &trajectory;
/* Start motion */
returnValue =
mpiMotionStart(motion,
MPIMotionTypeVELOCITY,
¶ms);
msgCHECK(returnValue);
}
/* Command simple trapezoidal motion */
void simpleTrapMove(MPIMotion motion,
double goalPosition,
double velocity,
double acceleration,
double deceleration)
{
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;
/* Setup parameters structure */
params.trapezoidal.trajectory = &trajectory;
params.trapezoidal.position = &goalPosition;
/* Start motion */
returnValue =
mpiMotionStart(motion,
MPIMotionTypeTRAPEZOIDAL,
¶ms);
msgCHECK(returnValue);
}
/* Display caputure status while waiting for the motion to be done */
void displayCaptureStatusUntilMotionDone(MPIMotion motion,
MPICapture capture,
MPICaptureStatus *captureStatus)
{
MPIStatus status;
long motionDone;
long returnValue;
/* Poll status until motion done */
motionDone = FALSE;
while (motionDone == FALSE) {
/* Get the capture status */
returnValue =
mpiCaptureStatus(capture,
captureStatus,
NULL);
msgCHECK(returnValue);
printf("CaptureState:0x%x\r",
captureStatus->state);
/* Get the motion supervisor status */
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state) {
case MPIStateSTOPPING:
case MPIStateMOVING: {
/* Sleep for 20ms and give up control to other threads */
meiPlatformSleep(20);
break;
}
case MPIStateIDLE:
case MPIStateERROR:
case MPIStateSTOPPING_ERROR: {
/* Motion is done */
motionDone = TRUE;
break;
}
default: {
/* Unknown State */
fprintf(stderr, "Unknown state from mpiMotionStatus.\n");
msgCHECK(MPIMessageFATAL_ERROR);
break;
}
}
}
/* Display latched position */
printf("\nLatched Home Position = %.0lf\n",
captureStatus->latch[0]);
}
int main(int argc,
char *argv[])
{
MPIControl control;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIMotion motion;
MPIAxis axis;
MPIMotor motor;
MPICapture capture;
MPICaptureStatus captureStatus;
long returnValue; /* Return value from library */
/* Perform basic command line parsing. (-control -server -port -trace) */
basicParsing(argc,
argv,
&controlType,
&controlAddress);
/* Create and initialize MPI objects */
programInit(&control,
controlType,
&controlAddress,
&motion,
MOTION_NUMBER,
&axis,
AXIS_NUMBER,
&motor,
MOTOR_NUMBER,
&capture,
captureNumber(MOTOR_NUMBER));
/* Configure capture */
configureCapture(capture,
CAPTURE_TRIGGER,
CAPTURE_EDGE);
/* Configure Home Event action to stop */
configureHomeAction(motor,
MPIActionSTOP,
TRUE); /* Active High */
/* Arm the capture */
returnValue =
mpiCaptureArm(capture,
TRUE);
msgCHECK(returnValue);
printf("Looking for home...\n");
/* Start a velocity move */
simpleVelocityMove(motion,
VELOCITY,
ACCELERATION,
DECELERATION);
/* Display caputure status while waiting for the motion to be done */
displayCaptureStatusUntilMotionDone(motion,
capture,
&captureStatus);
/* Set origin to home position */
returnValue =
mpiAxisOriginSet(axis,
captureStatus.latch[0]);
msgCHECK(returnValue);
printf("Moving back to origin.\n");
/* Configure Home Event action to none */
configureHomeAction(motor,
MPIActionNONE,
TRUE);
/* Move back to home */
simpleTrapMove(motion,
0.0, /* Go back to home position */
VELOCITY,
ACCELERATION,
DECELERATION);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(&control,
&motion,
&axis,
&motor,
&capture);
return ((int)returnValue);
}