Ruprecht Altenburger / Mbed OS mirror_actuator_V1

Mirror actuator for RT2 lab

Dependencies:   FastPWM

Mirror_Kinematic.cpp@11:d43f8b421d6d, 2021-04-28 (annotated)

Committer:
altb2
Date:
Wed Apr 28 12:51:02 2021 +0000
Revision:
11:d43f8b421d6d
Parent:
7:942fd77d5e19 
interm2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 7:942fd77d5e19 1 #include "Mirror_Kinematic.h"
altb2 11:d43f8b421d6d 2 // THis class describes the mirror kinematic.
altb2 7:942fd77d5e19 3 Mirror_Kinematic::Mirror_Kinematic(void)
altb2 7:942fd77d5e19 4 {
altb2 11:d43f8b421d6d 5 screen_h = 102;
altb2 7:942fd77d5e19 6 screen_d = 4;
altb2 7:942fd77d5e19 7 dist_L = 16;
altb2 7:942fd77d5e19 8 inc_offset[0] = inc_offset[1] = 0;
altb2 7:942fd77d5e19 9 inc_additional_offset[0] = inc_additional_offset[1] = 0;
altb2 7:942fd77d5e19 10 mot_inc_to_rad = (3.141592653589793 * 2.0) / 4000.0;
altb2 7:942fd77d5e19 11 mot_rad_to_inc = 4000.0/(3.141592653589793 * 2.0);
altb2 11:d43f8b421d6d 12 n = 1.585; // index of refraction, Brechungsindex
altb2 7:942fd77d5e19 13 old_phi[0]=0.0;
altb2 7:942fd77d5e19 14 old_phi[1]=0.0;
altb2 7:942fd77d5e19 15 trafo_is_on = false;
altb2 7:942fd77d5e19 16 external_control = false;
altb2 7:942fd77d5e19 17 controller_is_on = true;
altb2 7:942fd77d5e19 18 }
altb2 7:942fd77d5e19 19
altb2 7:942fd77d5e19 20 void Mirror_Kinematic::set_offsets(int16_t o1,int16_t o2)
altb2 7:942fd77d5e19 21 {
altb2 7:942fd77d5e19 22 inc_offset[0] = o1;
altb2 7:942fd77d5e19 23 inc_offset[1] = o2;
altb2 7:942fd77d5e19 24 }
altb2 7:942fd77d5e19 25 void Mirror_Kinematic::set_additional_offsets(int16_t o1,int16_t o2)
altb2 7:942fd77d5e19 26 {
altb2 7:942fd77d5e19 27 inc_additional_offset[0] = o1;
altb2 7:942fd77d5e19 28 inc_additional_offset[1] = o2;
altb2 7:942fd77d5e19 29 }
altb2 7:942fd77d5e19 30 void Mirror_Kinematic::add_additional_offsets(int16_t o1,int16_t o2)
altb2 7:942fd77d5e19 31 {
altb2 7:942fd77d5e19 32 inc_additional_offset[0] += o1;
altb2 7:942fd77d5e19 33 inc_additional_offset[1] += o2;
altb2 7:942fd77d5e19 34 }
altb2 7:942fd77d5e19 35 int16_t Mirror_Kinematic::get_additional_offsets(uint8_t axis)
altb2 7:942fd77d5e19 36 {
altb2 7:942fd77d5e19 37 if(axis>1)
altb2 7:942fd77d5e19 38 return 0;
altb2 7:942fd77d5e19 39 else
altb2 7:942fd77d5e19 40 return inc_additional_offset[axis];
altb2 7:942fd77d5e19 41 }
altb2 11:d43f8b421d6d 42 // this is the transformation, transforming phi to xy values
altb2 7:942fd77d5e19 43 bool Mirror_Kinematic::P2X(float *P, float *X)
altb2 7:942fd77d5e19 44 {
altb2 7:942fd77d5e19 45 // calculation time 5.7usec on F446RE
altb2 7:942fd77d5e19 46 float c1 = cosf(P[0]);
altb2 7:942fd77d5e19 47 float c2 = cosf(P[1]);
altb2 7:942fd77d5e19 48 float s1 = sinf(P[0]);
altb2 11:d43f8b421d6d 49 float s2 = -sinf(2.0f*P[1]); // alwayx sin(2*phi2) is needed
altb2 11:d43f8b421d6d 50 // and: reverse the phi2 values!
altb2 11:d43f8b421d6d 51 float n1x=c1; // normal vector of 2nd laser beam (from mirror 1 -> mirror 2)
altb2 7:942fd77d5e19 52 float n1y=c1*s1;
altb2 7:942fd77d5e19 53 float n1z=s1*s1;
altb2 7:942fd77d5e19 54 float sq2=sqrt(.5);
altb2 7:942fd77d5e19 55 // i.e.: cos(2*x) = 2*cos(x)^2-1
altb2 7:942fd77d5e19 56 //float n2x = n1z*cosf(2.0f*P[1]) + n1x*sinf(2.0f*P[1]);
altb2 11:d43f8b421d6d 57 float n2x = n1z*(2.0f*c2*c2-1.0f) + n1x*s2; // normal vector of 3rd laser beam (mirror 2 -> screen)
altb2 7:942fd77d5e19 58 float n2y = n1y;
altb2 7:942fd77d5e19 59 float a1 = sq2*c2 - sq2*s1;
altb2 7:942fd77d5e19 60 float a2 = sq2*c2 + sq2*s1;
altb2 7:942fd77d5e19 61 float dum1 = n1x * a1 - n1z*a2;
altb2 7:942fd77d5e19 62 float n2z = n1x*(2.0f*c2*c2-1.0f) - n1z*s2;
altb2 7:942fd77d5e19 63 if(dum1*n2z == 0)
altb2 7:942fd77d5e19 64 return false;
altb2 7:942fd77d5e19 65 float dad = dist_L * a1/dum1;
altb2 7:942fd77d5e19 66 float Q2x = dad * n1x - dist_L;
altb2 7:942fd77d5e19 67 float Q2y = dad * n1y;
altb2 7:942fd77d5e19 68 float Q2z = dad * n1z;
altb2 7:942fd77d5e19 69 float x = atanf(n2x/n2z)/n;
altb2 7:942fd77d5e19 70 float y = atanf(n2y/n2z)/n;
altb2 7:942fd77d5e19 71 float dx = screen_d * x/sqrt(1-x*x);
altb2 7:942fd77d5e19 72 float dy = screen_d * y/sqrt(1-y*y);
altb2 7:942fd77d5e19 73 X[0] = Q2x + (n2x*(screen_h - Q2z))/n2z - dx;
altb2 7:942fd77d5e19 74 X[1] = Q2y + (n2y*(screen_h - Q2z))/n2z - dy;
altb2 7:942fd77d5e19 75
altb2 7:942fd77d5e19 76 return true;
altb2 7:942fd77d5e19 77 }
altb2 11:d43f8b421d6d 78 // this is the trafo from xy -> phi1,phi2, not possible in an analytical form, use
altb2 11:d43f8b421d6d 79 // GN-iteration with a constant Jacobian
altb2 7:942fd77d5e19 80 bool Mirror_Kinematic::X2P(float *X, float *P)
altb2 7:942fd77d5e19 81 {
altb2 7:942fd77d5e19 82 float J12 = 0.0090517133f;
altb2 11:d43f8b421d6d 83 float J21 = -0.0052923231f; // on motor angle co-ordinates, we have a negative sensitivity
altb2 7:942fd77d5e19 84 float Xn[2];
altb2 7:942fd77d5e19 85 float dx = 1e4;
altb2 7:942fd77d5e19 86 float dy = 1e4;
altb2 7:942fd77d5e19 87 P[0] = old_phi[0];
altb2 7:942fd77d5e19 88 P[1] = old_phi[1];
altb2 7:942fd77d5e19 89 uint8_t k = 0;
altb2 7:942fd77d5e19 90 do
altb2 7:942fd77d5e19 91 {
altb2 7:942fd77d5e19 92 if( !P2X(P,Xn))
altb2 7:942fd77d5e19 93 return false;
altb2 7:942fd77d5e19 94 dx = Xn[0]-X[0];
altb2 7:942fd77d5e19 95 dy = Xn[1]-X[1];
altb2 7:942fd77d5e19 96 P[0] -= J12 * dy;
altb2 7:942fd77d5e19 97 P[1] -= J21 * dx;
altb2 7:942fd77d5e19 98 }
altb2 7:942fd77d5e19 99 while((dx*dx+dy*dy) > 1e-3 && ++k<20);
altb2 7:942fd77d5e19 100 data.num_it = k;
altb2 7:942fd77d5e19 101 old_phi[0] = P[0];
altb2 7:942fd77d5e19 102 old_phi[1] = P[1];
altb2 7:942fd77d5e19 103 return true;
altb2 7:942fd77d5e19 104 }