scdither.c -- Sinusoidal Commutation Initialization Using the Dithering Method
/* ScDither.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.
*/
/*
: 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 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"
#if defined(ARG_MAIN_RENAME)
#define main ScDither
argMainRENAME(main, ScDither)
#endif
#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
MEIRecorderRecord Record[RECORD_COUNT_MAX];
double
accelActualGet(MPIRecorder recorder,
MPIAxis axis);
void
setPositionZero(MPIAxis axis);
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;
MPIControlConfig controlConfig; /* motion controller configuration */
MPIAxisConfig axisConfig; /* axis configuration */
MPIMotorConfig motorConfig;
MEIMotorConfig motorConfigXmp;
MPIFilterGain gain;
MEIFilterGainPID *pid;
MEIXmpData *firmware;
MEIXmpCommutationBlock *commutation;
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. */
long returnValue; /* return value from library */
long argIndex;
/* 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) ||
(axisNumber >= MEIXmpMAX_Axes) ||
(motionNumber >= MEIXmpMAX_MSs)) {
meiPlatformConsole("usage: %s %s\n"
"\t\t[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MEIXmpMAX_Axes - 1,
MEIXmpMAX_MSs - 1);
exit(MPIMessageARG_INVALID);
}
/* Create motion controller object */
control =
mpiControlCreate(controlType,
&controlAddress);
msgCHECK(mpiControlValidate(control));
/* Initialize motion controller */
returnValue = mpiControlInit(control);
msgCHECK(returnValue);
returnValue =
mpiControlMemory(control,
(void**)&firmware,
(void**)NULL);
msgCHECK(returnValue);
/* Create a Data Recorder for future use */
recorder =
mpiRecorderCreate(control);
msgCHECK(mpiRecorderValidate(recorder));
/*
Configure motion controller
Set Sample Rate = 2 kHz
configure for commutation B phase DACs and ADCs
*/
returnValue =
mpiControlConfigGet(control,
&controlConfig,
NULL);
msgCHECK(returnValue);
controlConfig.sampleRate = 2000;
/*
When supporting commutation,
Cmd and Aux DACs must be enabled.
Enabling the same number of Cmd and Aux DACs as there are motors.
*/
controlConfig.cmdDacCount = controlConfig.motorCount;
controlConfig.auxDacCount = controlConfig.motorCount;
/* Configure required number of A/Ds */
controlConfig.adcCount = 8;
returnValue =
mpiControlConfigSet(control,
&controlConfig,
NULL);
msgCHECK(returnValue);
/* 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));
/* Disable the amplifier */
returnValue =
mpiMotorAmpEnableSet(motor,
FALSE);
msgCHECK(returnValue);
/* Configure axis */
returnValue =
mpiAxisConfigGet(axis,
&axisConfig,
NULL);
msgCHECK(returnValue);
/* Set inPositionLimit */
axisConfig.inPosition.tolerance.positionFine = (float)1.0e10;
returnValue =
mpiAxisConfigSet(axis,
&axisConfig,
NULL);
msgCHECK(returnValue);
/* Configure Motor */
returnValue =
mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
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;
/* Disable home event */
motorConfig.event[MPIEventTypeHOME].action = MPIActionNONE;
/* Disable amp fault event */
motorConfig.event[MPIEventTypeAMP_FAULT].action = MPIActionNONE;
/* Disable neg. HW limit event */
motorConfig.event[MPIEventTypeLIMIT_HW_NEG].action = MPIActionNONE;
/* Disable pos. HW limit event */
motorConfig.event[MPIEventTypeLIMIT_HW_POS].action = MPIActionNONE;
/*
Set encoder phase:
0 - for normal
1 - for reversed
*/
#if defined(ReverseEncPhasing)
motorConfig.encoderPhase = 1;
#else
motorConfig.encoderPhase = 0;
#endif
/* Setup commutation */
commutation = &motorConfigXmp.Commutation;
/* Motor Controller commutation parameters */
commutation->Mode = MEIXmpCommModeOPEN_LOOP;
commutation->Length = nEncCountPerCycle;
commutation->Scale = (float)MEIXmpCOMM_TABLE_SIZE * N_CYCLES /
(float)commutation->Length;
commutation->PhaseDelta = MEIXmpCOMM_120DEGREES;
commutation->OutputLevel = OpenDAC_Level;
commutation->Offset = (long)0;
#if defined(ReverseDACPhasing)
motorConfigXmp.Dac.Phase = MEIXmpDACPhaseINVERTED;
#else
motorConfigXmp.Dac.Phase = MEIXmpDACPhaseNORMAL;
#endif
/* Set configuration */
returnValue =
mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Get current configuration */
returnValue =
mpiFilterGainGet(filter,
MEIFilterGainIndexDEFAULT,
&gain);
msgCHECK(returnValue);
pid = (MEIFilterGainPID *)&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,
MEIFilterGainIndexDEFAULT,
&gain);
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)MEIActionMAP);
msgCHECK(returnValue);
/* Enable the amplifier */
returnValue =
mpiMotorAmpEnableSet(motor,
TRUE);
msgCHECK(returnValue);
/* Begin "Dither" phase-finding based on motor acceleration sequence */
{
double angle;
double delta;
long index;
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 =
mpiMotorMemorySet(motor,
&firmware->Motor[axisNumber].Commutation.Offset,
&angleSet,
sizeof(firmware->Motor[axisNumber].Commutation.Offset));
msgCHECK(returnValue);
delta *= .75;
/* Avoid dual null position issue with first dither */
if ((index == 0) &&
(accel == 0.0)) {
delta *= 2;
}
}
}
/* Zero Position */
setPositionZero(axis);
/* Go Closed Loop */
#if 0
returnValue =
meiMotorCommutationModeSet(motor,
MEIXmpCommModeCLOSED_LOOP);
msgCHECK(returnValue);
#endif
/* Delete Objects */
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiMotorDelete(motor);
msgCHECK(returnValue);
returnValue = mpiFilterDelete(filter);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis);
msgCHECK(returnValue);
returnValue = mpiRecorderDelete(recorder);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return ((int)returnValue);
}
double
accelActualGet(MPIRecorder recorder,
MPIAxis axis)
{
long returnValue; /* return value from library */
MEIRecorderRecord *recordPtr;
long recordsRemaining;
long v1;
long v2;
returnValue =
mpiRecorderRecordConfig(recorder,
(MPIRecorderRecordType)MEIRecorderRecordTypeAXIS,
1,
&axis);
msgCHECK(returnValue);
returnValue =
mpiRecorderStart(recorder,
RECORD_COUNT_MAX, /* number of records */
0, /* period (milliseconds) */
0);
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);
meiPlatformSleep(15);
actualPosition = 0;
returnValue =
mpiAxisCommandPositionSet(axis,
actualPosition);
msgCHECK(returnValue);
meiPlatformSleep(15);
}