Diff: Mirror_Kinematic.cpp

Revision:

0:d2e117716219

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Mirror_Kinematic.cpp	Sun May 02 19:32:30 2021 +0000
@@ -0,0 +1,104 @@
+#include "Mirror_Kinematic.h"
+// THis class describes the mirror kinematic.
+Mirror_Kinematic::Mirror_Kinematic(void) 
+{
+    screen_h = 102;
+    screen_d = 4;
+    dist_L = 16;
+    inc_offset[0] = inc_offset[1] = 0;
+    inc_additional_offset[0] = inc_additional_offset[1] = 0;
+    mot_inc_to_rad = (3.141592653589793 * 2.0) / 4000.0;
+    mot_rad_to_inc = 4000.0/(3.141592653589793 * 2.0);
+    n = 1.585;  // index of refraction, Brechungsindex
+    old_phi[0]=0.0;
+    old_phi[1]=0.0;
+    trafo_is_on = false;
+    external_control = false;
+    controller_is_on = true;
+    }
+    
+void Mirror_Kinematic::set_offsets(int16_t o1,int16_t o2)
+{
+    inc_offset[0] = o1;
+    inc_offset[1] = o2;
+    }
+void Mirror_Kinematic::set_additional_offsets(int16_t o1,int16_t o2)
+{
+    inc_additional_offset[0] = o1;
+    inc_additional_offset[1] = o2;
+    }
+void Mirror_Kinematic::add_additional_offsets(int16_t o1,int16_t o2)
+{
+    inc_additional_offset[0] += o1;
+    inc_additional_offset[1] += o2;
+    }
+int16_t Mirror_Kinematic::get_additional_offsets(uint8_t axis)
+{
+    if(axis>1)
+        return 0;
+    else
+        return inc_additional_offset[axis];
+    }
+// this is the transformation, transforming phi to xy values
+bool Mirror_Kinematic::P2X(float *P, float *X)
+{
+    // calculation time 5.7usec on F446RE   
+    float c1 = cosf(P[0]);
+    float c2 = cosf(P[1]);
+    float s1 = sinf(P[0]);
+    float s2 = -sinf(2.0f*P[1]); // alwayx sin(2*phi2) is needed
+                                // and: reverse the phi2 values!
+    float n1x=c1;               // normal vector of 2nd laser beam (from mirror 1 -> mirror 2)
+    float n1y=c1*s1;
+    float n1z=s1*s1;
+    float sq2=sqrt(.5);
+    // i.e.: cos(2*x) = 2*cos(x)^2-1
+    //float n2x = n1z*cosf(2.0f*P[1]) + n1x*sinf(2.0f*P[1]);
+    float n2x = n1z*(2.0f*c2*c2-1.0f) + n1x*s2;  // normal vector of 3rd laser beam (mirror 2 -> screen)
+    float n2y = n1y;
+    float a1 = sq2*c2 - sq2*s1;
+    float a2 = sq2*c2 + sq2*s1;
+    float dum1 = n1x * a1 - n1z*a2;
+    float n2z = n1x*(2.0f*c2*c2-1.0f) - n1z*s2;
+    if(dum1*n2z == 0)
+        return false;
+    float dad = dist_L * a1/dum1;
+    float Q2x = dad * n1x - dist_L;
+    float Q2y = dad * n1y;
+    float Q2z = dad * n1z;
+    float x = atanf(n2x/n2z)/n;
+    float y = atanf(n2y/n2z)/n;
+    float dx = screen_d * x/sqrt(1-x*x);
+    float dy = screen_d * y/sqrt(1-y*y);
+    X[0] =  Q2x + (n2x*(screen_h - Q2z))/n2z - dx;
+    X[1] =  Q2y + (n2y*(screen_h - Q2z))/n2z - dy;
+    
+    return true;
+    }
+// this is the trafo from xy -> phi1,phi2, not possible in an analytical form, use
+// GN-iteration with a constant Jacobian
+bool Mirror_Kinematic::X2P(float *X, float *P)
+{
+    float J12 = 0.0090517133f;
+    float J21 = -0.0052923231f;     // on motor angle co-ordinates, we have a negative sensitivity
+    float Xn[2];
+    float dx = 1e4;
+    float dy = 1e4;
+    P[0] = old_phi[0];
+    P[1] = old_phi[1];
+    uint8_t k = 0;
+    do
+        {
+            if( !P2X(P,Xn))
+                return false;
+            dx = Xn[0]-X[0];
+            dy = Xn[1]-X[1];
+            P[0] -= J12 * dy;
+            P[1] -= J21 * dx;
+        }
+        while((dx*dx+dy*dy) > 1e-3 && ++k<20);
+    data.num_it = k;
+    old_phi[0] = P[0];
+    old_phi[1] = P[1];
+    return true;
+}
\ No newline at end of file