ScHall.c -- Single-Axis Sinusoidal Commutation Sample Program with
Hall Initialization
/* ScHall.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.
*/
/*
:This version of the hall initialization code has been modified for motors
with unique hall to winding distances.
Single-Axis Sinusoidal Commutation Sample Program with Hall Initialization
Commutation modification (COMP) parameter added for initial offset
guess. COMP allows for adjusting the offset parameter to accomidate
different magnet to hall spacing of different models and brands of
motors. COMP should be -175 for Anorad LEM-S-1.
This program reads the Hall - I/O bit state, then sets the commutation
table entry point to match the active Hall sensor. This version latches
position and up-dates the commutation entry point based on the first
Hall transition (latches upon a single I/O with a specific edge).
Axis 0: Uses Hall-type initialization with a small trapezoidal
move (closed-loop) to the first Hall transition. Updates
commutation position based on the Hall transition location.
Background: The initial state of the three Hall sensors provides six
possible magnetic sectors of 60 degrees which may contain
the motor field vector (i.e., rotor magnetic position).
This program uses the following logic table:
Mag.Phase Angle: 0 - 60 60 - 120 120 - 180 180 - 240 240 - 300 300 - 360
-----------------------------------------------------------
| | | | | | |
Hall A Sensor | On | On | On | Off | Off | Off |
| | | | | | |
Hall B Sensor | Off | Off | On | On | On | Off |
| | | | | | |
Hall C Sensor | On | Off | Off | Off | On | On |
-----------------------------------------------------------
Hall Logic (C,B,A) (101) (001) (011) (010) (110) (100)
Hall Region 5 1 3 2 6 4
The rotor magnetic field vector will initially be located within
one of these six regions. Sufficient accuracy for closed-loop control
is obtained by assigning the magnetic location to the mid-point of
the active region. Thus, the actual commutation accuracy will be +/- 30
electrical degrees (assuming the halls have been accurately located on your
motor). The motor is configured under closed-loop control and a slow
trapezoidal move is implemented to the first observed Hall transition.
The commutation position is then corrected to reflect the exact magnetic
position of the Hall transition point.
Note - The XMP calculates: A Phase DAC Output = sin(Theta + 120)
B Phase DAC Output = sin(Theta)
Note - See the XMP Installation Manual for information on wiring the
Transceiver I/O to the Hall sensors.
Note - #defs are provided to reverse encoder and DAC phasing. Be aware
that the DAC phasing change should be called only once.
Configure the XMP:
Axis 0 Filter 0 Motor 0
Configure Transceivers as input (default state):
This sample application configures three transceivers on one motor.
Warning! 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 the MEI Sinusoidal Commutation
Documentation.
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 <math.h>
#include "stdmpi.h"
#include "stdmei.h"
#include "sqNodeLib.h"
#include "apputil.h"
#define AXISNUM 0
#define MOTIONNUM 0
#define MOTORNUM 0/* Command line arguments and defaults */
#define INITDIRECTION 1 /*
Direction that the stage will move to find a hall transition.
-1 = move in a negative direction
1 = move in a positive direciton
*/
#define VERBOSE 1 /* 0 for debugging statements off, 1 for on */
#define CYCLES (double)(1.0) /* Number of magnetic cycles per revolution. 1.0 for a linear motor */
#define LENGTH (long)300000 /* Encoder counts per revolution */
#define SCALE (float)(MPIXmpCOMM_TABLE_SIZE * CYCLES / (double)LENGTH)
#define PHASEDELTA (long)MPIXmpCOMM_120DEGREES /* for 3 phase amplifier. MPIXmpCOMM_90DEGREES for 2 or 4 phase */
/*
Commutation modification (COMP) parameter added for initial offset
guess. COMP allows for adjusting the offset parameter to accomidate
different magnet to hall spacing of various models and brands of
motors. Units are 0-1024 = 0-360 electrical degrees.
*/
#define COMP (-175)
long parseCommandLine(long motorNumber, /* Pass the number of the motor */
int argc, /* you want to use.*/
char *argv[],
MPIControlType *controlType,
MPIControlAddress *controlAddress)
{
long argIndex;
/* Command line arguments and defaults */
Arg argList[] =
{
{ "-motor", ArgTypeLONG, &motorNumber, },
{ 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[-axis # (0 .. %d)]\n"
"\t\t[-motion # (0 .. %d)]\n"
"\t\t[-type # (0 .. %d)]\n",
argv[0],
ArgUSAGE,
MPIXmpMAX_Axes,
MPIXmpMAX_MSs - 1,
MPIMotionTypeLAST - 1);
exit(MPIMessageARG_INVALID);
}
return 0;
}
long createObj(long motionNumber,
long motorNumber,
long axisNumber,
MPIControl *control,
MPIMotor *motor,
MPIFilter *filter,
MPIAxis *axis,
MPIMotion *motion,
MPICapture *capture,
MPIControlType *controlType,
MPIControlAddress *controlAddress)
{
MPIControlConfig controlConfig;
int returnValue;
long captureNumber;
/* Obtain a Control handle. */
*control = mpiControlCreate(*controlType,
controlAddress);
msgCHECK(mpiControlValidate(*control));
/* Initialize the controller */
returnValue = mpiControlInit(*control);
if (returnValue != MPIMessageOK) {
fprintf(stderr, "mpiControlInit(0x%x) returns 0x%x: %s\n",
*control,
returnValue,
mpiMessage(returnValue, NULL));
exit(returnValue);
}
/* Create motor object */
*motor = mpiMotorCreate(*control,
motorNumber);
msgCHECK(mpiMotorValidate(*motor));
*filter = mpiFilterCreate(*control,
axisNumber);
msgCHECK(mpiFilterValidate(*filter));
/* Create axis object */
*axis = mpiAxisCreate(*control,
motorNumber);
msgCHECK(mpiAxisValidate(*axis));
*motion = mpiMotionCreate(*control,
motionNumber,
*axis);
msgCHECK(mpiMotionValidate(*motion));
/* Use one capture per motor */
captureNumber = motorNumber;
returnValue = mpiControlConfigGet(*control,
&controlConfig, NULL);
msgCHECK(returnValue);
if(captureNumber >= controlConfig.captureCount)
{
controlConfig.captureCount = captureNumber + 1;
}
returnValue = mpiControlConfigSet(*control,
&controlConfig, NULL);
msgCHECK(returnValue);
/* Create capture object for captureNumber */
*capture = mpiCaptureCreate(*control,
captureNumber);
msgCHECK(mpiCaptureValidate(*capture));
return returnValue;
}
int trapMove(MPIMotion motion,
MPITrajectory trajectory,
double position)
{
MPIMotionParams params; /* Motion parameters */
MPIStatus status; /* Status handle */
int returnValue = 0;
params.trapezoidal.trajectory = &trajectory;
params.trapezoidal.position = &position;
returnValue = mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state) /* Check for a state and take the appropriate action */
{
case MPIStateIDLE: /* IDLE is OK, start a move. */
case MPIStateSTOPPED:
{
/* Start motion */
returnValue = mpiMotionStart(motion,
MPIMotionTypeTRAPEZOIDAL,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateSTOPPING: /* STOPPING is OK, modify a move. */
{
/* Modify the executing motion profile */
returnValue = mpiMotionModify(motion,
MPIMotionTypeTRAPEZOIDAL,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateMOVING: /* MOVING is OK, modify a move. */
{
/* Modify the executing motion profile */
returnValue = mpiMotionModify(motion,
MPIMotionTypeTRAPEZOIDAL,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateERROR: /* ERROR is not OK, return an error (1).*/
{
returnValue = 1;
break;
}
default: /* Don't know what happened, return an error (2).*/
{
returnValue = 2;
break;
}
}
return returnValue;
}
int velMove(MPIMotion motion,
MPITrajectory trajectory)
{
MPIMotionParams params; /* Motion parameters */
MPIStatus status; /* Status handle */
int returnValue = 0;
params.velocity.trajectory = &trajectory;
returnValue =mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
switch (status.state) /* Check for a state and take the appropriate action */
{
case MPIStateIDLE: /* IDLE is OK, start a move. */
case MPIStateSTOPPED:
{
/* Start motion */
returnValue = mpiMotionStart(motion,
MPIMotionTypeVELOCITY,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateSTOPPING: /* STOPPING is OK, modify a move.*/
{
/* Modify the executing motion profile */
returnValue = mpiMotionModify(motion,
MPIMotionTypeVELOCITY,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateMOVING: /* MOVING is OK, modify a move. */
{
/* Modify the executing motion profile */
returnValue = mpiMotionModify(motion,
MPIMotionTypeVELOCITY,
¶ms);
msgCHECK(returnValue);
break;
}
case MPIStateERROR: /* ERROR is not OK, return an error (1). */
{
returnValue = 1;
break;
}
default: /* Don't know what happened, return an error (2). */
{
returnValue = 2;
break;
}
}
return returnValue;
}
long isAxisStill(MPIAxis axis,
MPIMotion motion,
double tolerance)
{
MPIStatus status;
MPIMotionConfig config;
long returnValue = 0;
long time = 100;
double actual, temp, delta;
float tempDecel;
returnValue =
mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
if(!(status.state == MPIStateIDLE))
{
returnValue = mpiMotionConfigGet(motion,
&config,
NULL);
msgCHECK(returnValue);
tempDecel = config.decelTime.stop;
config.decelTime.stop = (float)0.001;
returnValue = mpiMotionConfigSet(motion,
&config,
NULL);
msgCHECK(returnValue);
returnValue = mpiMotionAction(motion,
MPIActionSTOP);
msgCHECK(returnValue);
config.decelTime.stop = tempDecel;
returnValue = mpiMotionConfigSet(motion,
&config,
NULL);
msgCHECK(returnValue);
mpiPlatformSleep(10);
}
returnValue = mpiAxisActualPositionGet(axis, &temp);
msgCHECK(returnValue);
mpiPlatformSleep(100);
returnValue = mpiAxisActualPositionGet(axis, &actual);
msgCHECK(returnValue);
delta = actual - temp;
if(fabs(delta) > (tolerance / 10))
{
printf("\nError - Axis in motion");
exit(-1);
}
return returnValue;
}
long analogDualDemandMode(MPIMotor motor)
{
MPIMotorConfig motorConfig2;
long returnValue = 0;
returnValue = mpiMotorConfigGet(motor,
NULL,
&motorConfig2);
msgCHECK(returnValue);
/* Make sure that the motor uses both Command channels */
motorConfig2.demandMode = MPIMotorDemandModeANALOG_DUAL_DAC;
returnValue = mpiMotorConfigSet(motor,
NULL,
&motorConfig2);
msgCHECK(returnValue);
return returnValue;
}
/*
baseLineSineComm sets the sine comm parameters to fresh settings so that it
is ready to find a hall transition. It does this by doing the following:
1. Get the pointer to the XMP's memory for later funtcions
2. Configure the motor to be in open loop mode with proper sine comm parameters and remove error limit acitons
3. Set the theta and offset commutation parameters to zero to prepare for hall initialization
4. Zero the command and actual position.
5. Return error limit action to its original values
*/
long baselineSineComm(MPIControl control,
MPIMotor motor,
MPIAxis axis,
long length,
float scale,
long phaseDelta)
{
MPIMotorConfig motorConfig;
MPIMotorConfig motorConfigXmp;
MPIXmpData *firmware;
MPIXmpCommutationBlock *Commutation;
MPIXmpCommutationBlock *comm;
long returnValue = 0;
long zero = 0;
long motorNumber;
long tempAction;
returnValue = mpiMotorAmpEnableSet(motor, FALSE); /* Turn amp off while changing commutation parameters */
msgCHECK(returnValue);
mpiPlatformSleep(200); /* Wait for amp to turn off */
mpiMotorNumber(motor,
&motorNumber);
/* Get XMP memory pointer for later memory writes */
returnValue = mpiControlMemory(control,
&firmware,
&Commutation);
msgCHECK(returnValue);
/* Configure motor */
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Disable limit error during virtual move */
tempAction = motorConfig.event[MPIEventTypeLIMIT_ERROR].action;
motorConfig.event[MPIEventTypeLIMIT_ERROR].action = MPIActionNONE;
/* Setup Motor commutation in open-loop mode */
comm = &motorConfigXmp.Commutation;
comm->Mode = 0; /* Set commutation mode to zero for modifying theta */
comm->Length = length;
comm->Scale = scale;
comm->PhaseDelta = phaseDelta;
comm->OutputLevel = 0.0; /* Open-loop DAC level set to zero to keep stage still during virtual move */
/* Set configuration */
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Zero the theta commutation parameter */
returnValue = mpiControlMemorySet(control,
&firmware->Motor[motorNumber].Commutation.Theta, /* destination */
&zero, /* Source */
sizeof(Commutation->Theta));
msgCHECK(returnValue);
mpiControlSampleWait(control,2);
/* Zero actual and command position*/
returnValue = mpiAxisActualPositionSet(axis,
zero);
msgCHECK(returnValue);
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Return limit error to what it was */
motorConfig.event[MPIEventTypeLIMIT_ERROR].action = tempAction;
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
return returnValue;
}
/* getHalls configures XCVRs to get hall sensor information and writes it to iTranState */
long getHalls(MPIMotor motor,
long *iTranState)
{
#define HALL_A_ID (RMBMotorIoConfigXCVR_A)
#define HALL_A_MASK (1<<RMBGeneralIoXCVR_A)
#define HALL_B_ID (RMBMotorIoConfigXCVR_B)
#define HALL_B_MASK (1<<RMBGeneralIoXCVR_B)
#define HALL_C_ID (RMBMotorIoConfigXCVR_C)
#define HALL_C_MASK (1<<RMBGeneralIoXCVR_C)
MPIMotorConfig motorConfig;
MPIMotorConfig motorConfigXmp;
long returnValue = 0;
long io;
/* Configure Motor */
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Verify that transceivers are configured for input (default) */
motorConfigXmp.Io[HALL_A_ID].Type = MPIMotorIoTypeINPUT;
motorConfigXmp.Io[HALL_B_ID].Type = MPIMotorIoTypeINPUT;
motorConfigXmp.Io[HALL_C_ID].Type = MPIMotorIoTypeINPUT;
/* Write XCVR configuration */
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Read general purpose input bits */
returnValue = mpiMotorGeneralIn(motor,
0, /* start bit */
16, /* bit count */
&io);
msgCHECK(returnValue);
/* Write XCVR information to a variable */
*iTranState = 0;
if(io & HALL_A_MASK)
{
*iTranState += 1; /* write a 0^1 */
}
if(io & HALL_B_MASK)
{
*iTranState += 2; /* write a 0^1 */
}
if(io & HALL_C_MASK)
{
*iTranState += 4; /* write a 0^1 */
}
return returnValue;
}
/*
hallOffset() does the following:
1. Decode the hall sensor position from getHalls() (passed in through iTranState).
2. Set the motor to closed loop with that information.
3. Come up with an commutation offset and write it to iOffset
*/
long hallOffset(MPIMotor motor,
long *iOffset,
long *iTranState,
MPICapture capture,
long initDirection)
{
MPIMotorConfig motorConfig;
MPIMotorConfig motorConfigXmp;
MPICaptureConfig captureConfig;
long returnValue = 0;
long motorNumber;
returnValue = mpiMotorNumber(motor, &motorNumber);
msgCHECK(returnValue);
/* Make sure initDirection is valid */
if(!((initDirection == -1) || (initDirection == 1)))
{
printf("Error - Invalid direction");
exit(-1);
}
/*
Clear out capture registers before modifiying them.
This keeps from unintentionally combining capture triggers.
*/
returnValue = mpiCaptureConfigReset(capture);
msgCHECK(returnValue);
/* Dis-arm position capture */
returnValue = mpiCaptureArm(capture,
FALSE);
msgCHECK(returnValue);
/* Wait for capture to disarm before doing capture config */
mpiControlSampleWait(mpiMotorControl(motor), 2);
/* Configure capture for Transceiver input trigger */
returnValue = mpiCaptureConfigGet(capture,
&captureConfig,
NULL);
msgCHECK(returnValue);
/* Set commutation Offset and expected Hall transition */
captureConfig.captureMotorNumber = motorNumber;
captureConfig.feedbackMotorNumber = motorNumber;
captureConfig.type = MPICaptureTypePOSITION;
captureConfig.encoder = MPIMotorEncoderPRIMARY;
captureConfig.captureIndex = 0;
if(initDirection == 1)
{
switch (*iTranState)
{
case 1: /* w/CW rotation: next Hall Region 3, next Hall/Trans. edge B+ */
{
*iOffset = 256 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_1].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 2: /* w/CW rotation: next Hall Region 6, next Hall/Trans. edge C+ */
{
*iOffset = 597 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_2].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 3: /* w/CW rotation: next Hall Region 2, next Hall/Trans. edge A- */
{
*iOffset = 426 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_0].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
case 4: /* w/CW rotation: next Hall Region 5, next Hall/Trans. edge A+ */
{
*iOffset = 938 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_0].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 5: /* w/CW rotation: next Hall Region 1, next Hall/Trans. edge C- */
{
*iOffset = 85 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_2].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
case 6: /* w/CW rotation: next Hall Region 4, next Hall/Trans. edge B- */
{
*iOffset = 768 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_1].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
default:
{
printf("\n Incorrect read of Transceivers \n");
exit(-1);
}
}
}
else if(initDirection == -1)
{
switch (*iTranState)
{
case 1: /* w/CW rotation: next Hall Region 3, next Hall/Trans. edge C+ */
{
*iOffset = 256 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_2].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 2: /* w/CW rotation: next Hall Region 6, next Hall/Trans. edge A+ */
{
*iOffset = 597 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_0].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 3: /* w/CW rotation: next Hall Region 2, next Hall/Trans. edge B- */
{
*iOffset = 426 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_1].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
case 4: /* w/CW rotation: next Hall Region 5, next Hall/Trans. edge B+ */
{
*iOffset = 938 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_1].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeRISING;
break;
}
case 5: /* w/CW rotation: next Hall Region 1, next Hall/Trans. edge A- */
{
*iOffset = 85 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_0].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
case 6: /* w/CW rotation: next Hall Region 4, next Hall/Trans. edge C- */
{
*iOffset = 768 + COMP;
captureConfig.source[MPICaptureSourceMOTOR_IO_2].enabled = TRUE; /* Next expected Hall transition */
captureConfig.edge = MPICaptureEdgeFALLING;
break;
}
default:
{
printf("\n Incorrect read of Transceivers \n");
exit(-1);
}
}
}
/* Write the XCVR and offset information */
returnValue = mpiCaptureConfigSet(capture,
&captureConfig,
NULL);
msgCHECK(returnValue);
/* Get motor configuration again */
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Setup commutation with closed-loop control and new Offset */
motorConfigXmp.Commutation.Mode = MPIXmpCommModeCLOSED_LOOP;
motorConfigXmp.Commutation.Offset = *iOffset; /* Hall updated Offset */
/* Set configuration */
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
mpiMotorAmpEnableSet(motor, /* Now that commutation is set, enable the amp */
TRUE);
return returnValue;
}
/* clearCapture sets the home event action to none and arms the capture */
long clearCapture(MPICapture capture,
MPIMotor motor)
{
MPIMotorLimitConfig eventConfig;
long returnValue;
/* Configure Home Event action */
returnValue = mpiMotorEventConfigGet(motor,
MPIEventTypeHOME,
&eventConfig,
NULL);
msgCHECK(returnValue);
eventConfig.action = MPIActionNONE; /* no event */
eventConfig.trigger.polarity = TRUE;
returnValue = mpiMotorEventConfigSet(motor,
MPIEventTypeHOME,
&eventConfig,
NULL);
msgCHECK(returnValue);
/* Re-arm position capture */
returnValue = mpiCaptureArm(capture,
TRUE);
msgCHECK(returnValue);
return returnValue;
}
long moveToHall(MPIMotion motion,
double vel,
double accel,
float scale,
long initDirection)
{
MPITrajectory trajectory;
long returnValue = 0;
double position;
position = initDirection * 1.5 * (float)MPIXmpCOMM_TABLE_SIZE / (double)scale / 6; /* One and a half hall regions */
trajectory.velocity = vel; /* counts per sec */
trajectory.acceleration = accel; /* counts per sec * sec */
trajectory.deceleration = accel;
trapMove(motion,
trajectory,
position);
return returnValue;
}
/*
Get hall transition does the following:
1. Wait for a capture event to occur.
2. If a capture event does not occur, timeout and exit the program
3. When the capture occurs, subtract out the origin to get the captured actual position
4. Write the captured actual position to latchedPosition
*/
long getHallTransition(MPICapture capture,
double *latchedPosition,
MPIAxis axis,
MPIMotor motor)
{
#define TIMEOUT 20000 /* Timeout value in msec */
#define TIMESTEP 20 /* Amount of time between checking capture status in msec */
MPICaptureStatus captureStatus;
long returnValue;
long done = FALSE;
long captureCount = 1;
double origin;
returnValue = mpiCaptureStatus(capture,
&captureStatus,
NULL);
msgCHECK(returnValue);
/* Poll capture status */
while (!done)
{
if (captureCount > (TIMEOUT / TIMESTEP)) /* If no transition was detected, exit program */
{
printf("\nError - Capture timeout");
mpiMotorAmpEnableSet(motor,
FALSE);
exit(-1);
}
returnValue = mpiCaptureStatus(capture,
&captureStatus,
NULL);
msgCHECK(returnValue);
if (captureStatus.state == MPICaptureStateCAPTURED) { /* Get captured position */
*latchedPosition = captureStatus.latchedValue;
done = TRUE;
}
mpiPlatformSleep(TIMESTEP);
captureCount++;
}
returnValue = mpiAxisOriginGet(axis,
&origin);
msgCHECK(returnValue);
*latchedPosition -= origin;
return returnValue;
}
/*
stopMove stops a move at a specified deceleration rate in counts / sec^2:
1. Make a velocity move motion modify to velocity zero at decel count / sec^2
2. Wait until the velocity falls below tolerance (in counts per second)
3. Set the stop time to zero
4. Call a stop action
5. Set the stop time back to its original value
*/
long stopMove(MPIAxis axis,
MPIMotion motion,
double decel,
double tolerance) /* tolerance is the allowable motor speed in
counts per second.*/
{
MPITrajectory trajectory;
MPIMotionConfig config;
long returnValue = 0;
long time = (long) ((double)1.0 / (double)tolerance * (double)1000.0);
double delta = 1;
double temp, actual;
float tempDecel;
trajectory.velocity = 0;
trajectory.acceleration = decel;
trajectory.jerkPercent = 0;
returnValue = velMove(motion,
trajectory);
msgCHECK(returnValue);
while(fabs(delta)>0)
{
mpiAxisCommandPositionGet(axis, &actual);
mpiPlatformSleep(time);
temp = actual;
mpiAxisCommandPositionGet(axis, &actual);
delta = actual - temp;
}
returnValue = mpiMotionConfigGet(motion,
&config,
NULL);
msgCHECK(returnValue);
tempDecel = config.decelTime.stop;
config.decelTime.stop = 0;
returnValue = mpiMotionConfigSet(motion,
&config,
NULL);
msgCHECK(returnValue);
returnValue = mpiMotionAction(motion,
MPIActionSTOP);
msgCHECK(returnValue);
config.decelTime.stop = tempDecel;
returnValue = mpiMotionConfigSet(motion,
&config,
NULL);
msgCHECK(returnValue);
return returnValue;
}
/* Use an updated iOffset to update the commutation offset. Used after hallOffset() */
long optimizeOffset(MPIMotor motor,
long *iOffset,
long length,
float scale,
long phaseDelta,
double latchedPosition,
long initDirection)
{
MPIMotorConfig motorConfig;
MPIMotorConfig motorConfigXmp;
MPIXmpCommutationBlock *commutation;
long returnValue = 0;
long oldOffset = *iOffset;
if(initDirection == 1)
{
*iOffset += (long)((((float)length)/(((double)length*(double)scale/(double)MPIXmpCOMM_TABLE_SIZE)*12) - latchedPosition)*(scale));
}
else if(initDirection == -1)
{
*iOffset -= (long)((((float)length)/(((double)length*(double)scale/(double)MPIXmpCOMM_TABLE_SIZE)*12) + latchedPosition)*(scale));
}
else
{
printf("\nError - Invalid initDirection");
exit(-1);
}
if(fabs(*iOffset - oldOffset) > (MPIXmpCOMM_TABLE_SIZE / 12))
{
printf("\nError - Inappropriate updated offset value");
printf("\ninitial offset = %d\t updated offset = %d", oldOffset, *iOffset);
mpiMotorAmpEnableSet(motor,
FALSE);
exit(-1);
}
/* Get motor configuration again */
returnValue = mpiMotorConfigGet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
/* Setup commutation with closed-loop control and new Offset */
commutation = &motorConfigXmp.Commutation;
commutation->Offset = *iOffset; /* Hall updated Offset */
/* Set configuration */
returnValue = mpiMotorConfigSet(motor,
&motorConfig,
&motorConfigXmp);
msgCHECK(returnValue);
return returnValue;
}
long deleteObj(MPICapture capture,
MPIFilter filter,
MPIMotion motion,
MPIAxis axis,
MPIMotor motor,
MPIControl control)
{
int returnValue;
/* Delete the remaining handles */
returnValue = mpiCaptureDelete(capture);
msgCHECK(returnValue);
returnValue = mpiMotorDelete(motor);
msgCHECK(returnValue);
returnValue = mpiFilterDelete(filter);
msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion);
msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis);
msgCHECK(returnValue);
returnValue = mpiControlDelete(control);
msgCHECK(returnValue);
return returnValue;
}
long clearError(MPIMotion motion)
{
MPIStatus status; /* Status handle */
long returnValue;
returnValue = mpiMotionAction(motion, MPIActionRESET);
msgCHECK(returnValue);
mpiControlSampleWait(mpiMotionControl(motion), 2);
returnValue = mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
if(status.state == MPIStateERROR)
{
returnValue = mpiMotionAction(motion, MPIActionRESET);
msgCHECK(returnValue);
}
mpiControlSampleWait(mpiMotionControl(motion), 2);
returnValue = mpiMotionStatus(motion,
&status,
NULL);
msgCHECK(returnValue);
if(status.state == MPIStateERROR)
{
returnValue = 3339;
}
return returnValue;
}
long motionAmpEnableSet(MPIMotion *motion,
int state)
{
MPIAxis currentAxis, tempAxis;
MPIObjectMap map;
long returnValue = 0;
long axisIndex, motorIndex;
int numAxes = mpiMotionAxisCount(*motion);
currentAxis = mpiMotionAxisFirst(*motion);
for(axisIndex = 0; axisIndex < numAxes; axisIndex++)
{
returnValue = mpiAxisMotorMapGet(currentAxis,
&map);
msgCHECK(returnValue);
for(motorIndex = 0; motorIndex < MPIXmpMAX_Motors; motorIndex++)
{
if(mpiObjectMapBitGET(map, motorIndex))
{
MPIMotor motor;
motor = mpiMotorCreate(mpiMotionControl(*motion),
motorIndex);
msgCHECK(mpiMotorValidate(motor));
returnValue = mpiMotorAmpEnableSet(motor, state); /* Enable the motor */
msgCHECK(returnValue);
returnValue = mpiMotorDelete(motor);
msgCHECK(returnValue);
}
}
tempAxis = currentAxis;
currentAxis = mpiMotionAxisNext(*motion, tempAxis);
}
return returnValue;
}
int main(int argc, char *argv[])
{
MPIControl control; /* motion controller handle */
MPIAxis axis; /* axis handles */
MPIMotor motor; /* motor handles */
MPIFilter filter; /* filter handles */
MPIMotion motion; /* Coordinated motion object handles */
MPICapture capture;
MPIControlType controlType;
MPIControlAddress controlAddress;
long returnValue = 0; /* return value from library */
long iTranState; /* value of the Hall transceiver word */
long iOffset; /* Commutation vector offset */
long motorNumber = MOTORNUM;
long axisNumber = AXISNUM;
long motionNumber = MOTIONNUM;
long oldOffset;
long initDirection = INITDIRECTION;
long length = LENGTH;
long phaseDelta = PHASEDELTA;
float scale = SCALE;
double latchedPosition;
double vel = 1e6;
double accel = 1e6;
if(!((initDirection == -1) || (initDirection == 1)))
{
printf("Error - Invalid direction");
exit(-1);
}
parseCommandLine(motorNumber,
argc,
argv,
&controlType,
&controlAddress);
createObj(motionNumber,
motorNumber,
axisNumber,
&control,
&motor,
&filter,
&axis,
&motion,
&capture,
&controlType,
&controlAddress);
/* Make sure the axis is still before initialization */
isAxisStill(axis,
motion,
10000);
/* Clear any errors on this motion supervisor */
clearError(motion);
/* Sets motor to use both demand channels for sine commutation */
analogDualDemandMode(motor);
/*
baseLineSineComm sets the sine comm parameters to fresh settings so that it
is ready to find a hall transition. It does this by doing the following:
1. Get the pointer to the XMP's memory for later funtcions
2. Configure the motor to be in open loop mode with proper sine comm parameters and remove error limit acitons
3. Set the theta and offset commutation parameters to zero to prepare for hall initialization
4. Zero the command and actual position.
5. Return error limit action to its original values
*/
baselineSineComm(control,
motor,
axis,
length,
scale,
phaseDelta);
if(VERBOSE)printf("\nbaseline");
/*
getHalls configures XCVRs to get hall sensor information and writes
it to iTranState
*/
getHalls(motor,
&iTranState);
if(VERBOSE)printf("\rgetHalls -- abc %d%d%d", (iTranState & 1) == 1, (iTranState & 2) == 2, (iTranState & 4) == 4);
/*
hallOffset() does the following:
1. Decode the hall sensor position from getHalls() (passed in through iTranState).
2. Set the motor to closed loop with that information.
3. Come up with an commutation offset and write it to iOffset
*/
hallOffset(motor,
&iOffset,
&iTranState,
capture,
initDirection);
if(VERBOSE)printf("\nhalloffset -- abc %d%d%d", (iTranState & 1) == 1, (iTranState & 2) == 2, (iTranState & 4) == 4);
/* clearCapture sets the home event action to none and arms the capture */
clearCapture(capture,
motor);
if(VERBOSE)printf("\nclearcapture");
/************loop is closed*************
now refine the commutation pointer
******************************************/
moveToHall(motion,
vel,
accel,
scale,
initDirection);
if(VERBOSE)printf("\nmovetohall");
/*
Get hall transition does the following:
1. Wait for a capture event to occur.
2. If a capture event does not occur, timeout and exit the program
3. When the capture occurs, subtract out the origin to get the captured actual position
4. Write the captured actual position to latchedPosition
*/
getHallTransition(capture,
&latchedPosition,
axis,
motor);
if(VERBOSE)printf("\ngethalltransition");
if(VERBOSE)printf("\nlatched position = %f",latchedPosition);
/*
stopMove stops a move at a specified deceleration rate in counts / sec^2:
1. Make a velocity move motion modify to velocity zero at decel count / sec^2
2. Wait until the velocity falls below tolerance (in counts per second)
3. Set the stop time to zero
4. Call a stop action
5. Set the stop time back to its original value
*/
stopMove(axis,
motion,
accel,
20);
if(VERBOSE)printf("\nstopmove");
oldOffset = iOffset;
/* Use an updated iOffset to update the commutation offset. Used after hallOffset() */
printf("\ninitial offset = %d", iOffset);
optimizeOffset(motor,
&iOffset,
length,
scale,
phaseDelta,
latchedPosition,
initDirection);
if(VERBOSE)printf("\noptimize");
printf("\nupdated offset = %d", iOffset);
deleteObj(capture,
filter,
motion,
axis,
motor,
control);
return (returnValue);
}
|
