ScDither.c -- Sinusoidal Commutation Initialization Using the Dithering
Method
/* ScDither.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.
*/
/*
: Sinusoidal Commutation Initialization Using the Dithering Method
Uses Dithering initialization for one axis. Axis remains in open-loop
mode with amplifier enabled.
Note -
#define provided for reversing encoder and DAC phasing. Program disables
HW Limits, Amp Fault, Home and Error Limit.
The commutation configuration parameters will be different
for your application. Take particular care in setting the open-loop
continuous DAC OutputLevel as this must be a safe level for your specific
drive and motor combination. Please read Sinusoidal Commutation Documentation
in the MPI/MEI Software Reference Manual for further information.
Successful initialization with "Dithering" requires a system with minimal
influence from external forces (e.g., gravitational force, cable carrier, etc)
and static friction.
The parameters DAC level ("OutputLevel") and time delay ("VEL_DELAY" and
"ACC_DELAY") should be set for optimum performance with the user's specific system.
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.
*/
#include <stdlib.h>
#include <stdio.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "apputil.h"
#define VEL_DELAY (5) /* servo samples */
#define ACC_DELAY (5) /* servo samples */
#define RECORD_COUNT_MAX (VEL_DELAY + ACC_DELAY) /* number of data recorder points */
/* Command line arguments and defaults */
long axisNumber = 0;
long motionNumber = 0;
Arg argList[] = {
{ "-axis", ArgTypeLONG, &axisNumber, },
{ "-motion", ArgTypeLONG, &motionNumber, },
{ NULL, ArgTypeINVALID, NULL, }
};
/* Option to reverse encoder phasing */
#undef ReverseEncPhasing
/* Option to reverse DAC phasing */
#undef ReverseDACPhasing
MPIRecorderRecord Record[RECORD_COUNT_MAX];
/* Perform basic command line parsing. (-control -server -port -trace) */
void parseCommandLine(int argc,
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress,
long *axisNumber,
long *motionNumber)
{
long argIndex;
Arg argList[] =
{
/* Determining motor number */
{ "-motor", ArgTypeLONG, axisNumber, },
/* Determining motion supervisor number */
{ "-motion", ArgTypeDOUBLE, motionNumber, },
/* End of argument list (null terminator) */
{ NULL, ArgTypeINVALID, NULL, }
};
/* Parse command line for Control type and address */
argIndex =
argControl(argc,
argv,
controlType,
controlAddress);
/* Parse command line for application-specific arguments */
while (argIndex < argc)
{
long argIndexNew;
argIndexNew = argSet(argList, argIndex, argc, argv);
if (argIndexNew <= argIndex)
{
argIndex = argIndexNew;
break;
}
else
{
argIndex = argIndexNew;
}
}
/* Check for unknown/invalid command line arguments */
if (argIndex < argc)
{
mpiPlatformConsole("usage: %s %s\n"
"\t\t[-motor # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes - 1,
MPIXmpMAX_MSs - 1);
exit(MPIMessageARG_INVALID);
}
}
void programInit(MPIControl *control,
MPIControlType controlType,
MPIControlAddress *controlAddress,
MPIMotion *motion,
MPIRecorder *recorder,
MPIAxis *axis,
MPIFilter *filter,
MPIMotor *motor)
{
long returnValue;
/* Create motion controller object */
*control = mpiControlCreate(controlType,
controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize motion controller */
returnValue = mpiControlInit(*control);
msgCHECK(returnValue);
/*
Create a Motion Object and append an Axis
Note 1 - separate axis appends will be required for coordinated motion
Note 2 - Axis Map does not take effect until motion is commanded
*/
*motion = mpiMotionCreate(*control,
motionNumber,
*axis);
msgCHECK(mpiMotionValidate(*motion));
/* Clear out any previous errors */
returnValue = mpiMotionAction(*motion,
MPIActionRESET);
msgCHECK(returnValue);
/* Immediately send Map to controller */
returnValue = mpiMotionAction(*motion,
(MPIAction)MPIActionMAP);
msgCHECK(returnValue);
/* Create a Data Recorder for future use */
*recorder = mpiRecorderCreate(*control,
-1); /* next available recorder number */
msgCHECK(mpiRecorderValidate(*recorder));
/* Create axis, filter and motor controller objects */
/* Create axis */
*axis = mpiAxisCreate(*control,
axisNumber);
msgCHECK(mpiAxisValidate(*axis));
/* Create filter */
*filter = mpiFilterCreate(*control,
axisNumber);
msgCHECK(mpiFilterValidate(*filter));
/* Create motor */
*motor = mpiMotorCreate(*control,
axisNumber);
msgCHECK(mpiMotorValidate(*motor));
}
void programCleanup(MPIControl control,
MPIRecorder recorder,
MPIMotion motion,
MPIAxis axis,
MPIFilter filter,
MPIMotor motor)
{
long returnValue;
/* Delete Objects */
returnValue = mpiMotorDelete(motor);
msgCHECK(returnValue);
returnValue = mpiFilterDelete(filter);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiRecorderDelete(recorder);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
}
double accelActualGet(MPIRecorder recorder,
MPIAxis axis)
{
MPIRecorderRecord *recordPtr;
long recordsRemaining;
long returnValue; /* return value from library */
__int64 v1;
__int64 v2;
returnValue = mpiRecorderRecordConfig(recorder,
(MPIRecorderRecordType)MPIRecorderRecordTypeAXIS,
1,
&axis);
msgCHECK(returnValue);
returnValue = mpiRecorderStart(recorder,
RECORD_COUNT_MAX); /* number of records */
msgCHECK(returnValue);
recordPtr = Record;
recordsRemaining = RECORD_COUNT_MAX;
while (recordsRemaining > 0)
{
long recordsRead;
returnValue = mpiRecorderRecordGet(recorder,
recordsRemaining,
(MPIRecorderRecord *)recordPtr,
&recordsRead);
msgCHECK(returnValue);
recordsRemaining -= recordsRead;
recordPtr += recordsRead;
}
returnValue = mpiRecorderStop(recorder);
if (returnValue == MPIRecorderMessageSTOPPED)
{
returnValue = MPIMessageOK;
}
msgCHECK(returnValue);
v1 = Record[VEL_DELAY - 1].axis[0].actual -
Record[0].axis[0].actual;
v2 = Record[RECORD_COUNT_MAX - 1].axis[0].actual -
Record[RECORD_COUNT_MAX - VEL_DELAY].axis[0].actual;
return ((double)(v2 - v1));
}
void setPositionZero(MPIAxis axis)
{
double actualPosition;
double origin;
long returnValue;
returnValue = mpiAxisActualPositionGet(axis,
&actualPosition);
msgCHECK(returnValue);
returnValue = mpiAxisOriginGet(axis,
&origin);
msgCHECK(returnValue);
actualPosition += origin;
returnValue = mpiAxisOriginSet(axis,
actualPosition);
msgCHECK(returnValue);
mpiPlatformSleep(15);
actualPosition = 0;
returnValue = mpiAxisCommandPositionSet(axis,
actualPosition);
msgCHECK(returnValue);
mpiPlatformSleep(15);
}
void setFilterGains(MPIFilter filter)
{
MPIFilterGain gain;
MPIFilterGainPID *pid;
long returnValue;
/* Get current configuration */
returnValue = mpiFilterGainGet(filter,
MPIFilterGainIndexDEFAULT,
&gain);
msgCHECK(returnValue);
pid = (MPIFilterGainPID *)&gain;
/* Filter Controller control parameters */
pid->gain.proportional = (float)50.0;
pid->gain.integral = (float)0.0;
pid->gain.derivative = (float)200.0;
pid->feedForward.velocity = (float)0.0;
pid->feedForward.acceleration = (float)0.0;
pid->feedForward.friction = (float)0.0;
pid->integrationMax.moving = (float)0.0;
pid->integrationMax.rest = (float)0.0;
pid->output.limit = (float)26213.6;
pid->output.offset = (float)0.0;
/* Set filter configuration */
returnValue = mpiFilterGainSet(filter,
MPIFilterGainIndexDEFAULT,
&gain);
msgCHECK(returnValue);
}
void dither(MPIAxis axis, MPIMotor motor, MPIRecorder recorder)
{
/* Begin "Dither" phase-finding based on motor acceleration sequence */
MPIMotorConfig config2;
double angle;
double delta;
long index;
long returnValue;
angle = 0.0;
delta = 256.0;
/*
Find Field magnetic vector by dithering Stator vector (monitor
direction of acceleration
*/
for (index = 0; index < 8; index++)
{
double accel;
long angleSet;
accel = accelActualGet(recorder,
axis);
if (accel < 0.0)
{
angle += delta;
}
else
{
angle -= delta;
}
angleSet = (long)angle;
returnValue = mpiMotorConfigGet(motor, NULL, &config2);
msgCHECK(returnValue);
config2.Commutation.Offset = angleSet;
returnValue = mpiMotorConfigSet(motor, NULL, &config2);
msgCHECK(returnValue);
delta *= .75;
/* Avoid dual null position issue with first dither */
if ((index == 0) &&
(accel == 0.0))
{
delta *= 2;
}
}
}
float setFineTolerance(MPIAxis axis, float tolerance)
{
MPIAxisConfig axisConfig;
long returnValue;
float oldTolerance;
/* Configure axis */
returnValue = mpiAxisConfigGet(axis,
&axisConfig,
NULL);
msgCHECK(returnValue);
oldTolerance = axisConfig.inPosition.tolerance.positionFine;
/* Set inPositionLimit */
axisConfig.inPosition.tolerance.positionFine = (float)tolerance;
returnValue = mpiAxisConfigSet(axis,
&axisConfig,
NULL);
msgCHECK(returnValue);
return oldTolerance;
}
void configureMotor(MPIMotor motor)
{
MPIXmpCommutationBlock *commutation;
MPIMotorConfig motorConfig;
MPIMotorConfig motorConfig2;
long returnValue;
long nEncCountPerCycle = 4096; /* number of encoder counts per motor revolution */
float OpenDAC_Level = (float)3277.0; /* Open Loop DAC Level where 32767. = 10V) */
float N_CYCLES = (float)3.0; /* number of elec. cycles per motor rev. */
/* Configure Motor */
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfig2);
msgCHECK(returnValue);
/*
Disable limit error, home, amp fault & H/W Limits
Set encoder phase
*/
/* Disable limit error */
motorConfig.event[MPIEventTypeLIMIT_ERROR].action = MPIActionNONE;
motorConfig.event[MPIEventTypeLIMIT_ERROR].trigger.error = (float)50000;
/* Setup commutation */
commutation = &motorConfig2.Commutation;
/* Motor Controller commutation parameters */
commutation->Mode = MPIXmpCommModeOPEN_LOOP;
commutation->Length = nEncCountPerCycle;
commutation->Scale = (float)MPIXmpCOMM_TABLE_SIZE * N_CYCLES /
(float)commutation->Length;
commutation->PhaseDelta = MPIXmpCOMM_120DEGREES;
commutation->OutputLevel = OpenDAC_Level;
commutation->Offset = (long)0;
/* Set up motor to use both Demands (two phases of the three in a motor) */
motorConfig2.demandMode = MPIMotorDemandModeANALOG_DUAL_DAC;
/* Set configuration */
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfig2);
msgCHECK(returnValue);
}
int main(int argc,
char *argv[])
{
MPIControl control; /* motion controller handle */
MPIAxis axis; /* axis handles */
MPIFilter filter; /* filter handles */
MPIMotor motor; /* motor handles */
MPIMotion motion; /* coordinated motion object handles */
MPIRecorder recorder; /* data recorder handle */
MPIControlType controlType;
MPIControlAddress controlAddress;
long returnValue; /* return value from library */
float oldFineTolerance;
/* Perform basic command line parsing. (-control -server -port -trace) */
parseCommandLine(argc,
argv,
&controlType,
&controlAddress,
&axisNumber,
&motionNumber);
programInit(&control,
controlType,
&controlAddress,
&motion,
&recorder,
&axis,
&filter,
&motor);
/* Disable the amplifier */
returnValue = mpiMotorAmpEnableSet(motor,
FALSE);
msgCHECK(returnValue);
/* Set tolerance wide so dither does not trigger a following error */
oldFineTolerance = setFineTolerance(axis, 1.0e10);
/* Set up commutation */
configureMotor(motor);
/* Set up gains to be used after commutation */
setFilterGains(filter);
/* Enable the amplifier */
returnValue = mpiMotorAmpEnableSet(motor,
TRUE);
msgCHECK(returnValue);
/* Perform dither function - this finds motor phasing*/
dither(axis, motor, recorder);
/* Zero Position */
setPositionZero(axis);
/* Return to old fine tolerance */
setFineTolerance(axis, oldFineTolerance);
/* Go Closed Loop -- enable code to co to closed loop commutation */
#if 0
returnValue = mpiMotorCommutationModeSet(motor,
MPIXmpCommModeCLOSED_LOOP);
msgCHECK(returnValue);
#endif
/* Perform certain cleanup actions and delete MPI objects */
programCleanup(control,
recorder,
motion,
axis,
filter,
motor);
return ((int)returnValue);
}
|
