#ifdef LAPTOP #include "c:\slac\Repository\Lib\delay.h" #else #include "v:\\met\\repository\\lib\\delay.h" #include #endif #include #include #include #include #include #include "Cam_movers.h" #include "Hp34970.h" #include "cam_movers_dec.h" #include "cam_movers_param.h" #include "cm6k6_g.h" #include "ev16p.h" /* Public Variables Declaration*/ static int hp34970_ID; // HP Data Acquisition/Switch module static int cm6k6_ID; // Compumotor Controller static int ev16p_ID; // Linear gage /* Extern Variables */ static int panelHandle; // Panel ID for GUI /* PRIVATE GLOBAL VARIABLES */ /* * comd, buffer for GPIB I/O strings * mesg, buffer for messages to Standard I/O */ static char comd[HP34970_MAX_NUM_CHAR]; static char mesg[HP34970_MAX_NUM_CHAR]; static double device_coord[5]; // x,y,roll,pitch, and yaw /* Log file declaration */ static char log_file[256]; /*************************************************************** * Public Functions * *************************************************************** */ /* cam_move_device_init * * This function initializes devices * * Output: * int status * * Yurii Levashov * 10/11/2004 */ int cam_move_device_init(char log_file_in[], int panel_ID, int status_control, struct com_param param, struct move_param mv_param) { int dev_ID = 0; int err; int i; if (InitCVIRTE(0, 0, 0) == 0) // Needed if linking in external compiler; harmless otherwise return -1; // out of memory panelHandle = panel_ID; // Save log file name strcpy(log_file, log_file_in); // Initialize voltmeter if(hp34970_ID <= 0) { SetCtrlAttribute (panel_ID, status_control, ATTR_CTRL_VAL, "Initialization HP34970..."); DisplayPanel (panel_ID); hp34970_init(param.gpib_addr, param.hp34970_addr, &dev_ID); hp34970_ID = dev_ID; if(dev_ID <= 0) { write_to_log(log_file, "PROBLEM: HP34970 is not initialized"); return dev_ID; } // Reset slots to the power ON state hp34970_out(hp34970_ID, "SYST:CPON ALL"); } // Initialize motor controller if(cm6k6_ID <= 0) { SetCtrlAttribute (panel_ID, status_control, ATTR_CTRL_VAL, "Initialization CM6K6..."); DisplayPanel (panel_ID); cm6k6_init(panelHandle, param.cm6k6_com_port, param.cm6k6_addr, &dev_ID); cm6k6_ID = dev_ID; if(dev_ID <= 0) { write_to_log(log_file, "PROBLEM: CM6K6 is not initialized"); return dev_ID; } SetCtrlAttribute (panel_ID, status_control, ATTR_CTRL_VAL, "Initialization CM6K6 OK..."); DisplayPanel (panel_ID); } // Initialize EV16P counter if(ev16p_ID <= 0) { if(GAGE_NUMBER != 0) { ev16p_init(param.ev16p_com_port, param.ev16p_addr, &dev_ID); ev16p_ID = dev_ID; if(dev_ID <=0) { write_to_log(log_file, "PROBLEM: EV16P is not initialized"); return dev_ID; } SetCtrlAttribute (panel_ID, status_control, ATTR_CTRL_VAL, "Initialization EV16P OK..."); } } DisplayPanel (panel_ID); // write to log file write_to_log(log_file, "METER: HP34970 \t MOTOR DRIVER: CM6K6 \t GAGE: EV16P \t Initialized OK"); // Pause Delay(2.0); return 0; } /* ************************************************************** */ /* fill_param_structure * * This function fills com_param and move_param structures using data from param.h files. * * Yurii Levashov * 07/01/2005 */ void fill_param_structure(struct move_param *m_param, struct com_param *c_param, int test) { // Fill com_param structure c_param->gpib_addr = GPIB_ADDR; c_param->cm6k6_addr = CM6K6_DEV_ADDR; c_param->ev16p_addr = EV16P_ADDR; c_param->hp34970_addr = HP34970_ADDR; c_param->cm6k6_com_port = CM6K6_COM_PORT; c_param->ev16p_com_port = EV16P_COM_PORT; // Fill move_param structure m_param->axis = AXIS; m_param->num_steps = NUM_STEPS; m_param->step_size = STEP_SIZE; m_param->vel = MOT_VEL; //VELOCITY; m_param->accel = MOT_ACC; m_param->time_int = TIME_INT; m_param->distance = 0; // Zero settings for potentiometers m_param->pot_range[0] = POT_RANGE_1; m_param->pot_range[1] = POT_RANGE_2; m_param->pot_range[2] = POT_RANGE_3; m_param->pot_range[3] = POT_RANGE_4; m_param->pot_range[4] = POT_RANGE_5; m_param->pot_zero[0] = ANGLE_OFFSET_1; m_param->pot_zero[1] = ANGLE_OFFSET_2; m_param->pot_zero[2] = ANGLE_OFFSET_3; m_param->pot_zero[3] = ANGLE_OFFSET_4; m_param->pot_zero[4] = ANGLE_OFFSET_5; // Initial angle of cams m_param->init_angle[0] = CAM_1_ANGLE; m_param->init_angle[1] = CAM_2_ANGLE; m_param->init_angle[2] = CAM_3_ANGLE; m_param->init_angle[3] = CAM_4_ANGLE; m_param->init_angle[4] = CAM_5_ANGLE; // Direction of motor rotation to make all uniform m_param->motor_dir[0] = MOTOR_1_DIRECTION; m_param->motor_dir[1] = MOTOR_2_DIRECTION; m_param->motor_dir[2] = MOTOR_3_DIRECTION; m_param->motor_dir[3] = MOTOR_4_DIRECTION; m_param->motor_dir[4] = MOTOR_5_DIRECTION; // Direction of angle readings from pots to make them positive when cams move positive m_param->angle_dir[0] = ANGLE_DIR_1; m_param->angle_dir[1] = ANGLE_DIR_2; m_param->angle_dir[2] = ANGLE_DIR_3; m_param->angle_dir[3] = ANGLE_DIR_4; m_param->angle_dir[4] = ANGLE_DIR_5; // Individual cam offsets m_param->cam_offset[0] = CAM_OFFSET_1; m_param->cam_offset[1] = CAM_OFFSET_2; m_param->cam_offset[2] = CAM_OFFSET_3; m_param->cam_offset[3] = CAM_OFFSET_4; m_param->cam_offset[4] = CAM_OFFSET_5; // Gages m_param->gage_number = GAGE; if(test == 0) { m_param->num_gages = GAGE_NUMBER; m_param->num_cams = CAM_NUM; } else { m_param->num_gages = TEST_GAGE_NUMBER; m_param->num_cams = TEST_CAM_NUM; } m_param->gage[0] = GAGE_Y_FRONT; m_param->gage[1] = GAGE_Y_REAR; m_param->gage[2] = GAGE_X_FRONT; m_param->gage[3] = GAGE_X_REAR; // initial coordinates m_param->cam_coord[0] = START_X; m_param->cam_coord[1] = START_Y; m_param->cam_coord[2] = START_ROLL; m_param->cam_coord[3] = START_PITCH; m_param->cam_coord[4] = START_YAW; return; } /* cam_move_readings_zero * * This function zeroes coordinates and angles currently highlighted * on the user interface * * Yurii Levashov * 09/10/2004 */ void cam_move_gage_zero(struct com_param cm_param, int index) { int err; // Check, if ev16p is not initialized, initialize it if (ev16p_ID <= 0) ev16p_init(cm_param.ev16p_com_port, cm_param.ev16p_addr, &ev16p_ID); // Set gage to zero ev16p_zero(ev16p_ID, index); // write to log Fmt(mesg,"%s * All gages set to zero\n", TimeStr()); write_to_log(log_file, mesg); return; } /************************************************************** */ /* cam_move_get_coordinates * * This function reads coordinates and angles from currently highlighted * positions on the user interface * coord[0] - X center * coord[1] - Y center * coord[2] - Roll * coord[3] - Pitch * coord[4] - Yaw * * Yurii Levashov * 09/10/2004 */ int cam_move_get_coordinates(int panel_ID, struct move_param *mv_param, struct com_param cm_param, int GUI_control_order[]) { float max_coord = 0.001; // 1 mm float max_angle = PI4 * 2; // Pi/2 in radians int i; int err; double dumm; double f_angle[5]; // 5 is maximum number of gages // save axis number err = mv_param->axis; // Read cam angles from Pot's for(i = 0; i < mv_param->num_cams; i++) mv_param->cam_angle[i] = (cam_move_read_cam_angles(mv_param, cm_param, i + 1) + mv_param->pot_zero[i]) * mv_param->angle_dir[i]; // Restore axis number mv_param->axis = err; // Save angles for(i = 0; i < mv_param->num_cams; i++) f_angle[i] = mv_param->cam_angle[i];// - mv_param->pot_zero[i]; // calculate current device coordinates cam_move_angles_to_coord(mv_param, f_angle); // save coordinates cam_move_save_coord(*mv_param); // Read gages if(mv_param->num_gages != 0) { for(i = 0; i < mv_param->num_gages; i++) mv_param->gage_read[i] = cam_move_read_gage(cm_param, mv_param->gage[i]) / 1000.; //convert to meters } for(i = 0; i < 2; i++) // read X and Y { err = cam_move_read_from_GUI(panel_ID, GUI_control_order[i], &dumm, max_coord); mv_param->cam_coord[i] = dumm; if(err != 0) return err; } // Correct for X - direction // mv_param->cam_coord[0] = - mv_param->cam_coord[0]; for(i = 2; i < mv_param->num_cams; i++) // Read Roll, Pitch, and Yaw { err = cam_move_read_from_GUI(panel_ID, GUI_control_order[i], &dumm, max_angle); // convert to radians if(err != 0) return err; mv_param->cam_coord[i] = dumm; // / RAD_TO_ANGLE; } return 0; } /* ************************************************************** */ /* cam_move_set_coordinates * * This function outputs coordinates and angles into currently highlighted * positions on the user interface * * Yurii Levashov * 09/10/2004 */ void cam_move_set_coordinates(int panel_ID, int GUI_control_order[], struct move_param *mv_param) { int i; int index; index = mv_param->num_cams * 2; // for( i = 0; i < 3; i++) device_coord[i + 2] *= RAD_TO_ANGLE; for(i = 0; i < mv_param->num_cams; i++) { cam_move_GUI_out(panel_ID, GUI_control_order[i + mv_param->num_cams], device_coord[i]); cam_move_GUI_out(panel_ID, GUI_control_order[i + index], (mv_param->cam_angle[i] - mv_param->pot_zero[i]) * RAD_TO_ANGLE); //* mv_param->angle_dir[i] } if(mv_param->num_gages != 0) { for(i = 0; i < mv_param->num_gages; i++) cam_move_GUI_out(panel_ID, GUI_control_order[i + index + mv_param->num_gages + 1], mv_param->gage_read[i]); } return; } /* ************************************************************** */ /* cam_move_zero_coord * * This function outputs coordinates and angles into currently highlighted * positions on the user interface * * Yurii Levashov * 09/10/2004 */ void cam_move_zero_coord(struct move_param *param) { int i; for(i = 0; i < 5; i++) { param->cam_coord[i] = 0.0; } // write to log Fmt(mesg,"%s * All coordinates set to zero\n", TimeStr()); write_to_log(log_file, mesg); return; } /* ************************************************************** */ /* cam_move_save_coord * * This function outputs coordinates and angles into currently highlighted * positions on the user interface * * Yurii Levashov * 09/10/2004 */ void cam_move_save_coord(struct move_param param) { int i; for(i = 0; i < 5; i++) { device_coord[i] = param.cam_coord[i]; } return; } /* ************************************************************** */ /* cam_move_read_motion_data * * This function reads coordinates and angles from data file and outputs them on the user interface * * Yurii Levashov * 12/14/2005 */ /*void cam_move_read_motion_data(int panel_ID, struct move_param *mv_param, int GUI_control_order[]) { static char data_file[280]; int err; FILE *file_ptr = NULL; int i; char buffer[300]; // ask for name of file with data if(FileSelectPopup (DATA_DIR, "*.*", "", "Choose file with data", VAL_SELECT_BUTTON, 0, 0, 1, 0, data_file) == 0) //No file selected return; // Open data file with choosen name file_ptr = fopen (data_file, "r"); if (file_ptr == NULL) { cam_move_error_handling(1,"Unable to open data file"); fclose(file_ptr); return; } // read data i = 0; while(fscanf(file_ptr, "%s", buffer) > 0) { Fmt (&mv_param->cam_coord[i], "%f<%s", buffer); i++; } // close file fclose(file_ptr); // write to log Fmt(mesg,"Motion data read from file %s", data_file); write_to_log(log_file, mesg); // output on GUI for(i = 0; i < 5; i++) cam_move_GUI_out(panel_ID, GUI_control_order[i], mv_param->cam_coord[i]); return; } */ void cam_move_read_motion_data(int panel_ID, struct move_param *mv_param, int GUI_control_order[]) { static char data_file[280]; char file[280]; int err; FILE *file_ptr = NULL; int i; char buffer[300]; // ask for name of file with data // if(FileSelectPopup (DATA_DIR, "*.*", "", "Choose file with data", // VAL_SELECT_BUTTON, 0, 0, 1, 0, data_file) == 0) //No file selected // return; if(MakePathname (DATA_DIR, DATA_FILE, data_file) != 0) cam_move_error_handling(1,"Unable to create data file"); // Open data file with choosen name file_ptr = fopen (data_file, "r"); if (file_ptr == NULL) { cam_move_error_handling(1,"Unable to open data file"); fclose(file_ptr); return; } // read data i = 0; while(fscanf(file_ptr, "%s", buffer) > 0) { Fmt (&mv_param->cam_coord[i], "%f<%s", buffer); i++; } // close file fclose(file_ptr); // Convert millimeters to meters, change sign for(i = 0; i < 2; i++) mv_param->cam_coord[i] /= -1000.; // Convert milliradians to degrees, change sign for(i = 2; i < 5; i++) mv_param->cam_coord[i] /= -1000.; //*= -180 / PI / 1000.; // write to log Fmt(mesg,"Motion data read from file %s", data_file); write_to_log(log_file, mesg); // output on GUI for(i = 0; i < 5; i++) cam_move_GUI_out(panel_ID, GUI_control_order[i], mv_param->cam_coord[i]); return; } /* ************************************************************** */ /* cam_move_log_motion_data * * This function writes coordinates and angles to log file * * Yurii Levashov * 12/14/2005 */ void cam_move_log_motion_data(char txt[], struct move_param mv_param) { // make a string Fmt(mesg,"%s * %s * X = %f Y = %f Roll = %f Pitch = %f Yaw = %f\n", TimeStr(), txt, mv_param.cam_coord[0], mv_param.cam_coord[1] ,mv_param.cam_coord[2],mv_param.cam_coord[3],mv_param.cam_coord[4]); write_to_log(log_file, mesg); return; } /* ************************************************************** */ /* cam_move_go * This function reads coordinates and angles currently highlighted * on the user interface, calculates angles and updates coordinates, * commands motors to move, read final coordinates and compare to target ones. * * Yurii Levashov * 10/12/2004 */ void cam_move_go(int panel_ID, struct com_param cm_param, struct move_param *mv_param, int GUI_control_order[], char f_name[], int flag, int mode) { double f_angle[5]; // angle[] - target; f_angle[] - final int i, err; double coord_target[5], coord_init[5]; double distance[5]; double delta_x = 0.0, delta_y = 0.0, delta_pitch = 0, delta_yaw = 0.0; int j = 0, counter = 0; FILE *file_ptr = NULL; static int file_open = 0; double v_coord[4]; double gage[4] = {0}; // Open data file if(mv_param->log_data == TRUE && file_open == 0) { file_ptr = fopen(f_name, "a"); if (file_ptr == NULL) { fclose (file_ptr); cam_move_error_handling(1,"Unable to open log file"); return; } file_open = 1; } // Read measured coordinates and initial cam angles err = cam_move_get_coordinates(panel_ID, mv_param, cm_param, GUI_control_order); if(err != 0) { if(file_open == 1) { file_open = 0; fclose (file_ptr); } return; } // save coordinates for(i = 0; i < mv_param->num_cams; i++) coord_target[i] = mv_param->cam_coord[i]; // Save angles for(i = 0; i < mv_param->num_cams; i++) f_angle[i] = mv_param->cam_angle[i]; // calculate current coordinates cam_move_angles_to_coord(mv_param, f_angle); // save initial coordinates for( i = 0; i < mv_param->num_cams; i++) { // save coordinates coord_init[i] = mv_param->cam_coord[i]; // restore target coordinates mv_param->cam_coord[i] = coord_target[i]; } // log data cam_move_log_motion_data("Initial coordinates", *mv_param); if(mode == 0) // motion relative to current position { // calculate new coordinates (target) of the center for( i = 0; i < mv_param->num_cams; i++) mv_param->cam_coord[i] += coord_init[i];// - mv_param->cam_coord[i]; // ? } // Calculate target coordinates of vertex points cam_move_coord_to_angles(mv_param, v_coord);//f_angle); // Calculate new cam angles cam_move_cam_angles(mv_param, f_angle, v_coord); // main loop do { // Check, if angles < 90 degrees for (i = 0; i < mv_param->num_cams; i++) if( fabs(f_angle[i]) > PI4 * 2) err = danger_operation_message("Angle > 90. Continue?"); // Check, if cm6k6 controller is not initialized, initialize it if (cm6k6_ID <= 0) cm6k6_init(panel_ID, cm_param.cm6k6_com_port, cm_param.cm6k6_addr, &cm6k6_ID); for (i = 0; i < mv_param->num_cams; i++) { // calculate number of motor revolutions distance[i] = (f_angle[i] - mv_param->cam_angle[i]) * GEAR_RATIO / PI / 2.; if(fabs(distance[i]) > GEAR_RATIO / 2.) err = danger_operation_message(" Too many motor revolutions. Continue?"); if(err != 0) { if(file_open == 1) { file_open = 0; fclose (file_ptr); } return; } } //move motor move_cam_motor(panel_ID, mv_param, distance, 1); // Read final cam angles from Pots for(i = 0; i < 5; i++) mv_param->cam_angle[i] = (cam_move_read_cam_angles(mv_param, cm_param, i + 1) + mv_param->pot_zero[i]) * mv_param->angle_dir[i] ; // calculate final coordinates cam_move_angles_to_coord(mv_param, mv_param->cam_angle); // log data cam_move_log_motion_data("Final coordinates", *mv_param); if(flag != 0) { // Read gages for(i = 0; i < mv_param->num_gages; i++) { // save initial readings gage[i] = mv_param->gage_read[i]; mv_param->gage_read[i] = cam_move_read_gage(cm_param, i + 1) / 1000.; //convert to meters //calculate measured movements gage[i] = mv_param->gage_read[i] - gage[i]; } // calculate differences in coordinates delta_x = (mv_param->cam_coord[0] - coord_init[0]) - (gage[2] + gage[3]) / 2.0; // avarage of two readings delta_y = (mv_param->cam_coord[1] - coord_init[1]) - (gage[0] + gage[1]) / 2.0; // avarage of two readings delta_pitch = (mv_param->cam_coord[3] - coord_init[3]) - (gage[0] - gage[1]) / L; delta_yaw = (mv_param->cam_coord[4] - coord_init[3]) - (gage[2] - gage[3]) / L; // save differences if(file_open == 1) fprintf(file_ptr, " %f %f %f %f\n", delta_x, delta_y, delta_pitch, delta_yaw); // make iteractions mv_param->cam_coord[0] += delta_x; mv_param->cam_coord[1] += delta_y; mv_param->cam_coord[3] += delta_pitch; mv_param->cam_coord[4] += delta_yaw; // Calculate target coordinates of vertex points and new cam angles cam_move_coord_to_angles(mv_param, v_coord);//f_angle); // Calculate new cam angles cam_move_cam_angles(mv_param, f_angle, v_coord); // update GUI cam_move_set_coordinates(panel_ID, GUI_control_order, mv_param); } if(++counter > 5) break; } while(delta_x > 0.000001 || delta_y > 0.000001 || delta_pitch > 0.00001 || delta_yaw > 0.00001); // Close file if(file_open == 1) { file_open = 0; fclose (file_ptr); } return; } /* ************************************************************** */ /* cam_move_angles_to_coord * This function calculates center point coordinates from cam angles * * Input: * angles[5] - array of pot angles * * * Coordinates: coord 0 - x of magnet center * 1 - y of magnet center * 2 - roll * 3 - pitch * 4 - yaw * Vertex: v_coord 0 - x of front vertex point * 1 - y of front vertex point * 2 - x of rear vertex point * 3 - y of rear vertex point * * Cam Angles: 0 - front cam 1 3 - back cam 4 * 1 - front cam 2 4 - back cam 5 * 2 - front cam 3 * * Yurii Levashov * 01/07/2005 */ void cam_move_angles_to_coord(struct move_param *m_param, double angles[]) { double x_a, y_a, x_b, y_b, phi = 0; int i, err; double v_coord[4]; // Check input parameters for(i = 0; i < m_param->num_cams; i++) if(fabs(angles[i] >= PI4 * 2)) err = danger_operation_message("Angle out of range (>90 Degrees). Continue?"); // Calculate vertex coordinates and roll angle. cam_move_vertex_coord(m_param, angles, v_coord, &phi); // Calculate coordinates of points on the center line x_a = v_coord[0] + A - A * cos(phi) - B * sin(phi); // Front y_a = v_coord[1] + B * cos(phi) - A * sin(phi) - C3; x_b = v_coord[2] - D * sin(phi); // Rear y_b = v_coord[3] + D * cos(phi) - C2; // Calculate angles and coordinates of the center m_param->cam_coord[0] = (x_a + x_b) / 2.; // new x m_param->cam_coord[1] = (y_a + y_b) / 2.; // new y m_param->cam_coord[2] = phi; // Roll m_param->cam_coord[4] = asin((x_b - x_a) / L); // Yaw m_param->cam_coord[3] = asin((y_b - y_a) / L); // Pitch return; } /* ************************************************************** */ /* cam_move_coord_to_angles * * This function calculates cam angles from coordinates of center point * and angles * * Output: * angles - cam angles in radians, * * Yurii Levashov * 10/15/2004 */ void cam_move_coord_to_angles(struct move_param *m_param, double crd[]) { double X_A, Y_A, X_B, Y_B, phi; double distance; double gamma; // Calculate coordinates of points on the central line X_A = m_param->cam_coord[0] - L / 2. * sin(m_param->cam_coord[4]); Y_A = m_param->cam_coord[1] - L / 2. * sin(m_param->cam_coord[3]); X_B = m_param->cam_coord[0] + L / 2. * sin(m_param->cam_coord[4]); Y_B = m_param->cam_coord[1] + L / 2. * sin(m_param->cam_coord[3]); // Calculate new coordinates of vertex points phi = m_param->cam_coord[2]; // save roll distance = sqrt(A * A + B * B); gamma = atan(B / A); crd[0] = X_A + (distance * cos(gamma - phi) - A); crd[1] = Y_A + (C3 - distance * sin(gamma - phi)); crd[2] = X_B + D * sin(phi); crd[3] = Y_B - D * cos(phi) + C2; return; } /* ************************************************************** */ /* cam_move_vertex_coordinates * * This function calculates coordinates of vertex points * * Input: * cam_angles - cam angles in radians, * * Output: * vertex_coord (X positive goes inside the undulator, Y goes up) * * Yurii Levashov * 09/10/2004 */ void cam_move_vertex_coord(struct move_param *m_param, double cam_angle[], double v_coord[], double *alpha) { double alpha0, eps = 1.0; double tm, tp, tsum, xdif; double X_a, X_b, X_c, Y_a, Y_b, Y_c, xb, xc, yb, yc; int err; int front_cams; int j; // Check input parameters for(j = 0; j < m_param->num_cams; j++) if(fabs(cam_angle[j] >= PI4 *2)) err = danger_operation_message("Angle out of range (>90 Degrees). Continue?"); alpha0 = *alpha; // Front Cams // Calculate initial coordinates of cam centers X_a = -S2 + m_param->cam_offset[2] * cos (cam_angle[2]); // Cam 3 X_b = -S1 - m_param->cam_offset[1] * cos (THREE_CAM_ANGLE + cam_angle[1]); // Cam 2 X_c = S1 + m_param->cam_offset[0] * cos (THREE_CAM_ANGLE + cam_angle[0]); // Cam 1 Y_a = m_param->cam_offset[2] * sin (cam_angle[2]); Y_b = m_param->cam_offset[1] * sin (THREE_CAM_ANGLE + cam_angle[1]); Y_c = m_param->cam_offset[0] * sin (THREE_CAM_ANGLE + cam_angle[0]); while (eps > 0.0000001) // Loop for improving the accuracy of calculations { // calculate tangents of angles tm = tan(THREE_CAM_ANGLE - alpha0); tp = tan(THREE_CAM_ANGLE + alpha0); tsum = tm + tp; if (tsum == 0) { err = danger_operation_message(" Division by 0; tangent sum. Continue?"); return; } // calculate coordinates of tangent points xb = X_b + CAM_RADIUS * cos(THREE_CAM_ANGLE + alpha0); yb = Y_b + CAM_RADIUS * sin(THREE_CAM_ANGLE + alpha0); xc = X_c - CAM_RADIUS * cos(THREE_CAM_ANGLE - alpha0); yc = Y_c + CAM_RADIUS * sin(THREE_CAM_ANGLE - alpha0); // calculate coordinates of vertex v_coord[0] = (yb - yc + tm * xb + tp * xc) / tsum; // X - coordinate v_coord[1] = yb + (xb - v_coord[0]) * tm; // Y - Coordinate xdif = v_coord[0] - X_a; if (xdif == 0) { err = danger_operation_message(" Division by 0; Coordinate difference. Continue?"); return; } // calculate roll angle alpha0 = (v_coord[1] - C3 + B - Y_a) / xdif; // Roll angle eps = fabs(*alpha - alpha0); *alpha = alpha0; } // Rear Cams // Calculate initial coordinates of cam centers X_b = -S3 - m_param->cam_offset[4] * cos (TWO_CAM_ANGLE + cam_angle[4]); // Cam 4 X_c = S3 + m_param->cam_offset[3] * cos (TWO_CAM_ANGLE + cam_angle[3]); // Cam 5 Y_b = m_param->cam_offset[4] * sin (TWO_CAM_ANGLE + cam_angle[4]); Y_c = m_param->cam_offset[3] * sin (TWO_CAM_ANGLE + cam_angle[3]); // calculate tangents of angles tm = tan(TWO_CAM_ANGLE - alpha0); tp = tan(TWO_CAM_ANGLE + alpha0); tsum = tm + tp; if (tsum == 0) { err = danger_operation_message(" Division by 0; tangent sum. Continue?"); return; } // calculate coordinates of tangent points xb = X_b + CAM_RADIUS * cos(PI4 * 2 - TWO_CAM_ANGLE + alpha0); yb = Y_b + CAM_RADIUS * sin(PI4 * 2 - TWO_CAM_ANGLE + alpha0); xc = X_c - CAM_RADIUS * cos(PI4 * 2 - TWO_CAM_ANGLE - alpha0); yc = Y_c + CAM_RADIUS * sin(PI4 * 2 - TWO_CAM_ANGLE - alpha0); // calculate coordinates of vertex v_coord[2] = (yb - yc + tm * xb + tp * xc) / tsum; // X - coordinate v_coord[3] = yb + (xb - v_coord[2]) * tm; // Y - Coordinate return; } /* ************************************************************** */ /* cam_move_cam_angles * * This function calculates cam angles from vertex coordinates and roll * * Input: * crd[4] - coordinates of vertex points * crd[0] - X front (3 cams) * crd[1] - Y front * crd[2] - X rear (2 cams) * crd[3] - Y rear * * Output: * cam angles[5] * * Yurii Levashov * 09/10/2004 */ void cam_move_cam_angles(struct move_param *m_param, double cam_angle[], double crd[]) { double dumm, pi2; int err; double roll; pi2 = PI4 * 2; roll = m_param->cam_coord[2]; // roll angle // Three cams dumm = ((crd[0] + S2) * sin(roll) - crd[1] * cos(roll) + C3 - B) / m_param->cam_offset[2]; // check, if asin argument < 1 if (fabs(dumm) > 1) { err = danger_operation_message("Unreachable position. Continue?"); if(err != 0) return; } cam_angle[2] = roll - asin(dumm); // Teta 3 if( fabs(cam_angle[2]) > pi2) err = danger_operation_message("Angle Teta1 > 90.Continue?"); dumm = ((crd[0] + S1) * sin(THREE_CAM_ANGLE - roll) + crd[1] * cos(THREE_CAM_ANGLE - roll) - CAM_RADIUS) / m_param->cam_offset[1]; // check, if asin argument < 1 if (fabs(dumm) > 1) { err = danger_operation_message("Unreachable position. Continue?"); if(err != 0) return; } cam_angle[1] = asin(dumm) - roll; // Teta 2 if( fabs(cam_angle[1]) > pi2) err = danger_operation_message("Angle Teta2 > 90. Continue?"); dumm = ((crd[0] - S1) * sin(THREE_CAM_ANGLE + roll) - crd[1] * cos(THREE_CAM_ANGLE + roll) + CAM_RADIUS) / m_param->cam_offset[0]; // check, if asin argument < 1 if (fabs(dumm) > 1) { err = danger_operation_message("Unreachable position. Continue?"); if (err != 0) return; } cam_angle[0] = roll - asin(dumm); // Teta 1 if( fabs(cam_angle[2]) > pi2) err = danger_operation_message("Angle Teta3 > 90. Continue?"); // Two Cams dumm = ((crd[2] - S3) * sin(TWO_CAM_ANGLE + roll) - crd[3] * cos(TWO_CAM_ANGLE + roll) + CAM_RADIUS) / m_param->cam_offset[3]; // check, if asin argument < 1 if (fabs(dumm) > 1) { err = danger_operation_message("Unreachable position. Continue?"); if(err != 0) return; } cam_angle[3] = roll - asin(dumm); // Teta 4 if( fabs(cam_angle[3]) > pi2) err = danger_operation_message("Angle Teta4 > 90. Continue?"); dumm = ((crd[2] + S3) * sin(TWO_CAM_ANGLE - roll) + crd[3] * cos(TWO_CAM_ANGLE - roll) - CAM_RADIUS) / m_param->cam_offset[4]; // check, if asin argument < 1 if (fabs(dumm) > 1) { err = danger_operation_message("Unreachable position. Continue?"); if (err != 0) return; } cam_angle[4] = asin(dumm) - roll ; // Teta 5 if( fabs(cam_angle[4]) > pi2) err = danger_operation_message("Angle Teta5 > 90. Continue?"); return; } /* ************************************************************** */ /* cam_move_read_cam_angles * * This function reads pot voltages and calculates angles in radians * * Output: * angles * * Yurii Levashov * 09/10/2004 */ double cam_move_read_cam_angles(struct move_param *m_param, struct com_param cm_param, int pot_number) { double max_voltage; double angle; int err; // Check, if 34970A voltmeter is not initialized, initialize it if (hp34970_ID <= 0) hp34970_init(cm_param.gpib_addr, cm_param.hp34970_addr, &hp34970_ID); /// apply voltage to the Pot hp34970_switch_on_off(hp34970_ID, POT_SWITCH_POSITION, 1); // wait for 1 sec Delay(1.0); // Read voltages from Pot and convert to angle hp34970_get_chan_volt(hp34970_ID, pot_number, &angle); hp34970_get_chan_volt(hp34970_ID, 10, &max_voltage); if(max_voltage < MAX_POT_VOLTAGE) cam_move_error_handling(-1, " Problem reading pot voltage. \n Check power supply."); angle = (angle / max_voltage - 0.5) * m_param->pot_range[pot_number - 1]; // disconnect the voltage from the Pot hp34970_switch_on_off(hp34970_ID, POT_SWITCH_POSITION, 0); if (fabs(angle) >= PI4*2) // Check, if angle < 90 degrees { Fmt(mesg,"%s 0) hp34970_exit(hp34970_ID); // Check, if cm6k6 controller is initialized, close it if (cm6k6_ID > 0) cm6k6_exit(cm6k6_ID); // Check, if EV-16P is initialized, close it if (ev16p_ID > 0) ev16p_exit(ev16p_ID); return; } /* ************************************************************** */ /* cam_move_check_input * * This function checks if coordinates and angles are in an accepted range * Input: Control_ID, value, Control_value * * Yurii Levashov * 10/18/2004 */ int cam_move_check_input(int panel_ID, int control, double value, double control_value) { if(fabs(value) > control_value) { SetActivePanel (panel_ID); MessagePopup ("Warning", "Value out of Range\nInput a new one."); SetActiveCtrl (panel_ID, control); return 1; } else return 0; } /************************************************************** */ /* cam_move_read_from_GUI * * This function reads from currently highlighted * positions on the user interface * * Yurii Levashov * 09/10/2004 */ int cam_move_read_from_GUI(int ID, int panel_addr, double *value, float contr_val) { int err = 1; char msg[80]; err = GetCtrlVal (ID, panel_addr, value); if(err < 0) { Fmt(msg, "%snum_cams; // move motor if(n_cams == 5) { // Correct for direction of motor rotation for(i = 0; i < n_cams; i++) { distance[i] *= param->motor_dir[i]; //turn ON LED, if any if(led_flag != 0) SetCtrlAttribute (panel_ID, LED_name[i], ATTR_CTRL_VAL, 1); DisplayPanel (panelHandle); // Release motor brakes hp34970_switch_on_off(hp34970_ID, i + 1, 1); } err = cm6k6_cam_move_5(panel_ID, LED_name, cm6k6_ID, distance); // Apply motor brakes for(i = 0; i < n_cams; i++) { hp34970_switch_on_off(hp34970_ID, i + 1, 0); if(led_flag != 0) SetCtrlAttribute (panelHandle, LED_name[i], ATTR_CTRL_VAL, 0); } } else { // distance[0] *= param->motor_dir[param->axis - 1]; hp34970_switch_on_off(hp34970_ID, param->axis, 1); cm6k6_cam_move(panel_ID, LED_name[1], cm6k6_ID, param->axis, distance[0] * param->motor_dir[param->axis - 1], 0); //distance[param->axis - 1], 0); hp34970_switch_on_off(hp34970_ID, param->axis, 0); } DisplayPanel (panelHandle); return; } /* ************************************************************** */ /* cam_move_calculate_angles * * This function calculates cam angles based on cam geometry and coordinates * * Input: Coordinates: coord[0] xf - x of front cam cradle support * coord[1] yf - y of front cam cradle support * coord[2] xr - x of rear cam cradle support * coord[3] yr - y of rear cam cradle support * Angles: coord[4] alpha - roll * * Output: Cam Angles: angle[5] * * Yurii Levashov * 07/06/2005 */ void cam_move_calculate_angles(double coord[], double angle[]) { double a2, a3, b[3], x[3]; // coefficients double a_prim[3], b_prim[3], c[3]; int i, sign; double ca, sa; // Calculate all coefficients nesessary ca = cos(coord[4]); sa = sin(coord[4]); a2 = tan(-PI4); a3 = tan(PI4); b[0] = CAM_RADIUS - C3; b[1] = sqrt(2.) * CAM_RADIUS - C3 - S1 + A; b[2] = b[1] - 2. * A; x[0] = A - S2; x[1] = A - S1; x[2] = A + S1; a_prim[0] = tan(coord[4]); a_prim[1] = tan(coord[4] - PI4); a_prim[2] = tan(coord[4] + PI4); // calculate other parameters and cam angles for(i = 0; i < 3; i++) { b_prim[i] = coord[1] + b[i] * ca + (b[i] * sa - coord[0]) * a_prim[i]; c[i] = (a_prim[i] * x[i] + C3 + b_prim[i] - CAM_RADIUS / cos(atan(a_prim[i]))) / L; angle[i] = asin(c[i]/sqrt(a_prim[i] * a_prim[i])) - atan(-a_prim[i]); } return; } /* ************************************************************** */ void cam_go_zero(int panel_ID, struct move_param *m_param, struct com_param c_param) { double angle, pos; int err, i; double distance[CAM_NUM]; int cam_number; // if(m_param.axis < 3) gage = GAGE_Y_FRONT; // else gage = GAGE_Y_REAR; cam_number = m_param->num_cams; //number of cams; for(i = 0; i < cam_number; i++) { // measure current cam angle from pot angle = (cam_move_read_cam_angles(m_param, c_param, cam_number) - m_param->pot_zero[cam_number - 1]) * m_param->angle_dir[cam_number - 1] ; // // Calculate number of motor revolutions distance[i] = - angle * GEAR_RATIO / PI / 2.; } // move motors move_cam_motor(panel_ID, m_param, distance, LED_ON); // write to log Fmt(mesg,"%s * Cams moved to zero position\n", TimeStr()); write_to_log(log_file, mesg); return; } /* ************************************************************** * danger_operation_message * * This function notifies the operator of the dangerous action * * Output: * message * * Yurii Levashov * 09/02/2004 */ int danger_operation_message(char* message) { int err; char msg[80]; Beep(); Delay(.5); Beep(); err = GenericMessagePopup ("Danger Operation", message, "Yes", "No", "Reset", 0, 0, 0, VAL_GENERIC_POPUP_BTN1, VAL_GENERIC_POPUP_BTN1, VAL_GENERIC_POPUP_BTN3); switch (err) { case 2: cam_move_exit(); QuitUserInterface (0); break; case 1: return 0; break; default:; } return 1; } /* ************************************************************** * * file_open * This function opens a file and writes comments into it. First number is * the length of the comments in bytes. * * Output: * f_name - file name * * Yurii Levashov 11/24/2004 */ void file_open(char f_name[], int *f_handle, int mode) { size_t h_length; char header_text[1001], header[1005]; int err; // Prompt to input a file name err = FileSelectPopup("", "*.dat", "*.txt", "Input file name", VAL_OK_BUTTON, 0, 0, 1, 1, f_name); if(err < 0) { cam_move_error_handling(1,"Problem reading file name"); return; } // open file *f_handle = OpenFile (f_name, mode, VAL_TRUNCATE, VAL_ASCII); if(*f_handle < 0) { cam_move_error_handling(1,"Problem open the file"); CloseFile (*f_handle); return; } // prompt to input a header err = PromptPopup ("Input file header", "Type comments to the data", header_text, 1000); if(err < 0) { cam_move_error_handling(1,"Problem reading file header"); CloseFile (*f_handle); return; } // write header to the file Fmt(header,"%s<%s %s", DateStr (), header_text); h_length = NumFmtdBytes (); err = WriteFile (*f_handle, header, h_length); if(err < 0) { cam_move_error_handling(1,"Problem writing file header"); CloseFile (*f_handle); return; } CloseFile (*f_handle); return; } /* ************************************************************** * cam_move_write_to_log * * This function writes a string into log file * * * Yurii Levashov * 03/01/2005 */ void write_to_log(char f_name[], char message[]) { FILE *file_ptr = NULL; if(strcmp(f_name, "") != 0) { // Open log file file_ptr = fopen(f_name, "a"); if (file_ptr == NULL) { cam_move_error_handling(1,"Unable to open log file"); fclose(file_ptr); return; } // write to log file fprintf(file_ptr, " \n %s \n ", message); // close file fclose(file_ptr); } return; } //////////////////////////////////////////////////////////////////////////////////////////////////////////// // CAM TEST FUNCTIONS /////////////////////////////////////////////////////////////////////////////////////////////////////////// /* cam_test * * This function is used for testing individual cam assembly. Commands motor to move * a specified number of steps. If time interval is set, motor will move infinetelly, * until stop button pressed. * * Input: Moving parameters structure param. * Output: Cam angle and distance * * Yurii Levashov * 02/22/2005 */ int cam_test(struct com_param c_param, struct move_param m_param, char f_name[]) { int err, i; int event_control = 0; int ID; FILE *file_ptr = NULL; int file_open = 0; int count = 0; int start, end; double dist, pos, distance, angle; int hours, minutes, seconds; static int panel_ID; panel_ID = TEST_PANEL; // check input parameters if (m_param.axis < 1 || m_param.axis > 6) { err = ConfirmPopup ("ERROR", "axis is out of range, will be set to 1. Continue?"); if (err == 0) return -1; // exit the program m_param.axis = 1; } // Check, if 34970A voltmeter is not initialized, initialize it if (hp34970_ID <= 0) hp34970_init(c_param.gpib_addr, c_param.hp34970_addr, &hp34970_ID); // Check, if cm6k6 controller is not initialized, initialize it if (cm6k6_ID <= 0) cm6k6_init(panel_ID, c_param.cm6k6_com_port, c_param.cm6k6_addr, &cm6k6_ID); // Check, if EV-16P is not initialized, initialize it if (GAGE_NUMBER != 0 && ev16p_ID <= 0) ev16p_init(c_param.ev16p_com_port, c_param.ev16p_addr, &ev16p_ID); //Open data file if(strcmp(f_name, "") != 0) { file_ptr = fopen(f_name, "a"); if (file_ptr == NULL) { cam_move_error_handling(1,"Unable to open log file"); fclose(file_ptr); return -1; } file_open = 1; } // make relative move if distance specified if(m_param.distance != 0) { err = GetSystemTime (&hours, &minutes, &seconds); srand((unsigned int)((hours + minutes + 1)/(seconds + 1))); for(i = 0; i < 30; i++) { // save initial position if(GAGE_NUMBER != 0) pos = cam_test_get_position(c_param, m_param.gage[m_param.axis], 0); distance = (double) ((rand() - rand()) % 100)/1000000.; // move cam cam_test_rel_move(c_param, m_param, &distance); // read final position from EV16P if(GAGE_NUMBER != 0) dist = cam_test_get_position(c_param, m_param.gage[m_param.axis], 0); // calculate the difference dist -= pos; if(file_ptr != NULL) fprintf(file_ptr, " \n %10.7f %10.7f", distance, dist); } // output result on GUI SetCtrlVal (panel_ID, TEST_PANEL_GAGE1_OUT, dist); // angle = cam_test_get_angle(m_param, c_param); // cam_test_move_angle(m_param, m_param.distance / RAD_TO_ANGLE); // DisplayPanel(TEST_PANEL); // fclose(file_ptr); return 0; } // If time delay specified, do measurements until command button Q pressed if(m_param.time_int != 0) do { cam_test_out(c_param, m_param, file_ptr, count++); Delay(m_param.time_int); } while(count < m_param.num_steps); else // move motor specified number of steps { // for(count = 0; count < 10; count++) // { angle = m_param.step_size; if(m_param.step_size < 0) { cam_test_out(c_param, m_param, file_ptr, 0); for(i = m_param.num_steps + m_param.step_size; i > 0; i += m_param.step_size) { move_cam_motor(panel_ID, &m_param, &angle, 0); cam_test_out(c_param, m_param, file_ptr, i); } } else { cam_test_out(c_param, m_param, file_ptr, 0); for(i = 0; i < m_param.num_steps; i++) { move_cam_motor(panel_ID, &m_param, &angle, 0); cam_test_out(c_param, m_param, file_ptr, i); } } // param.step_size = - param.step_size; // } } // Close the log file if (file_open != 0) fclose(file_ptr); return 0; } /* cam_test_out * * This function measures and output on GUI the cam angle and distance. * Saves data in a file * * Input: Moving parameters structure param. * Output: Cam angle and distance * * Yurii Levashov * 02/22/2005 */ void cam_test_out(struct com_param c_param, struct move_param m_param, FILE *file_ptr, int step_number) { double position, angle, angle_deg, temp = 0.0; static int count = 4; int gage; if(m_param.axis < 4) gage = 1; else gage = 2; // Read final cam angles from Pot's and convert to angle angle = cam_test_get_angle(&m_param, c_param); if(GAGE_NUMBER != 0) position = cam_test_get_position(c_param, gage, 1); angle_deg = angle * RAD_TO_ANGLE; // Write the measured values to the log file if(file_ptr != NULL) fprintf(file_ptr, " \n%s %6d %10.7f %9.6f %8.3f", TimeStr (), step_number, position, angle, angle_deg); // plot a point on the graph cam_graph_out(panelHandle, position, step_number); // write temperature every 10-th step if(count == 4) { // cam_test_get_temp(&temp, c_param); if(file_ptr != NULL) fprintf(file_ptr, " %4.1f", temp); count = -1; } count++; return; } /* cam_graph_out * * This function output on graph GUI the cam distance. * * Input: position * * Yurii Levashov * 03/14/2005 */ void cam_graph_out(int panel, double position, int step_number) { int flag, err = 0; double y; static int xlimit = GEAR_RATIO; static double ylimit = 1.0; static int panel_ID; panel_ID = panel; // Check x - axis limit if(step_number > xlimit) { xlimit = step_number + 10; err = SetAxisRange(panel_ID, TEST_PANEL_GRAPH, VAL_MANUAL, 0.0, xlimit, VAL_NO_CHANGE, 0.0, 0.0); if(err < 0) cam_move_error_handling(err, " Problem changing X limit"); } // Check y - axis limit y = position * 1000.; // convert to mm if(fabs(y) >= ylimit) { ylimit = fabs(y) + 0.1; err = SetAxisRange(panel_ID, TEST_PANEL_GRAPH, VAL_NO_CHANGE, 0.0, xlimit, VAL_MANUAL, -ylimit, ylimit); if(err < 0) cam_move_error_handling(err, " Problem changing Y limit"); } // Plot the point on the graph err = PlotPoint (panel_ID, TEST_PANEL_GRAPH, step_number, y, VAL_DOTTED_EMPTY_CIRCLE, VAL_RED); if(err <= 0) cam_move_error_handling(err, " Problem plotting the graph"); Fmt(mesg, "%saxis - 1; //m_param->pot_number - 1; // Check, if 34970A voltmeter is not initialized, initialize it if (hp34970_ID <= 0) hp34970_init(c_param.gpib_addr, c_param.hp34970_addr, &hp34970_ID); // apply voltage to the Pot hp34970_switch_on_off(hp34970_ID, POT_SWITCH_POSITION, 1); // wait for 1 sec Delay(1.0); // Read voltages from Pot and convert to angle hp34970_get_chan_volt(hp34970_ID, m_param->axis, &angle); hp34970_get_chan_volt(hp34970_ID, 10, &max_voltage); if(max_voltage < MAX_POT_VOLTAGE) cam_move_error_handling(-1, " Problem reading pot voltage. \n Check power supply."); angle = m_param->angle_dir[index] * (angle / max_voltage - 0.5) * m_param->pot_range[index] - m_param->pot_zero[index]; // disconnect the voltage from the Pot hp34970_switch_on_off(hp34970_ID, POT_SWITCH_POSITION, 0); // Update GUI SetCtrlVal (panelHandle, TEST_PANEL_NUMERIC_CAM_ANGLE, m_param->angle_dir[index] * angle * RAD_TO_ANGLE); DisplayPanel(panelHandle); return angle; } /* cam_test_get_temp * * This function measures and output on GUI the temperature of cam. * * Output: double temperature in degrees C * * Yurii Levashov * 03/01/2005 */ void cam_test_get_temp(double *temp, struct com_param c_param) { // Check, if 34970A voltmeter is not initialized, initialize it if (hp34970_ID <= 0) hp34970_init(c_param.gpib_addr, c_param.hp34970_addr, &hp34970_ID); // Read temperature from channel 11 hp34970_get_chan_temp(hp34970_ID, VOLTMETER_SLOT + 11, temp); // Update GUI SetCtrlVal (panelHandle, TEST_PANEL_NUMERIC_TEMPERATURE, *temp); DisplayPanel(panelHandle); return; } /* cam_test_zero_position * * This function sets gage readings to zero * * * Yurii Levashov * 03/01/2005 */ void cam_test_zero_readings(int panel_ID, int control_ID) { // Check, if ev16p is not initialized, initialize it if (ev16p_ID <= 0) ev16p_init(EV16P_COM_PORT, EV16P_ADDR, &ev16p_ID); ev16p_zero(ev16p_ID, 1); // Gage 1 // ev16p_zero(ev16p_ID, 2); // Gage 2 return; } /* cam_test_rel_move * * This function moves the cam at specified distance * * Input: distance in mm * * Yurii Levashov * 03/01/2005 */ void cam_test_rel_move(struct com_param c_param, struct move_param param, double *distance) { double angle; double target_angle, target_dis; int index; int panel_ID; // panel_ID = TEST_PANEL; index = param.axis - 1; // measure current cam angle from pot angle = cam_test_get_angle(¶m, c_param); // Calculate current cam position target_dis = *distance - param.cam_offset[index] * sin(angle - param.pot_zero[index]); // Calculate difference in angles target_angle = asin(target_dis / param.cam_offset[index]); if(fabs(target_angle) > PI4 * 2) cam_move_error_handling(1,"Cam angle > 90 Deg."); angle = (param.pot_zero[index] - target_angle - angle) * GEAR_RATIO / 6.2831853; // Calculate number of motor revolutions // param.step_size = angle * GEAR_RATIO / 6.2831853; // move motor move_cam_motor(panelHandle, ¶m, &angle, 0); return; } /* cam_test_rand_num * * This function moves the cam at specified distance * * * Yurii Levashov * 03/01/2005 */ short int cam_test_rand_num(void) { int err; int hours, minutes, seconds; err = GetSystemTime (&hours, &minutes, &seconds); srand((unsigned int)((hours + minutes + 1)/(seconds + 1))); // Create a seed for random numbers generator return (short int) (rand() % 1000); } /* cam_test_find_work_point * * This function moves the cam at specified number of steps and finds the maximum position * * Output: number of motor steps to the work point(in the middle of the slope) * * Yurii Levashov * 05/26/2005 */ double cam_test_find_work_point (struct move_param param, struct com_param c_param) { int i; int err; double pos[21], steps[21]; double ms_err; double coeff[2] = {0.00075, 0.0}; double temp[121]; int number_points = 20; double cf; int panel_ID; double angle; // panel_ID = TEST_PANEL; cf = 2 * PI / GEAR_RATIO; param.step_size = GEAR_RATIO / number_points; // take 20 points steps[0] = 0; angle = param.step_size; // move equal steps for(i = 0; i < number_points; i++) { // get positions pos[i] = cam_test_get_position(c_param, param.gage_number, 1); ms_err = cam_test_get_angle(¶m, c_param); // plot a point on the graph cam_graph_out(panelHandle, pos[i], steps[i]); // save number of steps steps[i + 1] = steps[i] + param.step_size; //move motor into the next position move_cam_motor(panelHandle, ¶m, &angle, 0); } // measure final cam position pos[number_points] = cam_test_get_position(c_param, param.gage_number, 1); ms_err = cam_test_get_angle(¶m, c_param); // plot a point on the graph cam_graph_out(panelHandle, pos[number_points], steps[number_points]); // make non-linear fit NonLinearFit (steps, pos, temp, number_points, cam_test_model_function, coeff, 2, &ms_err); // Calculate and output fitting curve on GUI for(i = 0; i < GEAR_RATIO; i++) { temp[i] = 1000 * coeff[0] * sin(cf * i + coeff[1]); // Plot the point on the graph err = PlotPoint (panelHandle, TEST_PANEL_GRAPH, i, temp[i], VAL_SIMPLE_DOT, VAL_DK_BLUE); if(err <= 0) cam_move_error_handling(err, " Problem plotting the graph"); } // Output coefficients on GUI SetCtrlVal (panelHandle, TEST_PANEL_AMPLITUDE, coeff[0]); SetCtrlVal (panelHandle, TEST_PANEL_PHASE, coeff[1]); SetCtrlAttribute (panelHandle, TEST_PANEL_AMPLITUDE, ATTR_VISIBLE, 1); SetCtrlAttribute (panelHandle, TEST_PANEL_PHASE, ATTR_VISIBLE, 1); DisplayPanel(panelHandle); //Check if wrong slope found if(pos[1] < pos[0]) return (GEAR_RATIO / 2. - coeff[1] / cf); // wrong slope else return (-coeff[1] / cf); } /* cam_test_cable_test * * This function moves the cam back and forth along the work range and saves the cam positions * * * Yurii Levashov * 03/01/2005 */ /*void cam_test_cable_test(int panel_ID, struct move_param param, char f_name[]) { double pos, pos1; double angle, angle1; int i; int meas_number = 20; FILE *file_ptr = NULL; int coeff = 29; int gage; // Open data file file_ptr = fopen(f_name, "a"); if (file_ptr == NULL) { cam_move_error_handling(1,"Unable to open log file"); fclose(file_ptr); return; } // find gage number if(param.axis < 4) gage = 1; else gage = 2; // find the cam work point param.step_size = cam_test_find_work_point(gage, param); // start test // for(i = 0; i < meas_number; i++) // { // if(i == 0) // the first run // { move_cam_motor(param); // move into work point ange = cam_test_get_angle(param, &angle); // zero position readings cam_test_zero_position(panel_ID, TEST_PANEL_GAGE1_OUT); // do measurements cam_test_get_position(&pos, gage, 1); // param.step_size = - coeff; // fprintf(file_ptr, " \n %6d %10.7f %8.3f ", i, pos * 1000000., angle * 180. / 3.141592653); } // param.step_size = GEAR_RATIO; // // move cam close to lowest point move_cam_motor(param); cam_test_get_angle(param, &angle); cam_test_get_position(&pos, GAGE_NUM, 1); // move cam into work point back param.step_size = coeff; move_cam_motor(param); // do measurements cam_test_get_position(&pos, GAGE_NUM, 1); cam_test_get_angle(param, &angle); // move cam close to highest point move_cam_motor(param); cam_test_get_angle(param, &angle1); cam_test_get_position(&pos1, GAGE_NUM, 1); // move to the work point back param.step_size = -coeff; move_cam_motor(param); // do measurements cam_test_get_position(&pos1, GAGE_NUM, 1); cam_test_get_angle(param, &angle1); // Write the measured values to the log file fprintf(file_ptr, " \n %6d %10.7f %10.7f %8.3f %8.3f", i + 1, pos * 1000000., pos1 * 1000000., angle / 3.141592653 * 180., angle1 / 3.141592653 * 180.); move_cam_motor(param); // do measurements cam_test_get_position(&pos, GAGE_NUM, 1); cam_test_get_angle(param, &angle); Fmt(mesg, "%saxis; //num_cams; // measure current cam angle from pot angle = (cam_move_read_cam_angles(m_param, c_param, cam_number) - m_param->pot_zero[cam_number - 1]) * m_param->angle_dir[cam_number - 1] ; // // Calculate number of motor revolutions distance = - angle * GEAR_RATIO / PI / 2.; //} // move motors move_cam_motor(panel_ID, m_param, &distance, LED_ON); // write to log Fmt(mesg,"%s * Cams moved to zero position\n", TimeStr()); write_to_log(log_file, mesg); return; } void cam_test_go_home(struct move_param *param, struct com_param c_param) { double angle, pos; int err; int i; int save_axis; // int gage; int panel_ID; /* if(param->axis < 4) gage = 1; else gage = 2; */ // save current axis save_axis = param->axis; // measure current cam angle from pot angle = cam_test_get_angle(param, c_param); // * RAD_TO_ANGLE; // move motors cam_test_move_angle(panelHandle, param, c_param, angle - param->init_angle[param->axis - 1] );// RAD_TO_ANGLE); // do measurements pos = cam_test_get_position(c_param, param->gage_number, 1); angle = cam_test_get_angle(param, c_param); // } // write to log Fmt(mesg,"%s * Cams moved to home position\n", TimeStr()); write_to_log(log_file, mesg); // set initial axis back param->axis = save_axis; return; } /********************************************************************************************************/ void cam_test_move_angle(int panel_ID, struct move_param *m_param, struct com_param c_param, double angle) { // double pos; int err, i; double target_angle; // int gage; // if(m_param->axis < 4) gage = 1; // else gage = 2; // find index // i = m_param->axis - 1; // Calculate target cam angle // target_angle = angle - param.pot_zero[i];// + param.init_angle[i] / RAD_TO_ANGLE; // Calculate number of motor revolutions m_param->step_size = angle * GEAR_RATIO / PI / 2.; // move motor move_cam_motor(panel_ID, m_param, &m_param->step_size, 0); return; }