When writing the code for calculating the (X,Y) coordinates of the servo’s it became clear that the servo positions or servo angles can not simply be used. Since all coordinates must use (0, 0) as the reference point (tip of the snake’s head) the angle of the second servo is the sum of the first servo angle and the second servo angle. For the third servo it’s even more complex since this is the sum of three angles.

I remember that I ones needed to study vector based calculated, after some refreshing this is what needs to be used. The first servo is always parallel with the X-axe; it starts at (0, 0) and end at (-CL, 0) where CL stands for ServoLength. The delta angle that the second servo makes relative to a straight line (180 degrees angle) is needed for calculating the total angle of the servo compared to the positive direction of the X-axe. In below image dB is +45 degrees. The angle used for the servo is opposite to the direction of the servo that has a positive rotation CCW, this is needed to simplify the dX and dY calculations.

By adding the delta in angle to the previous servo angle the absolute angle is found. Using this angle dX and dY can be calculated referenced to (0, 0). The absolute (X, Y) can be found by adding these values to the previous servo position.

This process can be repeated for all servos, angles larger than 360 degrees can remain as they are since the sine and cosine of these angles automatically compensate for this.