Project Scribbles - Coordinate Maths

So this topic presented as one which is worthy of a page. When we're driving the robot, we want to be working in X/Y coordinates and not in A/B lengths. So the easy answer is to convert between the two.

Assuming we have an origin of the top left, and that as you go across, the X numbers get bigger, and as you go down, the Y numbers get bigger (which puts it the same as a computer graphics library). This makes the calculations significantly easier.

With the diagram here, we can see that the A length can be calculated as a straight right-angled triangle - the length of A is the square root of the sum of the squares of the opposite sides, and as such we can code that in python incredibly easily.

 a_len = math.sqrt(x**2 + y**2)

To calculate the B length, however, we don't have all the figures. Instead what we do is pre-measure the width of the machine - and this figure needs to stay fixed for a drawing, but could be changed between drawings as long as the lengths are recalculated. With the total width known, the X' length is simply the total minus the X. The Y value remains the same, so we end up with a function that looks like this:

def translate_xy_to_ab(coord):
    x = coord[0]
    y = coord[1]
    a_len = math.sqrt(x**2 + y**2)
    b_len = math.sqrt((MAX_WIDTH-x)**2 + y**2)
    
    return [a_len, b_len]

To do the inverse, however, requires an extra step. We have all three sides - total width, A and B lengths, which means we can use the cosine rule to calculate one of the angles. Once we have one of the angles, we can again reproduce the triangle and then using sin and cos functions, calculate the original X and Y values.

def translate_ab_to_xy(lengths):
    a = lengths[0]
    b = lengths[1]
    
    # Cosine rule!
    #cos(left) =  (a**2 + MAX_WIDTH**2 - b**2) / (2 * a * MAX_WIDTH)
    
    try:
        left_angle = math.acos((a**2 + MAX_WIDTH**2 - b**2) / (2 * a * MAX_WIDTH))
    except Exception as e:
        # This specifically happens if the values just arn't a triangle!
        # i.e. consider maxwidth = 100, left length = 10, right = 10... one of
        # the wires must have broken!
        print("Not a triangle!")
        print((a**2 + MAX_WIDTH**2 - b**2) / (2 * a * MAX_WIDTH))
        raise e
        
    #print(left_angle) # in radians, remember.
    
    # sin(left) = opp / hyp
    # cos(right) = adj / hyp
    # hyp is 'a'
    # Lack of precision here - chop to mm. Rounding 'down'
    y = int(math.sin(left_angle) * a) 
    x = int(math.cos(left_angle) * a)
    
    return [x,y]

Note this can throw an exception due to passing lengths which can not be a triangle. Consider if the width was 1m, and A was 10cm and B was 10cm - the two are too short to meet - the wire must have broken.

Whilst this function in python is again easy to implement, I found that I never used the function outside of my original debugging to make sure that I could go forwards and backwards from a particular coordinate for sanity checking.

The length of A and B however is not how my machine works - the robot wants to know how far to move *away* from the rest position (which for me is a length of 800mm). So this is always subtracted from the original length to give a 'target movement'.

Finally, these are all distances in mm (or inches, or whatever you like) but the Arduino and stepper motors don't work in mm. They work in steps. The calculation of steps to mm is done by a scaling factor. The exact factor depends on

• the step angle (for my motors, 1.8 degrees),
• the configuration of the driver chip (jumpers on the ramps board for micro-stepping),
• the radius of the gear (which will vary),
• the number of teeth on the gear (or tooth spacing).

This could be worked out, but the easier way is to draw a line of a fixed number of steps (e.g. 10000), then measure the distance moved (for me around 12.5mm) and then apply this as a scaling factor.

This is then used in the commands sent to the Arduino to say 'move to this number of steps'.