1 #include <math.h>
2 #include <stdio.h>
3 #include <stdlib.h>
4
5 float anglewrap(float d)
6 {
7 if (d < 0) d += 360.0;
8 if (d >= 360.0) d -= 360.0;
9 return d;
10 }
11
12 float r2d(float r)
13 {
14 float d = r*180.0/M_PI;
15 return anglewrap(d);
16 }
17
18 float iterate(float laser_off, float mX, float mY, float max)
19 {
20 float theta = 0;
21 float last_theta = 0;
22
23 do {
24 float hx = laser_off * cos(theta + M_PI_2);
25 float hy = laser_off * sin(theta + M_PI_2);
26 last_theta = theta;
27 theta = atan2((max/2.0)-mY-hy, (max/2.0)-mX-hx);
28 //printf("#theta = %f, x,hx=%f,%f, y,hy=%f,%f\n", r2d(theta), (max/2.0)-mX, hx, (max/2.0)-mY, hy);
29 } while (fabs(theta - last_theta) > 0.00001);
30
31 return r2d(theta);
32 }
33
34
35 float f2m(float f)
36 {
37 return 0.3048*f;
38 }
39
40 void convert(float feet, float *mx, float *my)
41 {
42 if (feet<=24) {
43 *mx = f2m(24);
44 *my = f2m(feet);
45 }
46 else if (feet<=49) {
47 *mx = f2m(24.0-(feet-25.0));
48 *my = f2m(24);
49 }
50 else if (feet<=74) {
51 *my = f2m(24.0-(feet-50.0));
52 *mx = 0;
53 }
54 else {
55 *mx = f2m(feet-75.0);
56 *my = 0;
57 }
58 }
59
60
61 int main(int argc, char **argv)
62 {
63 while (1) {
64 int sender;
65 int id;
66 float x,y,z,temp,rh;
67 char note[20];
68 note[0] = 0;
69 int status = scanf("%d %u %f %f %f %f %f %s",
70 &(sender),
71 &(id),
72 &(x),
73 &(y),
74 &(z),
75 &(temp),
76 &(rh),
77 note);
78
79 if (status != 8) return 0;
80
81 /*
82 * assumes this setup:
83 *
84
85 Emitter is E
86 Receiver is R
87 measurements are in feet from one contiguous tape measure
88 at each corner 1 foot is used to loop over to continue on the
89 next foot.
90
91 99 (mx) 75 (0,0)
92 0 ---------------E 74
93 | |
94 | |
95 | R | (my)
96 | |
97 | |
98 24 |_______________| 50
99 25 49
100
101 ^
102 |
103 ____|
104 | L|
105 0| * L|180 (laser side)
106 |____L|
107 |XXXXX|
108 90
109 (board
110 side)
111
112 */
113
114 #define LASER_OFFSETX 0.095
115 #define LASER_OFFSETZ 0.03
116
117 float mX,mY;
118
119 float angle = 0;
120 if (x) {
121 convert(x, &mX, &mY);
122 angle = anglewrap(270.0 + (45.0 - iterate(LASER_OFFSETX, mX, mY, f2m(24))));
123 }
124 else if (y) {
125 convert(y, &mX, &mY);
126 angle = anglewrap(270.0 + (45.0 - iterate(LASER_OFFSETX, mX, mY, f2m(24))));
127 }
128 else if (z) {
129 convert(z, &mX, &mY);
130 angle = anglewrap((iterate(LASER_OFFSETZ, mX, mY, f2m(24)) - 45.0));
131 if (angle > 90.0 && angle < 270.0) angle = anglewrap(180.0 - angle);
132 }
133
134 printf("%d %u %f %f %f %f %f %s\n",
135 sender, id, angle, 0.0, 0.0, temp, rh, note);
136 }
137 return 0;
138 }
139
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.