![](/media/cache/profiles/altb_JGUSAOG.jpg.50x50_q85.jpg)
Mirror actuator for RT2 lab
Mirror_Kinematic.cpp
- Committer:
- altb2
- Date:
- 2021-04-15
- Revision:
- 7:942fd77d5e19
- Child:
- 11:d43f8b421d6d
File content as of revision 7:942fd77d5e19:
#include "Mirror_Kinematic.h" Mirror_Kinematic::Mirror_Kinematic(void) { screen_h = 97; 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 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]; } 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 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 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; } bool Mirror_Kinematic::X2P(float *X, float *P) { float J12 = 0.0090517133f; float J21 = 0.0052923231f; 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; }