-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathStructured_Text_Documentation.txt
70 lines (65 loc) · 5.94 KB
/
Structured_Text_Documentation.txt
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
INPUT
//all of the units are in microns or microns/microns
s1_slope Array[0..2] of LReal //constant taken from survey. [0] is for dx/dl [1] is for dy/dl and [2] is for dz/dl
s2_slope Array[0..2] of LRea //ideally [0] and [1] will be very small and dz/dl will be nearly 1.0
s3_slope Array[0..2] of LReal //when looking at the lasers head on, laser 1 is on top, laser 2 is on bottom left, and laser 3 is on bottom right
s1_origin Array[0..2] of LReal //constants determined from survey and program written for it - file name XXXXX.c++
s2_origin Array[0..2] of LReal // [0] is the x coordinate, [1] is the y coordinate, and [2] is the z coordinate
s3_origin Array[0..2] of LReal //when looking at the lasers head on, positive z axis is coming towards you, positive x axis is to the left, positive y axis is directly up
axis_vector Array[0..2] of LReal //constant taken from survey. [0] is for dx/dl [1] is for dy/dl and [2] is for dz/dl
axis_point Array[0..2] of LReal //needed so that we know where the vector is in relation to the coordinate system. [0] is the x coordinate, [1] is the y coordinate, and [2] is the z coordinate
rod+plate_length LReal //The distance from the front of the plate to the center of the sphere. units are in microns
plate_thickness LReal //Full thickness of the plate
s1_reading DWord //laser reading coming in from laser 1, will be between -45,000 and +10,000
s2_reading DWord //laser reading from laser 2
s3_reading DWord //laser reading from laser 3
//for all coordinates, [0] is x, [1] is y, [2] is z unless otherwise stated
OUTPUT/CALCULATED VALUES
s1_point Array[0..2] of LReal //calculated coordinate of laser 1 intersecting with the plate
s2_point Array[0..2] of LReal //calculated coordinate of laser 2 intersecting with the plate
s3_point Array[0..2] of LReal //calculated coordinate of laser 3 intersecting with the plate
coldmass Array[0..2] of Real //final position of the cold mass
vector_a Array[0..2] of LReal //in order to find normal vector of the plane, we must find the cross product of 2 vectors
vector_b Array[0..2] of LReal //these vectors are created using the points of intersection with the plane
vector_norm Array[0..2] of LReal //the normal vector of the plate
unitlength LReal //vector_norm converted to unit form
plane_eqn Array[0..3] of LReal //calculated plane equation using standard from a(x-x1) + b(y-y1) + c(z-z1) + d = 0
plate_center Array[0..2] of LReal //calculated center point of the plate on the face of the plate
plate_temp LReal //a temporary value needed for the calculation of the point of intersection between the plane and the axis of the port tube
TEMP
num Int //used as a counter for 'for loops'
#num := 0; //initialize counter
FOR #num := 0 TO 2 DO
#s1_point[#num] := #s1_origin[#num] + #s1_slope[#num] * (65000 - #s1_reading); //determining location of each point on the plate
#s2_point[#num] := #s2_origin[#num] + #s2_slope[#num] * (65000 - #s2_reading); //calculates the actual length from sensor head to a point, multiplys by that laser's unit vector
#s3_point[#num] := #s3_origin[#num] + #s3_slope[#num] * (65000 - #s3_reading); //and then adds that to the origin of the laser head to determine point location
END_FOR;
#num := 0;
FOR #num := 0 TO 2 DO
#vector_a[#num] := #s2_point[#num] - #s1_point[#num]; //in order to find the normal vector of the plate, we must cross two vectors in the plane of the plate
#vector_b[#num] := #s3_point[#num] - #s1_point[#num]; //these vectors are made using points one and two for vector a, and points one and three for vector b
END_FOR;
#vector_norm[0] := (#vector_b[1] * #vector_a[2]) - (#vector_a[1] * #vector_b[2]); //determining the vector normal to the plate using cross product
#vector_norm[1] := (#vector_b[2] * #vector_a[0]) - (#vector_a[2] * #vector_b[0]);
#vector_norm[2] := (#vector_b[0] * #vector_a[1]) - (#vector_a[0] * #vector_b[1]);
#unitlength := SQRT(SQR(#vector_norm[0]) + SQR(#vector_norm[1]) + SQR(#vector_norm[2])); //used to determine unit length of normal vector
#num := 0;
FOR #num := 0 TO 2 DO
#vector_norm[#num] := #vector_norm[#num] / #unitlength; //changing the normal vector to be a unit normal vector
END_FOR;
#num := 0;
FOR #num := 0 TO 2 DO
#plane_eqn[#num] := #s1_point[#num] + #vector_norm[#num]*(#plate_thickness/2); //this is determining the x1,y1,and z1 to be used in the plane equation, which are just a coordinate set that lies on the plane you want to find the equation of
//the plane that will be solved for is the plane that bisects the plate, since the center point of the plate is what stays on the port tube axis
//in order to find a point on this plane, the product of the normal unit vector and half the thickness of the plate are added to point one
END_FOR;
#plane_eqn[3] := (#vector_norm[0] * #s1_point[0]) + (#vector_norm[1] * #s1_point[1]) + (#vector_norm[2] * #s1_point[2]); //finding the value of d in the plane eqn
#plate_temp := ((#vector_norm[0] * (#axis_point[0] - #s1_point[0])) + (#vector_norm[1] * (#axis_point[1] - #s1_point[1])) + (#vector_norm[2] * (#axis_point[2] - #s1_point[2]))) / (-1 * (#vector_norm[0] * #axis_vector[0] + #vector_norm[1] * #axis_vector[1] + #vector_norm[2] * #axis_vector[2]));
#num := 0; //yes the line above looks like a total mess, but I promise it works. this is a temporary value thats needed in order to calculate the point of intersection between a vector and a plane
FOR #num := 0 TO 2 DO
#plate_center[#num] := #axis_point[#num] + #axis_vector[#num] * #plate_temp; //calculates the coordinate for the center of the plate
END_FOR;
#num := 0;
FOR #num := 0 TO 2 DO
#coldmass[#num] := #plate_center[#num] + #vector_norm[#num] * (#center_ball_to_front_plate_length-(#plate_thickness/2)); //determines cold mass position by taking the product of the normal unit vector and the length of rod and plate (less half the plate) and adding it to the point at the center of the plate
END_FOR;