I am working on a project and essentially, I have this device made, which has 3 servos connected to a surface via a 2 bar linkage. Given a surface normal, I am trying to position the surface appropriately, by adjusting the vertical position on the 3 points while keeping the midpoint of a plane at a given height.
The length of the linkage attached the motor is 58mm. the positions of each point attached the plane (end of the second linkage), are (-101.035, -87.5, z1), (0, 87.5, z2), (101,035, -87,5, z3) and the midpoint at (0, 0, center_z). The coordinate system looks like this
I assume that the x and y position of each 4 points remain the same, since the changes are negligible in the operating range. In order to calculate the z positions of each outer 3 points, I specify a center_z (the height of the midpoint of the plane). Then I calculate the z positions as follows (in python):
A,B,C = self.normal
point1 = self.plane[1]
point2 = self.plane[2]
point3 = self.plane[3]
D = C * self.center_z
self.z1 = round((1 / C) * (D - A * point1[0] - B * point1[1]), 2)
self.z2 = round((1 / C) * (D - A * point2[0] - B * point2[1]), 2)
self.z3 = round((1 / C) * (D - A * point3[0] - B * point3[1]), 2)
Essentially I have the plane:
++=
where (r, s, t) is the normal vector (A,B,C) in the code chunk. To find k, I calculate:
= 1+1+1 = (0)+(0)+ 1 = *1
where (1, 1, 1) is the known middle point (0, 0, center_z). Finally, I can find the z position of each other point by using:
= (1/)(1+1+1−−) = (1/)(k−−)
I'm fairly sure this yields correct values. Next, I find the angle of the servo required to achieve each height. Which is essentially:
angle = math.asin(z / self.arm_length_1)
Now the results seem almost perfect, but after testing, I believe there is an error in a calculation somewhere. The problem is that the center point is not constant along all surface angles;
Tilting directly along the y axis (ref), results in a perfectly steady center_z position. Tilting forwards along x axis (top point moving down), the center point rises and tilting backwards along the x axis results in the center point falling. A difference of over 10 mm, basically 5+mm change in each direction. If this is confusing, I drew this to better explain the issue.
At first I assumed it was just lack of precision in the servos, but it seems to be a result of the geometry of the 3 points. If you take the working tilt in the y axis and keep the symmetry tilting in the axis 120º rotated (same geometry), the center point behaves the same. However when you have the single arm on one endpoint in the fail case, it behaves when the machine is oriented all 3 ways.
Is there some calculation error when calculating my servo angles? How can I keep a proper center point on one axis but not the other?
I made a plot viewing the bad tilt axis (viewing from the +x axis), where red dots are recorded heights of my machine in action, and the blue dots are where the heights are supposed to be. For reference, the center of the plane is 25mm above 0 point. This tells me there could be some error in finding the angles, or if there is a precision error, why are the results drastically more visible than when tilting on the other axis?
Any help would be very much appreciated, thanks!