/* * 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 = atan((lvdt_position_s1 - lvdt_position_s2) / (DYNAMICS_DX1 + DYNAMICS_DX2)); const float calculate_real_cx = lvdt_position_s3 - (tan(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) ? 0 : p1; psub->k = isnan(p2) ? 0 : p2; psub->l = isnan(p3) ? 0 : 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] = { cos(cam_position_s1), cos(cam_position_s2), cos(cam_position_s3) }; const float cam_sin[3] = { sin(cam_position_s1), sin(cam_position_s2), sin(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/sqrt(2.0)) * ((calculate_real_ct * cam_cos[1]) - cam_sin[1] + (calculate_real_ct * cam_cos[2]) - cam_sin[2])); const float y_new = (sqrt(2.0) * DYNAMICS_R) - DYNAMICS_S1 + (calculate_real_ct * calculate_real_cx) + ((DYNAMICS_L / sqrt(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 - (cos(calculate_real_ct) * DYNAMICS_A) - (sin(calculate_real_ct) * DYNAMICS_B); calculate_real_cy = calculate_real_cy - DYNAMICS_C + (cos(calculate_real_ct) * DYNAMICS_B) - (sin(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 * cos(desired_real_ct)) + (DYNAMICS_B * sin(desired_real_ct)) - DYNAMICS_A; const float y1 = desired_real_cy - (DYNAMICS_B * cos(desired_real_ct)) + (DYNAMICS_A * sin(desired_real_ct)) + DYNAMICS_C; const float bp = (cos(desired_real_ct) + sin(desired_real_ct))/sqrtf(2.0); const float bm = (cos(desired_real_ct) - sin(desired_real_ct))/sqrtf(2.0); const float p1 = desired_real_ct - asinf((1.0/DYNAMICS_L) * (((x1 + DYNAMICS_S2) * sin(desired_real_ct)) - (y1 * cos(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) ? 0 : p1 * RADIANS_TO_DEGREES; psub->k = isnan(p2) ? 0 : p2 * RADIANS_TO_DEGREES; psub->l = isnan(p3) ? 0 : p3 * RADIANS_TO_DEGREES; return 0; } /* Register subroutine functions for use in EPICS records */ epicsRegisterFunction(CalculateInit); epicsRegisterFunction(CalculateLVDT); epicsRegisterFunction(CalculateCAMS);