/* * Sextupole Magnet Mover System Calculations * ========================================= * * This module provides coordinate transformations between real-space coordinates * and hardware positions for the sextupole magnet mover system. * * COORDINATE SYSTEMS: * ------------------ * 1. Real-space coordinates (X, Y, Theta): * - X: Horizontal position in mm * - Y: Vertical position in mm * - Theta: Rotation angle in milliradians * * 2. Hardware positions: * - LVDT positions: Linear Variable Differential Transformer readings in mm * - CAM angles: Rotational positions of cam motors in degrees * * FUNCTIONS: * --------- * CalculateInit(): * Initialization function for sextupole calculations. * Currently doesn't perform any operations but could be used for future setup needs. * * CalculateLVDT(subRecord *psub): * Transforms between LVDT positions and real-space coordinates. * Inputs: * a, b, c: Current LVDT positions in mm * g, h, i: Desired real-space coordinates (mm, mm, mrad) * Outputs: * d, e, f: Calculated real-space coordinates from LVDT positions * j, k, l: Calculated LVDT positions from desired real-space coordinates * * CalculateCAMS(subRecord *psub): * Transforms between cam angles and real-space coordinates. * Inputs: * a, b, c: Current cam angles in degrees * g, h, i: Desired real-space coordinates (mm, mm, mrad) * Outputs: * d, e, f: Calculated real-space coordinates from cam angles * j, k, l: Calculated cam angles from desired real-space coordinates * * GEOMETRIC CONSTANTS: * ------------------ * The system uses several geometric constants specific to the * sextupole magnet mover design, including radii, lengths, and offsets. * These are defined as static constants in the code. */ #include #include #include #include #include // Geometric constants for the sextupole magnet mover system static const float DYNAMICS_R = 31.0; static const float DYNAMICS_L = 1.5875; static const float DYNAMICS_S1 = 34.925; static const float DYNAMICS_S2 = 290.5; static const float DYNAMICS_C = 282.0416; static const float DYNAMICS_A = 139.6238; static const float DYNAMICS_DX1 = 74.55; static const float DYNAMICS_DX2 = 89.54; static const float DYNAMICS_DY = 245.47; static const float DYNAMICS_B = 273.12; // Unit conversion constants static const float RADIANS_TO_MILLIRADIANS = 1e3; static const float MILLIRADIANS_TO_RADIANS = 1e-3; static const float RADIANS_TO_DEGREES = 180.0 / M_PI; static const float DEGREES_TO_RADIANS = M_PI / 180.0; // Initialization function for sextupole calculations static long CalculateInit(subRecord *psub) { return 0; } // Calculates LVDT positions from real-space coordinates and vice versa static long CalculateLVDT(subRecord *psub) { // Calculate real-space coordinates from LVDT positions const float lvdt_position_s1 = (float)psub->a; const float lvdt_position_s2 = (float)psub->b; const float lvdt_position_s3 = (float)psub->c; const float calculate_real_cy = (lvdt_position_s1 + lvdt_position_s2) / 2.0; const float calculate_real_ct = atanf((lvdt_position_s1 - lvdt_position_s2) / (DYNAMICS_DX1 + DYNAMICS_DX2)); const float calculate_real_cx = lvdt_position_s3 - (tanf(calculate_real_ct) * (calculate_real_cy + DYNAMICS_DY)); // Calculate LVDT positions from desired real-space coordinates const float desired_real_cx = (float)psub->g; const float desired_real_cy = (float)psub->h; const float desired_real_ct = (float)psub->i * MILLIRADIANS_TO_RADIANS; const float p1 = desired_real_cy + ((tanf(desired_real_ct) * (DYNAMICS_DX1 + DYNAMICS_DX2)) / 2.0); const float p2 = desired_real_cy - ((tanf(desired_real_ct) * (DYNAMICS_DX1 + DYNAMICS_DX2)) / 2.0); const float p3 = desired_real_cx + (tanf(desired_real_ct) * (desired_real_cy + DYNAMICS_DY)); // Real-space coordinates from LVDT positions psub->d = calculate_real_cx; psub->e = calculate_real_cy; psub->f = calculate_real_ct * RADIANS_TO_MILLIRADIANS; // LVDT positions from desired real-space coordinates psub->j = isnan(p1) ? NAN : p1; psub->k = isnan(p2) ? NAN : p2; psub->l = isnan(p3) ? NAN : p3; return 0; } // Calculates cam angles from real-space coordinates and vice versa static long CalculateCAMS(subRecord *psub) { // Calculate real-space coordinates from cam angles const float cam_position_s1 = (float)psub->a * DEGREES_TO_RADIANS; const float cam_position_s2 = (float)psub->b * DEGREES_TO_RADIANS; const float cam_position_s3 = (float)psub->c * DEGREES_TO_RADIANS; // Desired real-space coordinates for reverse calculation const float desired_real_cx = (float)psub->g; const float desired_real_cy = (float)psub->h; const float desired_real_ct = (float)psub->i * MILLIRADIANS_TO_RADIANS; // Precalculate cosines and sines of cam angles const float cam_cos[3] = {cosf(cam_position_s1), cosf(cam_position_s2), cosf(cam_position_s3)}; const float cam_sin[3] = {sinf(cam_position_s1), sinf(cam_position_s2), sinf(cam_position_s3)}; const int itermax = 100; // Initial values for iteration float calculate_real_cx = 0.0; float calculate_real_cy = DYNAMICS_C - DYNAMICS_B; float calculate_real_ct = 0.0; // Iterate to find real-space coordinates from cam angles int i = 0; for (i = 0; i < itermax; i++) { const float t_new = ((calculate_real_cy + DYNAMICS_B - DYNAMICS_C) + (DYNAMICS_L * ((calculate_real_ct * cam_cos[0]) - cam_sin[0]))) / (DYNAMICS_S2 + calculate_real_cx); const float x_new = (calculate_real_ct * (DYNAMICS_S1 - calculate_real_cy)) + ((DYNAMICS_L / sqrtf(2.0)) * ((calculate_real_ct * cam_cos[1]) - cam_sin[1] + (calculate_real_ct * cam_cos[2]) - cam_sin[2])); const float y_new = (sqrtf(2.0) * DYNAMICS_R) - DYNAMICS_S1 + (calculate_real_ct * calculate_real_cx) + ((DYNAMICS_L / sqrtf(2.0)) * ((calculate_real_ct * cam_cos[1]) - cam_sin[1] - (calculate_real_ct * cam_cos[2]) + cam_sin[2])); calculate_real_cx = x_new; calculate_real_cy = y_new; calculate_real_ct = t_new; } // Final adjustment to real-space coordinates calculate_real_cx = calculate_real_cx + DYNAMICS_A - (cosf(calculate_real_ct) * DYNAMICS_A) - (sinf(calculate_real_ct) * DYNAMICS_B); calculate_real_cy = calculate_real_cy - DYNAMICS_C + (cosf(calculate_real_ct) * DYNAMICS_B) - (sinf(calculate_real_ct) * DYNAMICS_A); // Convert angle to milliradians for output const float calculate_real_ct_mrad = calculate_real_ct * RADIANS_TO_MILLIRADIANS; // Reverse calculation: calculate cam angles from desired real-space coordinates const float x1 = desired_real_cx + (DYNAMICS_A * cosf(desired_real_ct)) + (DYNAMICS_B * sinf(desired_real_ct)) - DYNAMICS_A; const float y1 = desired_real_cy - (DYNAMICS_B * cosf(desired_real_ct)) + (DYNAMICS_A * sinf(desired_real_ct)) + DYNAMICS_C; const float bp = (cosf(desired_real_ct) + sinf(desired_real_ct)) / sqrtf(2.0); const float bm = (cosf(desired_real_ct) - sinf(desired_real_ct)) / sqrtf(2.0); const float p1 = desired_real_ct - asinf((1.0 / DYNAMICS_L) * (((x1 + DYNAMICS_S2) * sinf(desired_real_ct)) - (y1 * cosf(desired_real_ct)) + (DYNAMICS_C - DYNAMICS_B))); const float p2 = desired_real_ct - asinf((1.0 / DYNAMICS_L) * (((x1 + DYNAMICS_S1) * bm) + (y1 * bp) - DYNAMICS_R)); const float p3 = desired_real_ct - asinf((1.0 / DYNAMICS_L) * (((x1 - DYNAMICS_S1) * bp) - (y1 * bm) + DYNAMICS_R)); // Output calculated real-space coordinates psub->d = calculate_real_cx; psub->e = calculate_real_cy; psub->f = calculate_real_ct_mrad; // Output calculated cam angles in degrees psub->j = isnan(p1) ? NAN : p1 * RADIANS_TO_DEGREES; psub->k = isnan(p2) ? NAN : p2 * RADIANS_TO_DEGREES; psub->l = isnan(p3) ? NAN : p3 * RADIANS_TO_DEGREES; return 0; } // Register subroutine functions for use in EPICS records epicsRegisterFunction(CalculateInit); epicsRegisterFunction(CalculateLVDT); epicsRegisterFunction(CalculateCAMS);