/* 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" #include DELAY //adds most commonly used functions #include #include #include #include #include #include #include "hp34970_sw.h" #include "cam_movers_dec.h" #include "cam_movers_param.h" #include "cm6k6_g.h" /* Public Variables Declaration*/ static int cam_hp34970_ID; // HP Data Acquisition/Switch module static int cm6k6_ID; // Compumotor Controller /* 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[], struct com_param *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 // Save log file name strcpy(log_file, log_file_in); // Initialize voltmeter if(cam_hp34970_ID <= 0) { hp34970_init(param->gpib_addr, param->hp34970_addr, &dev_ID); cam_hp34970_ID = dev_ID; if(dev_ID <= 0) { write_to_log(log_file, "PROBLEM: HP34970 is not initialized"); printf("PROBLEM: HP34970 is not initialized\n"); return dev_ID; } // Reset slots to the power ON state hp34970_reset_slots(cam_hp34970_ID); } // Initialize motor controller if(cm6k6_ID <= 0) { printf("\nInitializing CM6K6...\n"); cm6k6_init(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"); printf("PROBLEM: CM6K6 is not initialized"); return dev_ID; } printf("Initialization of CM6K6 complete.\n"); } // write to log file write_to_log(log_file, "METER: HP34970 \t MOTOR DRIVER: CM6K6 \t Initialized OK"); // Pause Delay(2.0); return 0; } /* ************************************************************** */ /* cam_fill_param_structure * * This function fills com_param and move_param structures using data from param.h files. * * Yurii Levashov * 07/01/2005 */ void cam_fill_param_struct(struct move_param *m_param, struct com_param *c_param) { // Fill com_param structure c_param->gpib_addr = CAM_HP_GPIB_ADDR; c_param->cm6k6_addr = CM6K6_DEV_ADDR; c_param->hp34970_addr = HP34970_CAM_MOVER_ADDR; c_param->cm6k6_com_port = CM6K6_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; m_param->accel = MOT_ACC; m_param->time_int = TIME_INT; m_param->distance = 0; m_param->num_cams = CAM_NUM; // 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; // initial coordinates m_param->cam_coord[X_CIND] = CAM_START_X; m_param->cam_coord[Y_CIND] = CAM_START_Y; m_param->cam_coord[ROLL_CIND] = CAM_START_ROLL; m_param->cam_coord[PITCH_CIND] = CAM_START_PITCH; m_param->cam_coord[YAW_CIND] = CAM_START_YAW; return; } /************************************************************** */ /* cam_move_get_coordinates * * This function reads angles from pots and converts them to coordinates * * Michael Levashov * 07/23/2007 */ void cam_move_get_coordinates(struct move_param *mv_param, struct com_param *cm_param) { int i; // 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); // calculate final coordinates cam_move_angles_to_coord(mv_param, mv_param->cam_angle); printf("X: %f Y: %f Roll: %f Pitch: %f, Yaw: %f\n", mv_param->cam_coord[X_CIND],mv_param->cam_coord[Y_CIND],mv_param->cam_coord[ROLL_CIND],mv_param->cam_coord[PITCH_CIND], mv_param->cam_coord[YAW_CIND]); return; } /* ************************************************************** */ /* cam_move_zero_coord * * This function zeroes coordinates and angles in the global structure * * 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 * * 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 * * Yurii Levashov * 12/14/2005 */ void cam_move_read_motion_data(struct move_param *mv_param) { 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 (CAM_DATA_DIR, CAM_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); // write to log Fmt(mesg,"Motion data read from file %s", data_file); write_to_log(log_file, mesg); cam_move_log_motion_data(" ", mv_param); 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[p6] Y = %f[p6] Roll = %f[p6] Pitch = %f[p6] Yaw = %f[p6]", TimeStr(), txt, mv_param->cam_coord[X_CIND], mv_param->cam_coord[Y_CIND] ,mv_param->cam_coord[ROLL_CIND],mv_param->cam_coord[PITCH_CIND],mv_param->cam_coord[YAW_CIND]); write_to_log(log_file, mesg); return; } /* ************************************************************** */ /* cam_move_go * This function receives coordinates and angles, calculates cam angles and * commands motors to move, reads final coordinates * * Yurii Levashov * 10/12/2004 * * Heavily modified by Michael Levashov 07/23/2007 */ void cam_move_go(struct com_param *cm_param, struct move_param *mv_param, int mode) { double f_angle[5]; // angle[] - target; f_angle[] - final int i, err; double coord_target[5], coord_init[5]; double distance[5]; static int file_open = 0; double v_coord[4]; // 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]; } 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]; } // Calculate target coordinates of vertex points cam_move_coord_to_angles(mv_param, v_coord); // Calculate new cam angles cam_move_cam_angles(mv_param, f_angle, v_coord); //new cam angles(target) in f_angle // 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(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?"); } //move motor move_cam_motor(mv_param, distance); // Read final cam angles from Pots and calculate final coordinates cam_move_get_coordinates(mv_param, cm_param); 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[X_CIND] = (x_a + x_b) / 2.; // new x m_param->cam_coord[Y_CIND] = (y_a + y_b) / 2.; // new y m_param->cam_coord[ROLL_CIND] = phi; // Roll m_param->cam_coord[YAW_CIND] = asin((x_b - x_a) / L); // Yaw m_param->cam_coord[PITCH_CIND] = 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[X_CIND] - L / 2. * sin(m_param->cam_coord[YAW_CIND]); Y_A = m_param->cam_coord[Y_CIND] - L / 2. * sin(m_param->cam_coord[PITCH_CIND]); X_B = m_param->cam_coord[X_CIND] + L / 2. * sin(m_param->cam_coord[YAW_CIND]); Y_B = m_param->cam_coord[Y_CIND] + L / 2. * sin(m_param->cam_coord[PITCH_CIND]); // Calculate new coordinates of vertex points phi = m_param->cam_coord[ROLL_CIND]; // 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(PI4*2 - THREE_CAM_ANGLE + alpha0); yb = Y_b + CAM_RADIUS * sin(PI4*2 - THREE_CAM_ANGLE + alpha0); xc = X_c - CAM_RADIUS * cos(PI4*2 - THREE_CAM_ANGLE - alpha0); yc = Y_c + CAM_RADIUS * sin(PI4*2 - 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[ROLL_CIND]; // 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 voltage, max_voltage; double angle; int err; int index; index = pot_number - 1; // Check, if 34970A voltmeter is not initialized, initialize it if (cam_hp34970_ID <= 0) hp34970_init(cm_param->gpib_addr, cm_param->hp34970_addr, &cam_hp34970_ID); /// apply voltage to the Pot hp34970_switch_on_off(cam_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(cam_hp34970_ID, 100+pot_number, &voltage); hp34970_get_chan_volt(cam_hp34970_ID, 110, &max_voltage); angle = m_param->angle_dir[index] * (voltage / max_voltage - 0.5) * m_param->pot_range[index]- m_param->pot_zero[index]; // disconnect the voltage from the Pot hp34970_switch_on_off(cam_hp34970_ID, POT_SWITCH_POSITION, 0); return angle; } /* ************************************************************** */ /* cam_move_exit * * This function closes devices and GUI * * Yurii Levashov * 10/12/2004 */ void cam_move_exit(void) { // close devices // Check, if 34970A voltmeter is initialized, close it if (cam_hp34970_ID > 0) hp34970_exit(cam_hp34970_ID); // Check, if cm6k6 controller is initialized, close it if (cm6k6_ID > 0) cm6k6_exit(cm6k6_ID);; return; } /* move_cam_motor * * This function commands motor to move a specified number of steps. * Input: Moving parameters structure param. * * Yurii Levashov * 02/22/2005 * Modified by Michael Levashov 07/02/2007 */ void move_cam_motor(struct move_param *param, double distance[]) { double rev; int err , i; int n_cams; n_cams = param->num_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]; // Release motor brakes hp34970_switch_on_off(cam_hp34970_ID, i + 1, 1); } err = cm6k6_cam_move_5(0, 0, cm6k6_ID, distance, 0); // Apply motor brakes for(i = 0; i < n_cams; i++) hp34970_switch_on_off(cam_hp34970_ID, i + 1, 0); } else { // distance[0] *= param->motor_dir[param->axis - 1]; hp34970_switch_on_off(cam_hp34970_ID, param->axis, 1); cm6k6_cam_move(0, 0, cm6k6_ID, param->axis, distance[0] * param->motor_dir[param->axis - 1], 0); //distance[param->axis - 1], 0); hp34970_switch_on_off(cam_hp34970_ID, param->axis, 0); } 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(struct move_param *m_param, struct com_param c_param) { double angle, pos; int err, i; double distance[CAM_NUM]; int cam_number; 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, i+1); // Calculate number of motor revolutions distance[i] = - angle * GEAR_RATIO / PI / 2.; } move_cam_motor(m_param, distance); // 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(CAM_LOG_DIR, "*.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, "%s \n ", message); // close file fclose(file_ptr); } return; } /* ************************************************************** */ /* error_handling * * This function inform operator about error and error codes * * Yurii Levashov * 09/10/2004 */ void cam_move_error_handling(int err, char descr[]) { char msg[200]; Fmt(msg, "%s