. |
path1.c -- Two-axis path motion, around a rectangle with rounded corners.
#if defined(ARG_MAIN_RENAME) #define main path1Main
argMainRENAME(main, path1) #endif
/* Command line arguments and defaults */ static int32_t axisNumber[AXIS_COUNT] = { 0, 1}; static int32_t motionNumber = 0;
static Arg argList[] = { { "-axis1", ArgTypeLONG, &axisNumber[0], }, { "-axis2", ArgTypeLONG, &axisNumber[1], }, { "-motion", ArgTypeLONG, &motionNumber, }, { NULL, ArgTypeINVALID, NULL, } };
/* Perform command line parsing. (-control -server -port -trace) */ static void parsing(int argc, char *argv[], MPIControlType *controlType, MPIControlAddress *controlAddress) { int32_t 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) { int32_t 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[0] > (MPIControlMAX_OBJECTS - AXIS_COUNT)) || (motionNumber >= MPIControlMAX_OBJECTS)) { mpiPlatformConsole("usage: %s %s\n" "\t\t[-axis1 # (0 .. %d)]\n" "\t\t[-axis2 # (0 .. %d)]\n" "\t\t[-motion # (0 .. %d)]\n", argv[0], ArgUSAGE, MPIControlMAX_OBJECTS - AXIS_COUNT, MPIControlMAX_OBJECTS - 1); exit(MPIMessageARG_INVALID); } }
/* Event service error handler - logs error data to stdout */ static MPI_BOOL MPI_DECL2 eventServiceErrorHandler(MPIControlEventServiceErrorParams *params) { mpiPlatformConsole("Event service thread error occurred:\n" " Error Count = %d Error = 0x%x (%s)\n", params->errorCount, params->message, mpiMessage(params->message, NULL)); return FALSE; }
static void programInit(MPIControl *control, MPIControlType controlType, MPIControlAddress *controlAddress, MPIAxis axis[AXIS_COUNT], MPIMotion *motion, MPIEventMask eventMask, MPINotify *notify, MPIGeometricPath *path) { MPIMotionAxisMap motionAxisMap; MPI_RESULT returnValue; MPIControlEventServiceErrorConfig serviceErrorConfig; int32_t index;
/* Create motion controller object */ returnValue = mpiControlCreate(control, controlType, controlAddress); msgCHECK(returnValue);
/* Create axis objects */ returnValue = mpiAxisCreate(&axis[0], *control, axisNumber[0]); msgCHECK(returnValue);
returnValue = mpiAxisCreate(&axis[1], *control, axisNumber[1]); msgCHECK(returnValue);
/* Create motion supervisor object using MS number 0 */ returnValue = mpiMotionCreate(motion, *control, motionNumber); msgCHECK(returnValue);
/* Map axes to motionSupervisor */ returnValue = mpiMotionAxisMapGet(*motion, &motionAxisMap); msgCHECK(returnValue);
motionAxisMap.count = AXIS_COUNT; for (index = 0; index < AXIS_COUNT; index++) { motionAxisMap.number[index] = axisNumber[index]; }
returnValue = mpiMotionAxisMapSet(*motion, &motionAxisMap); msgCHECK(returnValue);
/* Request notification of all MPI events from motion */ mpiEventMaskCLEAR(eventMask); mpiEventMaskALL(eventMask);
/* Request notification of all events from motion */ returnValue = mpiMotionEventNotifySet(*motion, eventMask, NULL); msgCHECK(returnValue);
/* Create event notification object for motion */ /* Add notify to event manager's list */ returnValue = mpiControlNotifyCreate(*control, notify, eventMask, *motion); msgCHECK(returnValue);
returnValue = mpiGeometricPathCreate(path); msgCHECK(returnValue);
/* Create the event service */ serviceErrorConfig.errorHandler = eventServiceErrorHandler; serviceErrorConfig.errorThreshold = 1;
returnValue = mpiControlEventServiceStart(*control, MPIThreadPriorityHIGHEST, MPIWaitFOREVER, /* FOREVER => enable interrupts */ &serviceErrorConfig); msgCHECK(mpiASSERT(returnValue == MPIMessageOK, "Failed to create the event service")); }
static void deleteObjects(MPIGeometricPath path, MPINotify notify, MPIMotion motion, MPIAxis axis[AXIS_COUNT], MPIControl control) { MPI_RESULT returnValue;
returnValue = mpiGeometricPathDelete(path); msgCHECK(returnValue);
returnValue = mpiControlNotifyDelete(control, notify); msgCHECK(returnValue);
returnValue = mpiMotionDelete(motion); msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis[0]); msgCHECK(returnValue);
returnValue = mpiAxisDelete(axis[1]); msgCHECK(returnValue);
returnValue = mpiControlDelete(control); msgCHECK(returnValue); }
/* Ensure that the E_STOP will finish before the points are empty. Only shorten the EStop time, don't lengthen it. This is important if the point list has an underflow and the path tries to EStop in response. If the EStop takes longer than the amount of motion left in the buffer, violent motion can occur. */ static void setEStop(MPIMotion motion, double timeSlice, int32_t emptyCount) { MPIMotionConfig motionConfig; MPI_RESULT returnValue;
/* Read current motion supervisor configuration */ returnValue = mpiMotionConfigGet(motion, &motionConfig); msgCHECK(returnValue);
if((timeSlice * (double)emptyCount) < motionConfig.decelTime.eStop) { /* Set new E_STOP deceleration rates */ motionConfig.decelTime.eStop = (float)(timeSlice * (double)emptyCount);
returnValue = mpiMotionConfigSet(motion, &motionConfig); msgCHECK(returnValue); } }
static void getCommandPositions2d(MPIAxis axis[AXIS_COUNT], double start[2]) { MPI_RESULT returnValue;
returnValue = mpiAxisCommandPositionGet(axis[0], &start[0]); msgCHECK(returnValue);
returnValue = mpiAxisCommandPositionGet(axis[1], &start[1]); msgCHECK(returnValue); }
/* Sets the configuration needed to set up a path. start is the starting point for the path. is start == NULL, the path will start from 0,0. TimeSlice in seconds. MotionType is typically MPIMotionTypeBSPLINE */ static void pathConfig2d(MPIGeometricPath path, double start[2], double velocity, double accel, MPIGeometricPathInterpolationType interpType, double timeSlice) { MPIGeometricPathParams params; MPI_RESULT returnValue;
returnValue = mpiGeometricPathParamsGet(path, ¶ms); msgCHECK(returnValue);
params.dimension = 2; MPIGeometricPathPointX(params.start) = start? start[0] : 0; /*Allow the user to pass a null start position and get zeroes */ MPIGeometricPathPointY(params.start) = start? start[1] : 0; params.velocity = velocity; params.acceleration = accel; params.deceleration = accel; params.interpolation = interpType; params.timeSlice = timeSlice;
returnValue = mpiGeometricPathParamsSet(path, ¶ms); msgCHECK(returnValue);
}
/* Convenient function for appending an arc to a path */ static void pathArc(MPIGeometricPath path, MPIGeometricPathElement *element, double start, double included, double radius) { MPI_RESULT returnValue;
element->type = MPIGeometricPathElementTypeARC; element->params.arc.angle.start = start; element->params.arc.angle.included = included; element->params.arc.radius = radius;
returnValue = mpiGeometricPathSimpleAppend(path, element); msgCHECK(returnValue); }
/* Convenient function for appending a line to a path. start is a starting point for the move and x and y are relative from that point. If start == NULL, the x and y points are absolute. */ static void pathLine(MPIGeometricPath path, MPIGeometricPathElement *element, double x, double y, double start[2]) { MPI_RESULT returnValue;
element->type = MPIGeometricPathElementTypeLINE; MPIGeometricPathPointX(element->params.line.point) = x + (start? start[0] : 0); /* Make a default of 0,0 start point for null startpoint */ MPIGeometricPathPointY(element->params.line.point) = y + (start? start[1] : 0);
returnValue = mpiGeometricPathSimpleAppend(path, element); msgCHECK(returnValue); }
/* Collects events and prints them out. Event collection ends when a motion done event is found. This function waits for an event for timeout milliseconds. A timeout value of -1 will make the function wait forever for and event. A timeout value of 0 will make the function not wait for an event (falls through). */ static void eventCollectUntilMotionDone(MPINotify notify, int32_t timeout) { MPIEventData eventStatus; MPI_RESULT returnValue = MPIMessageOK;
/* Collect motion events */ while (returnValue == MPIMessageOK) { /* Wait for event */ returnValue = mpiNotifyEventWait(notify, &eventStatus, timeout); msgCHECK(returnValue);
if (eventStatus.eventType == MPIEventTypeMOTION_DONE) { mpiPlatformConsole("Motion Done\n"); break; } else { mpiPlatformConsole("mpiNotifyEventWait(0x%x, 0x%x, %d) returns 0x%x\n" "\teventStatus: type %d objectType %d index %u\n", notify, &eventStatus, MPIWaitFOREVER, returnValue, eventStatus.eventType, eventStatus.objectType, eventStatus.objectIndex); } } }
int main(int argc, char *argv[]) { MPIControl control; MPIAxis axis[AXIS_COUNT]; MPIMotion motion; MPINotify notify; MPIGeometricPath path; MPIGeometricPathElement element; MPIMotionPTPoint *point; int32_t pointCount; MPIControlType controlType; MPIControlAddress controlAddress; MPIEventMask eventMask;
MPI_RESULT returnValue; double start[2];
parsing(argc, argv, &controlType, &controlAddress);
programInit(&control, controlType, &controlAddress, axis, &motion, eventMask, ¬ify, &path);
/* Ensure that the E_STOP will finish before the points are empty. Only shorten the EStop time, don't lengthen it */ setEStop(motion, TIME_SLICE, EMPTY_COUNT);
/* Get the command positions to offset the move */ getCommandPositions2d(axis, start);
/* Set path configurations before appending points */ pathConfig2d(path, start, VELOCITY, ACCEL, INTERPOLATION_TYPE, TIME_SLICE);
/* Append all the path segments */ pathLine(path, &element, 1800, 0, start); pathArc(path, &element, 270.0, 90.0, 200.0); pathLine(path, &element, 2000, 1800.0, start); pathArc(path, &element, 0.0, 90.0, 200.0); pathLine(path, &element, -1800.0, 2000.0, start); pathArc(path, &element, 90.0, 90.0, 200.0); pathLine(path, &element, -2000.0, 200.0, start); pathArc(path, &element, 180.0, 90.0, 200.0); pathLine(path, &element, 0, 0, start);
returnValue = mpiGeometricPathToPTPoints(path, &point, &pointCount, NULL); msgCHECK(returnValue);
returnValue = mpiMotionPTMove(motion, pointCount, point, MPIMotionPathAttrMaskNONE, NULL); msgCHECK(returnValue);
/* Prints events to screen until a motion done */ eventCollectUntilMotionDone(notify, TIMEOUT);
/* Clean up objects */ deleteObjects(path, notify, motion, axis, control);
return returnValue; }
|
| | Copyright © 2001-2009 Motion Engineering |