Diff: Mirror_Kinematic.cpp

Revision:

11:d43f8b421d6d

Parent:

7:942fd77d5e19

--- a/Mirror_Kinematic.cpp	Wed Apr 28 08:26:37 2021 +0000
+++ b/Mirror_Kinematic.cpp	Wed Apr 28 12:51:02 2021 +0000
@@ -1,15 +1,15 @@
 #include "Mirror_Kinematic.h"
-
+// THis class describes the mirror kinematic.
 Mirror_Kinematic::Mirror_Kinematic(void) 
 {
-    screen_h = 97;
+    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;  // Brechungsindex
+    n = 1.585;  // index of refraction, Brechungsindex
     old_phi[0]=0.0;
     old_phi[1]=0.0;
     trafo_is_on = false;
@@ -39,20 +39,22 @@
     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]);
-    float n1x=c1;
+    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;
+    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;
@@ -73,11 +75,12 @@
     
     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;
+    float J21 = -0.0052923231f;     // on motor angle co-ordinates, we have a negative sensitivity
     float Xn[2];
     float dx = 1e4;
     float dy = 1e4;