// ************************************************************** // // Module UMAC // This module contains I/O functions for the Delta TAU controller // // // Seva Kaplounenko, Stanford Linear Accelerator Center // 6/14/06 // static char module_name[] = {"UMAC"}; // ************************************************************** // INCLUDES // To avoid compiling errors when using local copy of repository // locate local copy of repository.h file and change all pathes there // to your local copies location. Do not forget change location of repository.h // file in the project //#include "repository.h" //04/24/2014 //02/28/2014 #include DELAY //adds most commonly used functions #include #include #include #include "umac.h" #include #include "UmacLib.h" #include "umacParam.h" //#include "umacuir.h" #define LINE 80 //--------------- // PRIVATE GLOBAL VARIABLES ------------------ //--------------------------------------------------------------- // This table allows several devices of the same type to be // used in the system. // dev_addr, contains the addresses of opened devices // dev_descr, contains the device descriptors of opened devices // dev_count, contains the number of devices open of this type static DWORD dev_addr[UMAC_MAX_NUM_DEV] = {-1}; static int dev_descr[UMAC_MAX_NUM_DEV ]; static int dev_count =0; char message[LINE]; //---------------------------------------------------------------- //umac driver variables static int m_iPmacLibID = -1000; //structure to keep private variables static UMAC_PARAM_STRUCT g_umac_param[UMAC_NUM_AXIS]; //should be extended //to two dimensional array for use with more then one UMAC device //**************************************************************************** // PRIVATE FUNCTIONS DECLARATION // //library functions BOOL umac_get_lib_functions(void); BOOL OpenRuntimeLink(void); BOOL CloseRuntimeLink(void); //umac registration functions DWORD umac_open_dev (DWORD addr ); void umac_close_dev (DWORD dev_ID ); BOOL umac_get_position_scale(int dev_ID, int axis, unsigned int *pScale); static char log_file[128] = {"Positions.txt"}; //**************************************************************************** //-------------------------- UMAC.C FUNCTIONS ----------------------------- // //**************************************************************************** // --- umac.c::umac_init --- // //!\brief Initializes driver and check it status //!\param DWORD [in] //!\param int * [out] for compatibility only //!\param UMAC_PARAM_STRUCT[] [in] //!\retval DWORD //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** DWORD umac_init(DWORD dev_id, int * ID, UMAC_PARAM_STRUCT pParam[]) { int i = 0,j = 0; int retVal; char vs[30]; unsigned long status_x, status_y; BOOL res1,res2, res3; unsigned int mask = 1L; char result[64]; int umac_axes_sequence[UMAC_NUM_AXIS] = {1,2,0}; //After homing axis y first, then x then z //printf("\n"); //printf("Initializing the UMAC...\n"); // Exit here if testing without hardware #ifdef DUMMY_DEVICES *ID = 0; return 1; #endif //add for loop for multiple UMAC devices for(){ //check against device table whether drivers are open if(umac_is_dev_open(dev_id)) { sprintf(message,"%s Tries to open same device\n", module_name); MessagePopup ("UMAC Error", message); } //apply default parameters to private structure for(i = 0; i < UMAC_NUM_AXIS; i++) umac_setParameters(&pParam[i],i+1);//explicitly do real copy of parameters, not a pointers of source structure, //thus we do not need to keep source structure in the memory // Get the PMAC library functions (only 3) if(OpenRuntimeLink()) { if( (*OpenPmacDevicePtr)(dev_id) ) { //register drive in table //check whether the com is open dev_id = umac_open_dev(dev_id); if(dev_id == -1) { sprintf(message,"%s currently no more than one driver allowed\n", module_name); MessagePopup ("UMAC Error", message); } } else { sprintf(message,"%s Failed to open UMAC driver\n", module_name); MessagePopup ("UMAC Error", message); } } else { sprintf(message,"%s DLL library is missed!\n", module_name); MessagePopup ("UMAC Error", message); } //verify library by requesting ROM version retVal = (*PmacGetResponseAPtr)(dev_id,vs,10,"ver");//returns number of read chars vs[retVal-1] = '\0'; //printf("ROM version:%s\n",vs); //slow but safe to take from controller directly for(i = 0; i < UMAC_NUM_AXIS; i++) umac_get_position_scale( dev_id, i+1, &g_umac_param[i].position_scale_factor); //do settings from initialisation structures for(i = 0; i < UMAC_NUM_AXIS ; i++) { umac_set_velocity( dev_id, i+1, g_umac_param[i].velocity); //do not convert to mm/s umac_set_acceleration( dev_id, i+1, g_umac_param[i].acceleration); //do not convert to mm/s*s } //check whether all motors are in close loop mode for(i = 0; i < UMAC_NUM_AXIS; i++) { umac_get_mot_status(dev_id, i+1, &status_x, &status_y); res1 = (status_x & (mask << MOTOR_ACTIVATED_X)); res2 = !(status_x & (mask << MOTOR_OPEN_LOOP_X)); res3 = (status_x & (mask << MOTOR_AMPLIFIER_ENABLED_X)); if(res1 && res2 && res3) { //ready to use //sprintf(result,"UMAC:motor:%i is ready to use\n",(i+1)); //printf(result); } else { //do initialization MessagePopup ("ATTENTION!", "Umac is about to perform startup initialization\nAll motors will move!\nRESTART WILL BE REQUIRED UPON COMPLETION"); /*//disable all plcs retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"displc2,3,4,5");//returns number of read chars if(retVal <0) printf("initialization is failed"); */ //cvi_Delay(3.0); Delay(3.0); //02/28/2014 //03/05/2014 enable plc 10 retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"q1443=-9"); retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"q1442=-138"); retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc10");//returns number of read chars if(retVal <0) printf("initialization is failed\n"); //cvi_Delay(3.0); Delay(3.0); //02/28/2014 //03/05/2014 enable plc 5 retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc5");//returns number of read chars if(retVal <0) { sprintf(message,"initialization failed\n"); MessagePopup ("UMAC Error", message); } //cvi_Delay(3.0); Delay(3.0); //02/28/2014 // xm=x/1000; ym=y/1000; zm=z/1000; wzm=wz/1000; vym=vy/1000; //rxm=rx/3.14*360; rym=ry/3.14*360; rzm=rz/3.14*360; //try to get away from obstacles ///res = umac_abs_move(dev_id, 3, UMAC_AXIS3_ACCELERATION, UMAC_AXIS3_VELOCITY, -150); ///delay(0.25); //res = umac_abs_move(umac_ID, 2, UMAC_AXIS2_ACCELERATION, UMAC_AXIS2_VELOCITY, ym); //delay(0.25); //res = umac_abs_move(umac_ID, 1, UMAC_AXIS1_ACCELERATION, UMAC_AXIS1_VELOCITY, zm); //delay(0.25); ///if(res <0) /// message("initialization is failed"); //10/20/2014 Set the homing axis order,"p812=$123" -> #1=axis 3(x), #2=axis2 (y), #3=axis 1 (z) retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"p812=$98765123");//returns number of read chars if(retVal <0) { sprintf(message,"initialization failed\n"); MessagePopup ("UMAC Error", message); } //cvi_Delay(3.0); Delay(3.0); //10/20/2014 //04/01/2014 Set the homing mask to home z, x and y(1+2+4) retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"p810=7");//returns number of read chars if(retVal <0) { sprintf(message,"initialization is failed\n"); MessagePopup ("UMAC Error", message); } //cvi_Delay(3.0); Delay(3.0); //02/28/2014 //03/05/2014 home retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"b1200r");//returns number of read chars if(retVal <0) { sprintf(message,"initialization is failed\n"); } //cvi_Delay(3.0); Delay(130); //02/28/2014 /* 03/05/2014 //run plc3 to synchronize z and w axes retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc3");//returns number of read chars if(retVal <0) printf("initialization is failed\n"); //cvi_Delay(5.0); Delay(5.0); //20/28/2014 //run plc2 (safety) and plc 4 to manage switches retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc2,4");//returns number of read chars if(retVal <0) printf("initialization is failed\n"); // cvi_Delay(3.0); Delay(3.0); //02/28/2014 //and finally run stages to close feedback loop retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc5");//returns number of read chars if(retVal <0) printf("initialization is failed\n"); */ umac_exit(dev_id); sprintf(message, "%s Please restart program after all motors stop\n", module_name); //stop_error_msg(module_name,"Allow All Motion to Stop Before Hitting OK\nThe Progarm Must Be Restarted"); MessagePopup ("UMAC Start-up Initialization Complete", message); return -1; } } *ID = (int)dev_id; //cvi_Delay(10.); // Move to initial positions for(j = 0; j < UMAC_NUM_AXIS; j++) { i = umac_axes_sequence[j]; //set a sequence for axes to move umac_abs_move(*ID,i+1, g_umac_param[i].acceleration,g_umac_param[i].velocity,g_umac_param[i].init_offset); do { Delay(2.); }while(umac_AxisIsMoving(*ID,i+1)); } return dev_id; } // //**************************************************************************** // --- umac.c::umac_go_home --- // //!\brief Moves motor to home position. The velocity and acceleration are specified. //!\param DWORD [in] //!\param int [in] for compatibility only //!\param double [in] //!\param double [in] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_go_home(DWORD dev_ID, int axis, double acc, double vel) { /* Declare variables */ // umac_go_home is not implemented //int dev_open; /* Done */ return; } // //**************************************************************************** // --- umac.c::umac_exit --- // //!\brief Closes driver //!\param DWORD [in] //!\retval none //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_exit(DWORD dev_ID) { BOOL res =TRUE; int status =0; /* Exit here if testing without hardware */ #ifdef DUMMY_DEVICES return; #endif res = (*ClosePmacDevicePtr)(dev_ID); //chec all umac devices if use more then one driver umac_close_dev(dev_ID); status = ReleaseExternalModule (m_iPmacLibID); if(status < 0 ) { sprintf(message,"DLL unload problem:%i\n",status); MessagePopup ("UMAC Error", message); } /* Done */ return; } // //**************************************************************************** // --- umac.c::umac_open_dev --- // //!\brief Checks whether device is open, and let it open if it is not opened //!\param DWORD //!\retval DWORD //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** DWORD umac_open_dev(DWORD addr) { /* Declare variables */ int i; int dev_ID = -1; int descr =1; /* Initialization */ dev_ID = -1; /* Check to see if the device is already in the device table */ for (i = 0; i < UMAC_MAX_NUM_DEV; i++) //UMAC_MAX_NUM_DEV=1 usually 03/28/2014 { if (dev_addr[i] == addr) { dev_ID = addr; return dev_ID; } } /* Find an ID for the device */ for (i = 0; i < UMAC_MAX_NUM_DEV; i++) { if (dev_addr[i] == -1) { dev_addr[i] = addr; dev_ID = addr; dev_descr[i] = descr; dev_count++; break; } } /* If an entry could not be opened in the device table, return an error */ if (dev_ID < 0) { sprintf(message,"%s The umac device could not be opened\n", module_name); MessagePopup ("UMAC Error", message); return -1; } /* Return device ID */ return dev_ID; } // //**************************************************************************** // --- umac.c::umac_close_dev --- // //!\brief Does record at device table upon closing //!\param DWORD //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_close_dev(DWORD addr) { int i = 0; /* Close the device */ for (i = 0; i < UMAC_MAX_NUM_DEV; i++) { if (dev_addr[i] == addr) { dev_addr[i] = -1; break; } } return; } // //**************************************************************************** // --- umac4.c::umac_is_dev_open --- // //!\brief Checks whether device is open using address //!\param int //!\retval int //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** int umac_is_dev_open(DWORD dev_address) { int i = 0; for (i = 0; i < UMAC_MAX_NUM_DEV; i++) { if (dev_addr[i] == dev_address) { return 1; } } return 0; } // //**************************************************************************** // --- umac.c::umac_homing --- // //!\brief returns 0 if not moving, othewise returns 1 //!\param int - device id [in] //!\param int - axis [in] //!\param int - direction [in] //!\retval int - 1 negative limit hit, 2 - positive, 0 - none //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** int umac_homing(DWORD dev_ID, int axis,int homing_direction) { int retVal =0; /* Declare variables */ //The function umac_homing is not impemented, motor moves to zero position instead //umac_abs_move( dev_ID, axis,g_umac_param[axis-1].acceleration,g_umac_param[axis-1].home_velocity,0.0); /* Check for command errors */ //umac_read_error_status(dev_ID); return retVal; } // //**************************************************************************** // --- umac.c::umac_moveToLimit --- // //!\brief returns 0 if not moving, othewise returns 1 //!\param int - device id [in] //!\param int - axis [in] //!\param int - direction [in] //!\param int - acceleration [in] //!\param int - velocity [in] //!\retval int //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** int umac_moveToLimit(DWORD dev_ID, int axis,int moving_direction, double acc, double vel, double step, double offset) { int retVal =0; /* Declare variables */ //The function umac_moveToLimit is not impemented, motor moves to zero position instead //umac_abs_move( dev_ID, axis,g_umac_param[axis-1].acceleration,g_umac_param[axis-1].home_velocity,0.0); /* Check for command errors */ //umac_read_error_status(dev_ID); return retVal; } // //**************************************************************************** // --- umac.c::umac_LimitSwitchesStatus --- // //!\brief Returns limit switches status //!\param int - device id [in] //!\param int - axis [in] //!\retval int - 1 negative limit hit, 2 - positive, 0 - none //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** int umac_LimitSwitchesStatus(DWORD dev_ID, int axis) { /* Declare variables */ unsigned long status_x, status_y; unsigned int mask = 1L; umac_get_mot_status(dev_ID, axis, &status_x, &status_y); if((status_x & (mask << MOTOR_POSITIVE_LIMIT_SET_X))) return 2; if((status_x & (mask << MOTOR_NEGATIVE_LIMIT_SET_X))) return 1; return 0; } // //**************************************************************************** // --- umac.c::umac_getParameters --- // //!\brief Get parameters from private umac_param_struct //!\param LPUMAC_PARAM_STRUCT [out] //!\retval none //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_getParameters(LPUMAC_PARAM_STRUCT pParam, int axis) { int idx = 0; if(axis > UMAC_NUM_AXIS || axis < 1) { sprintf(message,"%s axis number out of range\n", module_name); MessagePopup ("UMAC Error", message); } idx = axis - 1; pParam->cs_counts_per_mm = g_umac_param[idx].cs_counts_per_mm ; pParam->position_scale_factor = g_umac_param[idx].position_scale_factor ; pParam->velocity = g_umac_param[idx].velocity ; pParam->acceleration = g_umac_param[idx].acceleration ; pParam->home_acceleration = g_umac_param[idx].home_acceleration ; pParam->axis_on = g_umac_param[idx].axis_on ; pParam->home_velocity = g_umac_param[idx].home_velocity ; pParam->home_acceleration = g_umac_param[idx].home_acceleration ; pParam->home_direction = g_umac_param[idx].home_direction ; } // //**************************************************************************** // --- umac.c::umac_setParameters --- // //!\brief Set parameters of umac_param_struct //!\param LPUMAC_PARAM_STRUCT [in] //!\retval none //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_setParameters(LPUMAC_PARAM_STRUCT pParam, int axis) { int idx = 0; if(axis > UMAC_NUM_AXIS || axis < 1) { sprintf(message,"%s axis number out of range\n", module_name); MessagePopup ("UMAC Error", message); } idx = axis - 1; g_umac_param[idx].cs_counts_per_mm = pParam->cs_counts_per_mm ; g_umac_param[idx].position_scale_factor = pParam->position_scale_factor ; g_umac_param[idx].velocity = pParam->velocity ; g_umac_param[idx].acceleration = pParam->acceleration ; g_umac_param[idx].init_offset = pParam->init_offset ; g_umac_param[idx].home_acceleration = pParam->home_acceleration ; g_umac_param[idx].axis_on = pParam->axis_on ; g_umac_param[idx].home_velocity = pParam->home_velocity ; g_umac_param[idx].home_acceleration = pParam->home_acceleration ; g_umac_param[idx].home_direction = pParam->home_direction ; } // //**************************************************************************** // --- umac.c::umac_get_lib_functions --- // //!\brief Gets pointers to DLL functions //!\param none //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_get_lib_functions(void) { int status =0; //take this function name from manual (PCOMMPRO2 Software reference manual 03/28/2014) OpenPmacDevicePtr = GetExternalModuleAddr (m_iPmacLibID, "OpenPmacDevice", &status); if (OpenPmacDevicePtr == NULL) { sprintf(message, "Error at %s:status=%i\n", "OpenPmacDevice",status); MessagePopup ("UMAC Error", message); return FALSE; } //PmacGetResponseAPtr = GetExternalModuleAddr (m_iPmacLibID, "PmacGetResponseA", &status); PmacGetResponseAPtr = GetExternalModuleAddr (m_iPmacLibID, "PmacGetResponseA", &status); if (PmacGetResponseAPtr == NULL) { sprintf(message,"Error at %s:status=%i\n", "PmacGetResponseA",status); MessagePopup ("UMAC Error", message); return FALSE; } ClosePmacDevicePtr = GetExternalModuleAddr (m_iPmacLibID, "ClosePmacDevice", &status); if (ClosePmacDevicePtr == NULL) { sprintf(message,"Error at %s:status=%i\n", "ClosePmacDevice",status); MessagePopup ("UMAC Error", message); return FALSE; } return TRUE; } // //**************************************************************************** // --- umac.c::OpenRuntimeLink --- // //!\brief Opens link to dll library //!\param none //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL OpenRuntimeLink(void) { m_iPmacLibID = LoadExternalModuleEx (DRIVERNAME, __CVIUserHInst); if(m_iPmacLibID > 0) { return umac_get_lib_functions(); //return TRUE; } else return FALSE; } // //**************************************************************************** // --- umac.c::CloseRuntimeLink --- // //!\brief Closes link to dll library //!\param none //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL CloseRuntimeLink(void) { int status = 0; status = ReleaseExternalModule (m_iPmacLibID); if(status < 0) return FALSE; else return TRUE; } // //**************************************************************************** // --- umac.c::umac_command --- // //!\brief Sends text command an get reply //!\param DWORD [in] //!\param char * [out] //!\param int [in] //!\param char* [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_command(DWORD id,char * result , int rlength, char * command ) { int retVal = -1; retVal = (*PmacGetResponseAPtr)(id,result,rlength,command);//returns number of read chars if(retVal <=0) return FALSE; result[retVal-1] = '\0'; //for debug purposes only //printf("%s\n",result); return TRUE; } // //**************************************************************************** // --- umac.c::umac_get_axis --- // //!\brief Returns current motor number //!\param DWORD [in] //!\retval int //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** int umac_get_axis( DWORD id) { char result[10]; int retVal = -1; #ifdef DUMMY_DEVICES return 1; #endif retVal = (*PmacGetResponseAPtr)(id,result,10,"#");//returns number of read chars if(retVal <=0) return -1; return atoi(result); } // //**************************************************************************** // --- umac.c::umac_set_axis --- // //!\brief Sets current motor number //!\param DWORD [in] //!\param int [in] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_axis( DWORD id , int axis) { char result[10]; char command[20]; int retVal = -1; if(axis > UMAC_NUM_AXIS || axis < 1) { sprintf(message,"umac: axis number out of range\n"); MessagePopup ("UMAC Error", message); return ; } sprintf(command,"# %i",axis); retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <0) { printf(message,"umac_set_axis failed!\n"); MessagePopup ("UMAC Error", message); } } // //**************************************************************************** // --- umac.c::umac_get_velocity --- // //!\brief Returns motor velocity //!\param DWORD [in] //!\param int [in] //!\retval double //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** double umac_get_velocity( DWORD id, int motor) { char result[16]; int retVal = -1; char command[32]; #ifdef DUMMY_DEVICES return 1.; #endif switch(motor) { case umac_z: strcpy(command,"p203"); break; case umac_x: strcpy(command,"p204"); break; case umac_y: strcpy(command,"p205"); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message,"get_velocity:No such axis!\n"); MessagePopup ("UMAC Error", message); return 0; } retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars if(retVal <=0) return -1; return atof(result)/1000.; } // //**************************************************************************** // --- umac.c::umac_set_velocity --- // //!\brief Sets motor velocity [mm/s] //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_velocity( DWORD id, int motor, double velocity) { char result[16]; int retVal = -1; char command[32]; char abs_command[32]; double abs_velocity; int iabs_velocity; abs_velocity = velocity*g_umac_param[motor-1].cs_counts_per_mm; // counts/ms iabs_velocity = (int)abs_velocity; sprintf(abs_command,"i%i22=%i",motor,iabs_velocity); velocity*=1000.;//convert to mm switch(motor) { case umac_z: sprintf(command,"p203=%.3f",velocity); break; case umac_x: sprintf(command,"p204=%.3f",velocity); break; case umac_y: sprintf(command,"p205=%.3f",velocity); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message,"set_velocity:No such axis!\n"); MessagePopup ("UMAC Error", message); break; } retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message,"umac_set_velocity failed\n"); MessagePopup ("UMAC Error", message); } retVal = (*PmacGetResponseAPtr)(id,result,0,abs_command);//returns number of read chars if(retVal <0) { sprintf(message,"umac_set_velocity failed\n"); MessagePopup ("UMAC Error", message); } } // //**************************************************************************** // --- umac.c::umac_get_acceleration --- // //!\brief Returns motor acceleration //!\param DWORD [in] //!\param int [in] //!\retval double //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** double umac_get_acceleration( DWORD id, int motor) { char result[16]; int retVal = -1; char command[32]; #ifdef DUMMY_DEVICES return 0.001; #endif switch(motor) { case umac_z: strcpy(command,"p206"); break; case umac_x: strcpy(command,"p207"); break; case umac_y: strcpy(command,"p208"); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message,"get_acceleration:No such axis!\n"); MessagePopup ("UMAC Error", message); return 0; } retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars if(retVal <=0) return -1; return atof(result)/1000.; } // //**************************************************************************** // --- umac.c::umac_set_acceleration --- // //!\brief Sets motor acceleration //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_acceleration( DWORD id, int motor, double acceleration) { char result[16]; int retVal = -1; char command[32]; char abs_command[32]; double abs_acceleration; abs_acceleration = acceleration*g_umac_param[motor-1].cs_counts_per_mm; abs_acceleration/=1000.; // counts/ms^2 sprintf(abs_command,"i%i19=%f",motor,abs_acceleration); acceleration*=1000.;//convert to mm switch(motor) { case umac_z: sprintf(command,"p206=%.3f",acceleration); break; case umac_x: sprintf(command,"p207=%.3f",acceleration); break; case umac_y: sprintf(command,"p208=%.3f",acceleration); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message,"set_acceleration:No such axis!"); MessagePopup ("UMAC Error", message); break; } retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message,"umac_set_acceleration failed\n"); MessagePopup ("UMAC Error", message); } retVal = (*PmacGetResponseAPtr)(id,result,0,abs_command);//returns number of read chars if(retVal <0) { sprintf(message,"umac_set_acceleration failed\n"); MessagePopup ("UMAC Error", message); } } // //**************************************************************************** // --- umac.c::umac_get_distance --- // //!\brief Returns motor relative distance to move //!\param DWORD [in] //!\param int [in] //!\retval double //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** double umac_get_distance( DWORD id, int motor) { char result[16]; int retVal = -1; char command[32]; #ifdef DUMMY_DEVICES return 0.011; #endif switch(motor) { case umac_z: strcpy(command,"p200"); break; case umac_x: strcpy(command,"p201"); break; case umac_y: strcpy(command,"p202"); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message,"get_distance:No such axis!\n"); MessagePopup ("UMAC Error", message); return 0; } retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars if(retVal <=0) return -1; return atoi(result)/1000.; } // //**************************************************************************** // --- umac.c::umac_set_distance --- // //!\brief Sets corresponding motor relative distance to move //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_distance( DWORD id, int motor, double distance) { char result[16]; int retVal = -1; char command[32]; distance*=1000.;//convert to mm switch(motor) { case umac_z: sprintf(command,"p200=%.3f",distance); break; case umac_x: sprintf(command,"p201=%.3f",distance); break; case umac_y: sprintf(command,"p202=%.3f",distance); break; case umac_zp: //sprintf(command,"#4p"); break; case umac_a: //sprintf(command,"#5p"); break; case umac_v: //sprintf(command,"#6p"); break; case umac_w: //sprintf(command,"#7p"); break; case umac_b: //sprintf(command,"#8p"); break; case umac_c: //sprintf(command,"#9p"); break; default: sprintf(message, "set_distance:No such axis!\n"); MessagePopup ("UMAC Error", message); break; } retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_set_distance failed\n"); MessagePopup ("UMAC Error", message); } } // //**************************************************************************** // --- umac.c::umac_get_mot_position --- // //!\brief Returns actual motor position //!\param DWORD [in] //!\param int [in] //!\param double [out] //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_get_mot_position(DWORD dev_ID, int axis, double* pos) { char result[16]; int retVal = -1; char command[32]; #ifdef DUMMY_DEVICES return 1; #endif // distance*=1000.;//convert to mm switch(axis) { case umac_z: sprintf(command,"#1p"); break; case umac_x: sprintf(command,"#3p"); break; case umac_y: sprintf(command,"#2p"); break; case umac_zp: sprintf(command,"#4p"); break; case umac_a: sprintf(command,"#5p"); break; case umac_v: sprintf(command,"#6p"); break; case umac_w: sprintf(command,"#7p"); break; case umac_b: sprintf(command,"#8p"); break; case umac_c: sprintf(command,"#9p"); break; default: sprintf(message, "umac_get_mot_position:No such axis!\n"); MessagePopup ("UMAC Error", message); return FALSE; } retVal = (*PmacGetResponseAPtr)(dev_ID,result,10,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_get_mot_position is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } *pos = atof(result); // [counts] (*pos) /= g_umac_param[axis-1].cs_counts_per_mm; //[mm] (*pos) /= 1000.; // [m ] return TRUE; } // //**************************************************************************** // --- umac.c::umac_get_position_scale --- // //!\brief Finds scale applied to axis //!\param DWORD [in] //!\param int [in] //!\param unsigned int [out] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_get_position_scale(int dev_ID, int axis, unsigned int *pScale) { char result[16]; int retVal = -1; char command[32]; // distance*=1000.;//convert to mm switch(axis) { case umac_z: sprintf(command,"i108"); break; case umac_x: sprintf(command,"i308"); break; case umac_y: sprintf(command,"i208"); break; case umac_zp: //sprintf(command,"i408"); break; case umac_a: //sprintf(command,"i508"); break; case umac_v: //sprintf(command,"i608"); break; case umac_w: //sprintf(command,"i708"); break; case umac_b: //sprintf(command,"i808"); break; case umac_c: //sprintf(command,""i908""); break; default: sprintf(message, "umac_get_position_scale:No such axis!\n"); MessagePopup ("UMAC Error", message); return FALSE; } retVal = (*PmacGetResponseAPtr)(dev_ID,result,10,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_get_position_scale is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } *pScale = atoi(result) * 32; //5 bits offset return TRUE; } // //**************************************************************************** // --- umac.c::umac_rel_move --- // //!\brief Moves axis to specified distance [m] //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\param double [in] //!\param double [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_rel_move(DWORD dev_ID, int axis, double acceleration, double velocity, double distance) { char result[16]; int retVal = -1; char command[32]; umac_set_distance( dev_ID, axis,distance); umac_set_velocity( dev_ID, axis,velocity); umac_set_acceleration( dev_ID, axis,acceleration); switch(axis) { case umac_z: //sprintf(command,"b4r"); break; case umac_x: //sprintf(command,"b5r"); break; case umac_y: //sprintf(command,"b6r"); break; default: sprintf(message, "umac_rel_move:No such axis!\n"); MessagePopup ("UMAC Error", message); return FALSE; } retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_rel_move is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } return TRUE; } // //**************************************************************************** // --- umac.c::umac_AxisIsMoving --- // //!\brief returns 0 if not moving, othewise returns 1 //!\param int - device id [in] //!\param int - axis [in] //!\retval int //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_AxisIsMoving(DWORD dev_ID, int axis) { unsigned long status_x, status_y; BOOL res1,res2, res3; unsigned int mask = 1L; umac_get_mot_status(dev_ID, axis, &status_x, &status_y); res1 = !(status_y & (mask << MOTOR_INPOSITION_Y)); res2 = !(status_x & (mask << MOTOR_DESIRED_VELOCITY0_X)); res3 = (status_x & (mask << MOTOR_MOVE_TIMER_ACTIVE_X)); return res1 && res2 && res3; } // //**************************************************************************** // --- umac.c::umac_abs_move --- // //!\brief Moves axis to specified position [m] //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\param double [in] //!\param double [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_abs_move(DWORD dev_ID, int axis, double acceleration, double velocity, double position) { char result[16]; int retVal = -1; char command[32]; umac_set_velocity( dev_ID, axis,velocity); umac_set_acceleration( dev_ID, axis,acceleration); sprintf(command,"#%i j=%f",axis, (position*1000.)*g_umac_param[axis-1].cs_counts_per_mm); retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_abs_move is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } return TRUE; } // //**************************************************************************** // --- umac.c::umac_get_mot_status --- // //!\brief Returns motor status //!\param DWORD [in] //!\param int [in] //!\param unsigned long * [out] //!\param unsigned long * [out] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_get_mot_status(DWORD dev_ID, int axis, unsigned long * status_x, unsigned long * status_y) { char result[30]; int retVal = -1; char command[32]; unsigned long xreg = 0L, yreg = 0L; //corresponds to registers X and Y on DELTA TAU controller char sxreg[14], syreg[14]; //and corresponding to them strings char shxreg[14], shyreg[14]; //and corresponding to them strings sprintf(command,"#%i?",axis); //03/19/2014 //retVal = (*PmacGetResponseAPtr)(dev_ID,result,16,command);//returns number of read chars retVal = (*PmacGetResponseAPtr)(dev_ID,result,30,command);//returns number of read chars result[retVal-1] = '\0'; if(retVal <0) { sprintf(message, "umac_get_mot_status is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } strncpy(sxreg,result,6); sxreg[6] = '\0'; strcpy(syreg,&result[6]); syreg[6] = '\0'; sprintf(shxreg,"0x%s",sxreg); sprintf(shyreg,"0x%s",syreg); sscanf(shxreg,"%x",&xreg); sscanf(shyreg,"%x",&yreg); *status_x = xreg; *status_y = yreg; return TRUE; } // //**************************************************************************** // --- umac.c::umac_get_cs_mot_status --- // //!\brief Returns coordinate system status //!\param DWORD [in] //!\param int [in] //!\param unsigned long * [out] //!\param unsigned long * [out] //!\param unsigned long * [out] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_get_cs_status(DWORD dev_ID, int cs, unsigned long * status_x, unsigned long * status_y1, unsigned long * status_y2) { char result[24]; int retVal = -1; char command[32]; unsigned long xreg = 0L, y1reg = 0L,y2reg = 0L ; //corresponds to registers X and Y on DELTA TAU controller char sxreg[14], sy1reg[14], sy2reg[14]; //and corresponding to them strings char shxreg[14], shy1reg[14],shy2reg[14]; //and corresponding to them strings sprintf(command,"&%i??",cs); retVal = (*PmacGetResponseAPtr)(dev_ID,result,20,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_get_cs_status is failed\n"); MessagePopup ("UMAC Error", message); return FALSE; } strncpy(sxreg,result,6); sxreg[6] = '\0'; strncpy(sy1reg,&result[6],6); sy1reg[6] = '\0'; strncpy(sy2reg,&result[12],6); sy2reg[6] = '\0'; sprintf(shxreg,"0x%s",sxreg); sprintf(shy1reg,"0x%s",sy1reg); sprintf(shy2reg,"0x%s",sy2reg); sscanf(shxreg,"%x",&xreg); sscanf(shy1reg,"%x",&y1reg); sscanf(shy2reg,"%x",&y2reg); *status_x = xreg; *status_y1 = y1reg; *status_y2 = y2reg; return TRUE; } // //**************************************************************************** // --- umac.c::umac_set_z_break --- // //!\brief Sets Z break in manual mode //!\param DWORD [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_set_z_break_manual( DWORD id, BOOL state) { char result[16]; int retVal = -1; char command[32]; if(state) { strcpy(command,"q2=1"); } else { strcpy(command,"q2=0"); } retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <=0) return FALSE; return TRUE; } // //**************************************************************************** // --- umac.c::umac_zero --- // //!\brief This function sets the indexer position counter to zero. //!\param DWORD [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_zero(DWORD dev_ID, int axis) { char result[16]; int retVal = -1; char command[32]; BOOL confirmation = TRUE; confirmation = ConfirmPopup ("Reset axis zero position!", "Are you sure to set current position to be zero?"); if(confirmation) { sprintf(command,"#%ihmz",axis); retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars if(retVal <=0) return FALSE; } return TRUE; } // //**************************************************************************** // --- umac.c::umac_set_position --- // //!\brief This function sets the indexer position counter to given position //!\param DWORD [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_position(DWORD dev_ID, int axis, double position) { /* Declare variables */ /* Check input parameters */ //umac_set_position is not implemented yet /* Done */ return; } // //**************************************************************************** // --- umac.c::umac_set_controller_zero --- // //!\brief Sets zero offset at controller and request reboot if position value changes for more than 1 micron //!\param DWORD [in] //!\param int [in] //!\param double [in] //!\retval BOOL //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** BOOL umac_set_controller_zero (DWORD dev_ID, int axis,double position) { int retVal = 0; BOOL needRestart = FALSE; char result[16]; char command[32]; char command1[32]; double cur_position; //in mm position*=1000.; //convert to mm switch(axis) { case umac_z: sprintf(command,"q9=%.3f",position); sprintf(command1,"q9"); break; case umac_x: sprintf(command,"q10=%.3f",position); sprintf(command1,"q10"); break; case umac_y: sprintf(command,"q11=%.3f",position); sprintf(command1,"q11"); break; default: sprintf(message, "umac_set_controller_zero:No such axis!\n"); MessagePopup ("UMAC Error", message); break; } retVal = (*PmacGetResponseAPtr)(dev_ID,result,15,command1);//returns number of read chars if(retVal <0) { sprintf(message, "umac_set_controller_zero failed\n"); MessagePopup ("UMAC Error", message); } cur_position = atof(result); //mm /g_umac_param[axis-1].cs_counts_per_mm; if( fabs(cur_position - position) > 0.001) { needRestart = TRUE; retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_set_controller_zero failed\n"); MessagePopup ("UMAC Error", message); } //kill motor sprintf(command,"#%ik",axis); retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_set_controller_zero failed\n\n"); MessagePopup ("UMAC Error", message); } } return needRestart; } // //**************************************************************************** // --- umac.c::add_log --- // //!\brief Adds string to log file //!\param char[] //!\retval none //!\author Seva //**************************************************************************** void umac_add_log(char buf[], BOOL newline) { char header[] = {"UMAC: "}; FILE* file_ptr; file_ptr = fopen(log_file, "a"); if (file_ptr == NULL) { sprintf(message, "Unable to open log file\n"); MessagePopup ("UMAC Error", message); return; } // Write the values to the log file if(newline) fprintf(file_ptr, "\n%s: %s",header, buf); else fprintf(file_ptr, "\t %s", buf); fclose(file_ptr); } // //**************************************************************************** // --- umac.c::umac_set_soft_positive_limit --- // //!\brief Sets software positive limit switch in counts, set 0 to disable it //!\param DWORD [in] //!\param int [in] //!\param double [in] - mm //!\retval void //!\author Seva Kaplounenko, Stanford Linear Accelerator Center //**************************************************************************** void umac_set_soft_positive_limit( DWORD id, int motor, double limit) { char result[32]; int retVal = -1; char command[64]; int nCounts = 0; switch(motor) { case umac_z: nCounts = g_umac_param[0].cs_counts_per_mm * limit; sprintf(command,"i0113=%I",nCounts); break; case umac_x: nCounts = g_umac_param[1].cs_counts_per_mm * limit; sprintf(command,"i0213=%i",nCounts); break; case umac_y: nCounts = g_umac_param[2].cs_counts_per_mm * limit; sprintf(command,"i0313=%I",nCounts); break; default: printf("set_distance:No such axis!\n"); break; } retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars if(retVal <0) { sprintf(message, "umac_set_soft limit swich failed\n"); MessagePopup ("UMAC Error", message); } }