Ruprecht Altenburger
/
mirror_actuator_V1
Mirror actuator for RT2 lab
Mirror_Kinematic.cpp@7:942fd77d5e19, 2021-04-15 (annotated)
- Committer:
- altb2
- Date:
- Thu Apr 15 05:36:55 2021 +0000
- Revision:
- 7:942fd77d5e19
- Child:
- 11:d43f8b421d6d
intermediate
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 7:942fd77d5e19 | 1 | #include "Mirror_Kinematic.h" |
altb2 | 7:942fd77d5e19 | 2 | |
altb2 | 7:942fd77d5e19 | 3 | Mirror_Kinematic::Mirror_Kinematic(void) |
altb2 | 7:942fd77d5e19 | 4 | { |
altb2 | 7:942fd77d5e19 | 5 | screen_h = 97; |
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 | 7:942fd77d5e19 | 12 | n = 1.585; // 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 | 7:942fd77d5e19 | 42 | bool Mirror_Kinematic::P2X(float *P, float *X) |
altb2 | 7:942fd77d5e19 | 43 | { |
altb2 | 7:942fd77d5e19 | 44 | // calculation time 5.7usec on F446RE |
altb2 | 7:942fd77d5e19 | 45 | float c1 = cosf(P[0]); |
altb2 | 7:942fd77d5e19 | 46 | float c2 = cosf(P[1]); |
altb2 | 7:942fd77d5e19 | 47 | float s1 = sinf(P[0]); |
altb2 | 7:942fd77d5e19 | 48 | float s2 = sinf(2.0f*P[1]); |
altb2 | 7:942fd77d5e19 | 49 | float n1x=c1; |
altb2 | 7:942fd77d5e19 | 50 | float n1y=c1*s1; |
altb2 | 7:942fd77d5e19 | 51 | float n1z=s1*s1; |
altb2 | 7:942fd77d5e19 | 52 | float sq2=sqrt(.5); |
altb2 | 7:942fd77d5e19 | 53 | // i.e.: cos(2*x) = 2*cos(x)^2-1 |
altb2 | 7:942fd77d5e19 | 54 | //float n2x = n1z*cosf(2.0f*P[1]) + n1x*sinf(2.0f*P[1]); |
altb2 | 7:942fd77d5e19 | 55 | float n2x = n1z*(2.0f*c2*c2-1.0f) + n1x*s2; |
altb2 | 7:942fd77d5e19 | 56 | float n2y = n1y; |
altb2 | 7:942fd77d5e19 | 57 | float a1 = sq2*c2 - sq2*s1; |
altb2 | 7:942fd77d5e19 | 58 | float a2 = sq2*c2 + sq2*s1; |
altb2 | 7:942fd77d5e19 | 59 | float dum1 = n1x * a1 - n1z*a2; |
altb2 | 7:942fd77d5e19 | 60 | float n2z = n1x*(2.0f*c2*c2-1.0f) - n1z*s2; |
altb2 | 7:942fd77d5e19 | 61 | if(dum1*n2z == 0) |
altb2 | 7:942fd77d5e19 | 62 | return false; |
altb2 | 7:942fd77d5e19 | 63 | float dad = dist_L * a1/dum1; |
altb2 | 7:942fd77d5e19 | 64 | float Q2x = dad * n1x - dist_L; |
altb2 | 7:942fd77d5e19 | 65 | float Q2y = dad * n1y; |
altb2 | 7:942fd77d5e19 | 66 | float Q2z = dad * n1z; |
altb2 | 7:942fd77d5e19 | 67 | float x = atanf(n2x/n2z)/n; |
altb2 | 7:942fd77d5e19 | 68 | float y = atanf(n2y/n2z)/n; |
altb2 | 7:942fd77d5e19 | 69 | float dx = screen_d * x/sqrt(1-x*x); |
altb2 | 7:942fd77d5e19 | 70 | float dy = screen_d * y/sqrt(1-y*y); |
altb2 | 7:942fd77d5e19 | 71 | X[0] = Q2x + (n2x*(screen_h - Q2z))/n2z - dx; |
altb2 | 7:942fd77d5e19 | 72 | X[1] = Q2y + (n2y*(screen_h - Q2z))/n2z - dy; |
altb2 | 7:942fd77d5e19 | 73 | |
altb2 | 7:942fd77d5e19 | 74 | return true; |
altb2 | 7:942fd77d5e19 | 75 | } |
altb2 | 7:942fd77d5e19 | 76 | |
altb2 | 7:942fd77d5e19 | 77 | bool Mirror_Kinematic::X2P(float *X, float *P) |
altb2 | 7:942fd77d5e19 | 78 | { |
altb2 | 7:942fd77d5e19 | 79 | float J12 = 0.0090517133f; |
altb2 | 7:942fd77d5e19 | 80 | float J21 = 0.0052923231f; |
altb2 | 7:942fd77d5e19 | 81 | float Xn[2]; |
altb2 | 7:942fd77d5e19 | 82 | float dx = 1e4; |
altb2 | 7:942fd77d5e19 | 83 | float dy = 1e4; |
altb2 | 7:942fd77d5e19 | 84 | P[0] = old_phi[0]; |
altb2 | 7:942fd77d5e19 | 85 | P[1] = old_phi[1]; |
altb2 | 7:942fd77d5e19 | 86 | uint8_t k = 0; |
altb2 | 7:942fd77d5e19 | 87 | do |
altb2 | 7:942fd77d5e19 | 88 | { |
altb2 | 7:942fd77d5e19 | 89 | if( !P2X(P,Xn)) |
altb2 | 7:942fd77d5e19 | 90 | return false; |
altb2 | 7:942fd77d5e19 | 91 | dx = Xn[0]-X[0]; |
altb2 | 7:942fd77d5e19 | 92 | dy = Xn[1]-X[1]; |
altb2 | 7:942fd77d5e19 | 93 | P[0] -= J12 * dy; |
altb2 | 7:942fd77d5e19 | 94 | P[1] -= J21 * dx; |
altb2 | 7:942fd77d5e19 | 95 | } |
altb2 | 7:942fd77d5e19 | 96 | while((dx*dx+dy*dy) > 1e-3 && ++k<20); |
altb2 | 7:942fd77d5e19 | 97 | data.num_it = k; |
altb2 | 7:942fd77d5e19 | 98 | old_phi[0] = P[0]; |
altb2 | 7:942fd77d5e19 | 99 | old_phi[1] = P[1]; |
altb2 | 7:942fd77d5e19 | 100 | return true; |
altb2 | 7:942fd77d5e19 | 101 | } |