gantry.c -- Configures two motors to be gantry axes made of of a
linear and yaw axis.
/* 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.
*/
/*
:Configures two motors to be gantry axes with a linear and a yaw axis
This program demonstrates how to configure a standard gantry system.
gantryConfigSet(...) configures axis 0 and 1 for gantry operation where axis 0
controls linear motion and axis 1 controls yaw. User can also set axis 0 to
control yaw and axis 1 to control linear motion. gantryConfigClear(...) clears
the gantry configuration. controlAddressGet(...) and controlAddrToAddr(...)
are used to read and write application memory to controller memory for gantry
configuration.
- The control algorithms are not affected. Each motor still only uses
the feedback from itself. There is no linear and yaw control loops in this
configuration. The control loop on each motor is unaffected by the gantry
configuration.
- The linear axis that is created shows an actual position that is simply
the average ((x+y)/2)of position between the first and second motors.
- The yaw axis that is created shows an actual position that is simply the
average of the positive first motor position and negative second motor
position ((x-y)/2).
- The origin can be set (to zero or otherwise shift the origin) on the axis
object as is done normally. The origin shift will occur on the combined
positions. This is as if the linear axis had a single encoder indicating
linear position and a second encoder indicating yaw.
- The linear axis number will be the same as the first motor number. The
Yaw axis number will be the same as the second motor number.
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 MOTION_COUNT (2)
#define AXIS_COUNT (2)
#define FILTER_COUNT (2)
#define MOTOR_COUNT (2)
/* 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 numberOfMotions,
MPIAxis *axis,
long numberOfAxes,
MPIFilter *filter,
long numberOfFilters,
MPIMotor *motor,
long numberOfMotors)
{
long index;
long returnValue;
/* Create motion controller object */
*control =
mpiControlCreate(controlType, controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue =
mpiControlInit(*control);
msgCHECK(returnValue);
/* Create axes objects */
for (index = 0; index < numberOfAxes; index++)
{
axis[index] =
mpiAxisCreate(*control, index);
msgCHECK(mpiAxisValidate(axis[index]));
}
/* Create motion supervisor objects with axis[] */
for (index = 0; index < numberOfMotions; index++)
{
motion[index] =
mpiMotionCreate(*control, index, axis[index]);
msgCHECK(mpiMotionValidate(motion[index]));
/* Map axis to motion supervisor on XMP */
returnValue =
mpiMotionAction(motion[index], MPIActionMAP);
msgCHECK(returnValue);
}
/* Create filter objects */
for (index = 0; index < numberOfFilters; index++)
{
filter[index] =
mpiFilterCreate(*control, index);
msgCHECK(mpiFilterValidate(filter[index]));
}
/* Create motor objects */
for (index = 0; index < numberOfMotors; index++)
{
motor[index] =
mpiMotorCreate(*control, index);
msgCHECK(mpiMotorValidate(motor[index]));
}
}
/* Perform certain cleanup actions and delete MPI objects */
void programCleanup(MPIControl *control,
MPIMotion *motion,
long numberOfMotions,
MPIAxis *axis,
long numberOfAxes,
MPIFilter *filter,
long numberOfFilters,
MPIMotor *motor,
long numberOfMotors)
{
long index;
long returnValue;
/* Delete motor objects */
for (index = 0; index < numberOfMotors; index++)
{
returnValue =
mpiMotorDelete(motor[index]);
msgCHECK(returnValue);
motor[index] = MPIHandleVOID;
}
/* Delete filter objects */
for (index = 0; index < numberOfFilters; index++)
{
returnValue =
mpiFilterDelete(filter[index]);
msgCHECK(returnValue);
filter[index] = MPIHandleVOID;
}
/* Delete motion supervisor objects */
for (index = 0; index < numberOfMotions; index++)
{
returnValue =
mpiMotionDelete(motion[index]);
msgCHECK(returnValue);
motion[index] = MPIHandleVOID;
}
/* Delete axes objects */
for (index = 0; index < numberOfAxes; index++)
{
returnValue =
mpiAxisDelete(axis[index]);
msgCHECK(returnValue);
axis[index] = MPIHandleVOID;
}
/* Delete motion controller object */
returnValue =
mpiControlDelete(*control);
msgCHECK(returnValue);
*control = MPIHandleVOID;
}
/* controlAddressGet(...) gets the controller memory location of the specified
address string obtained from command utility VM3.
*/
long controlAddressGet(MPIControl control, void *dst, void *src)
{
MPIPlatform platform;
long returnValue;
platform = mpiControlPlatform(control);
returnValue = mpiPlatformValidate(platform);
if(returnValue == MPIMessageOK)
{
returnValue =
mpiPlatformMemoryToFirmware(platform,
(void**)src,
(void**)dst);
}
return returnValue;
}
/* controlAddrToAddr(...) writes data stored in controller memory location src
to dst. The memory addresses are strings obtained from command utility VM3.
*/
long controlAddrToAddr(MPIControl control, void *dst, void *src)
{
long returnValue;
long addr;
returnValue = controlAddressGet(control,
&addr,
src);
if(returnValue == MPIMessageOK)
{
returnValue =
mpiControlMemorySet(control,
(void**)dst,
&addr,
sizeof(dst));
}
return returnValue;
}
/* gantryConfigSet(...) demonstrates how to configure a standard gantry setup
on two axes. This function sets the first motor to be linear and the second one
twist (yaw) by default. This order can be swapped by replacing the code below
which sets the first motor to be twist (yaw) and second motor to be linear.
Replace:
mpiConfig[0].GantryType = MPIXmpAxisGantryTypeLINEAR;
mpiConfig[1].GantryType = MPIXmpAxisGantryTypeTWIST;
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
fValue = -1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
With:
mpiConfig[0].GantryType = MPIXmpAxisGantryTypeTWIST;
mpiConfig[1].GantryType = MPIXmpAxisGantryTypeLINEAR;
fValue = -1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
*/
long gantryConfigSet(MPIControl control, MPIMotor *motor, MPIAxis *axis, long firstMotor, long secondMotor)
{
MPIXmpData *firmware;
MPIXmpBufferData *buffer;
MPIAxisConfig config[2];
MPIAxisConfig mpiConfig[2];
long returnValue;
float fValue;
returnValue =
mpiControlMemory(control,
&firmware,
&buffer);
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigGet(axis[firstMotor],
&config[0],
&mpiConfig[0]);
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigGet(axis[secondMotor],
&config[1],
&mpiConfig[1]);
}
/* configure Axes for Linear and Yaw by mixing the encoder */
if(returnValue == MPIMessageOK){
mpiConfig[0].FeedbackDeltaPtr[0] = &firmware->Motor[firstMotor].IO.Encoder[0].Delta;
mpiConfig[0].FeedbackDeltaPtr[1] = &firmware->Motor[secondMotor].IO.Encoder[0].Delta;
mpiConfig[0].GantryType = MPIXmpAxisGantryTypeLINEAR; /* Reverse this value with the one below to switch linear and yaw axes */
mpiConfig[1].FeedbackDeltaPtr[0] = &firmware->Motor[firstMotor].IO.Encoder[0].Delta;
mpiConfig[1].FeedbackDeltaPtr[1] = &firmware->Motor[secondMotor].IO.Encoder[0].Delta;
mpiConfig[1].GantryType = MPIXmpAxisGantryTypeTWIST; /* Reverse this value with the one above to switch linear and yaw axes */
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigSet(axis[firstMotor],
&config[0],
&mpiConfig[0]);
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigSet(axis[secondMotor],
&config[1],
&mpiConfig[1]);
}
/* Mix axes to first filter input */
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[0].Ptr,
&firmware->Axis[firstMotor].Link);
}
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[1].Ptr,
&firmware->Axis[secondMotor].Link);
}
/* Mix axes to second filter input */
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Ptr,
&firmware->Axis[firstMotor].Link);
}
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Ptr,
&firmware->Axis[secondMotor].Link);
}
/* Set the filter input coefficients */
if(returnValue == MPIMessageOK){
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = 1.0; /* Reverse this value with the one below to switch linear and yaw axes */
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = -1.0; /* Reverse this value with the one above to switch linear and yaw axes */
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
}
return returnValue;
}
/* gantryConfigClear(...) demonstrates how to clear gantry configuration.
The gantry axes return to normal operation after this function is run.
*/
long gantryConfigClear(MPIControl control, MPIMotor *motor, MPIAxis *axis, long firstMotor, long secondMotor)
{
MPIXmpData *firmware;
MPIXmpBufferData *buffer;
MPIAxisConfig config[2];
MPIAxisConfig mpiConfig[2];
long returnValue;
float fValue;
returnValue =
mpiControlMemory(control,
&firmware,
&buffer);
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigGet(axis[firstMotor],
&config[0],
&mpiConfig[0]);
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigGet(axis[secondMotor],
&config[1],
&mpiConfig[1]);
}
/* configure Axes for normal operation */
if(returnValue == MPIMessageOK){
mpiConfig[0].FeedbackDeltaPtr[0] = &firmware->Motor[firstMotor].IO.Encoder[0].Delta;
mpiConfig[0].FeedbackDeltaPtr[1] = &firmware->Motor[firstMotor].IO.Encoder[0].Delta;
mpiConfig[0].GantryType = MPIXmpAxisGantryTypeNONE;
mpiConfig[1].FeedbackDeltaPtr[0] = &firmware->Motor[secondMotor].IO.Encoder[0].Delta;
mpiConfig[1].FeedbackDeltaPtr[1] = &firmware->Motor[secondMotor].IO.Encoder[0].Delta;
mpiConfig[1].GantryType = MPIXmpAxisGantryTypeNONE;
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigSet(axis[firstMotor],
&config[0],
&mpiConfig[0]);
}
if(returnValue == MPIMessageOK){
returnValue =
mpiAxisConfigSet(axis[secondMotor],
&config[1],
&mpiConfig[1]);
}
/* Set both filter 0 input channels to axis 0 */
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[0].Ptr,
&firmware->Axis[firstMotor].Link);
}
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[1].Ptr,
&firmware->Axis[firstMotor].Link);
}
/* Set both filter 1 input channels to axis 1 */
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Ptr,
&firmware->Axis[secondMotor].Link);
}
if(returnValue == MPIMessageOK){
returnValue = controlAddrToAddr(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Ptr,
&firmware->Axis[secondMotor].Link);
}
/* Set the filter input coefficients */
if(returnValue == MPIMessageOK){
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = 0.0; /* 0 for no gantry */
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[firstMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = 1.0;
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[0].Coeff,
&fValue,
sizeof(fValue));
}
if(returnValue == MPIMessageOK){
fValue = 0.0; /* 0 for no gantry */
returnValue = mpiControlMemorySet(control,
&firmware->ControlLaw.Standard.Filter[secondMotor].Axis[1].Coeff,
&fValue,
sizeof(fValue));
}
return returnValue;
}
/* Main user routine */
int main(int argc,
char *argv[])
{
MPIControl control;
MPIControlType controlType;
MPIControlAddress controlAddress;
MPIMotion motion[MOTION_COUNT];
MPIAxis axis[AXIS_COUNT];
MPIFilter filter[FILTER_COUNT];
MPIMotor motor[MOTOR_COUNT];
long returnValue;
/* 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_COUNT,
axis,
AXIS_COUNT,
filter,
FILTER_COUNT,
motor,
MOTOR_COUNT);
/* Set up gantry operation on axis 0 and 1 */
returnValue = gantryConfigSet(control, motor, axis, 0, 1);
/* Clear gantry operation on axis 0 and 1 */
//returnValue = gantryConfigClear(control, motor, axis, 0, 1);
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(&control,
motion,
MOTION_COUNT,
axis,
AXIS_COUNT,
filter,
FILTER_COUNT,
motor,
MOTOR_COUNT);
return returnValue;
}
|
