#include "flexmotn.h" #include <stdio.h> #include <chplot.h> #include <array.h> #include <unistd.h> #define MAX 1000 void ErrorHandler(i32 errorCode, u16 commandID, u16 resourceID); void main(void) { u8 boardID = 1; //Board Identification number u8 axis = 1; //Axis Number i32 status; //Return Status of functions i32 targetPosition = 0; //Target Position in counts or steps u16 axisStatus; //Axis Status u16 csr; //Communication Status Register u32 scanVar; //scanf wants a u16 to put numeric values in. i32 CurrentPosition; //Current position of axis //Variables for modal error handling u16 commandID; //The command ID of the function u16 resourceID; //The resource ID i32 errorCode; //The error generated u16 numberOfSamples = MAX; //number of samples to take u16 timePeriod = 5; //time between samples i32 velocity[MAX]; i32 position[MAX]; i32 XAxis[MAX]; int i; int j; i32 returnData[2]; u16 axisMap = 0x0002; printf("\nAssuming BoardID of 1.\n"); //Check if the board is at power up reset condition status = flex_read_csr_rtn(boardID, &csr); if(status == NIMC_noError) { if (csr & NIMC_POWER_UP_RESET ) { printf("\nThe FlexMotion board is in the reset condition."); printf("\nPlease initialize using initialize.ch. \n"); exit(0); } } printf("Assuming Axis 1.\n"); if (status == NIMC_noError) { //Read the current position of axis status = flex_read_pos_rtn(boardID, axis, &CurrentPosition); printf("Current position of Axis 1: %d", CurrentPosition); printf("\n\nEnter Target Position: "); scanf("%d", &scanVar); targetPosition = (i32)scanVar; fflush(stdin); // flushes stdin. printf("\n"); //Load target position status = flex_load_target_pos(boardID, axis, targetPosition, 0xFF); } if( status == NIMC_noError) { //Start acquire trajectory status = flex_acquire_trajectory_data(boardID, axisMap, numberOfSamples, timePeriod); } //usleep(100); //Tells the computer to sleep for 100 mSec. //This is used on fast computers to give //the flex_acquire_trajector_data enough time //to start before the motion of the axis is started. if (status == NIMC_noError) { //Start Motion on the axis selected status = flex_start(boardID, axis, 0); } //If there was a non-modal error report it if (status) { ErrorHandler(status, 0, 0); } //Wait for move to complete on the axis and //also check for modal errors at the same time do { //Read the current position of axis status = flex_read_pos_rtn(boardID, axis, &position); //Dislay the current position of axis //printf("\rAxis %d position: %10d", axis, position); if (status == NIMC_noError) { status = flex_read_axis_status_rtn(boardID, axis, &axisStatus); } if (status == NIMC_noError) { //Read the Communication Status Register - check the //modal error bit status = flex_read_csr_rtn(boardID, &csr); } if (status == NIMC_noError) { while (csr & NIMC_MODAL_ERROR_MSG) { //Get the command ID, resource and the error code of the modal //error from the error stack on the board status = flex_read_error_msg_rtn(boardID, &commandID, &resourceID, &errorCode); if (status == NIMC_noError) { ErrorHandler(errorCode, commandID, resourceID); } //Read the Communication Status Register - check the //modal error bit status = flex_read_csr_rtn(boardID, &csr); if (status) break; } } //If any function to the FlexMotion board failed due to a //non-modal error exit the loop if(status) { ErrorHandler(status, 0, 0); break; } }while ( ! (axisStatus & (NIMC_MOVE_COMPLETE_BIT | NIMC_AXIS_OFF_BIT ))); //Test against the move complete bit or the axis off bit for(i=0; i<MAX; i++) { status=flex_read_trajectory_data_rtn(boardID, returnData); if(status != NIMC_noError) { break; } position[i]=returnData[0]; velocity[i]=returnData[1]; XAxis[i]=i; } i32 X[i]; i32 Vel[i]; i32 Pos[i]; for(j=0; j<i; j++) { Pos[j]=position[j]; Vel[j]=velocity[j]; X[j]=XAxis[j]; } plotxy(X[], Pos[], "Position vs. Time", "Time", "Position"); plotxy(X[], Vel[], "Velocity vs. Time", "Time", "Velocity"); //plotxy(XAxis, position,"Position vs. Time","Time","Position"); //plotxy(XAxis, velocity, "Velocity vs. Time", "Time", "Velocity"); printf("\n\nFinished.\n"); exit(0); } void ErrorHandler(i32 errorCode, u16 commandID, u16 resourceID) { i8 *errorDescription; //Pointer to i8's - to get error description u32 sizeOfArray; //Size of error description u16 descriptionType; //The type of description to be printed i32 status; //Error returned by function if(commandID == 0) { descriptionType = NIMC_ERROR_ONLY; } else { descriptionType = NIMC_COMBINED_DESCRIPTION; } //First get the size for the error description sizeOfArray = 0; errorDescription = NULL; //Setting this to NULL returns the size required status = flex_get_error_description(descriptionType, errorCode, commandID, resourceID, errorDescription, &sizeOfArray ); //Allocate memory on the heap for the description errorDescription = malloc(sizeOfArray + 1); sizeOfArray++; //So that the sizeOfArray is size of description + NULL character // Get Error Description status = flex_get_error_description(descriptionType, errorCode, commandID, resourceID, errorDescription, &sizeOfArray ); if (errorDescription != NULL) { printf("\n"); printf(errorDescription);//Print description to screen free(errorDescription);//Free allocated memory } else { printf("Memory Allocation Error"); } } /* End Of Program */