MABEL (Multiple Axis Balancer Electronically Levelled)

MABEL is quite different from other balancing robots in that the legs are able to move and the robot does not need to run in a fixed position. This is where the Raspberry Pi and the servo controller come into play. I've written an Inverse Kinematics class in python that takes an (X, Y) coordinate and works backwards to calculate the angle of each individual robot joint required to move precisely to that position. Briefly, since we know the length of each leg section is constant, we can draw a virtual triangle using the X and Y component as the third line in the  triangle and calculate the angles in the triangle using the cosine rule. Here upper refers to the distance between the upper leg pivot and lower leg pivot (92mm), lower refers to lower leg pivot to wheel centre (75mm), X and Y are given by the user (also in mm).

In code this looks like:

# IKSolve.py is an inverse kinematics module for MABEL to move the wheel (effector) in 2D Cartesian coordinates.
# Contributors: (@raspibotics)

# ***USAGE GUIDANCE***
# x translates the robot leg vertically and y translates the leg horizontally, each value should be in integer mm
# x is the distance between the upper leg pivot and wheel centre (Vertically) - acceptable range for MABEL (160-98mm)
# y is the distance between the upper leg pivot and wheel centre (Horizontally) - acceptable range for MABEL (-50, 50mm)

from math import acos, degrees, atan2


class IKSolve:  # IKSolve - Inverse Kinematics solver for MABEL

    def __init__(self, ru_home, rl_home, lu_home,  # IKSolve Constructor, Values are default leg section lengths in mm
                 ll_home, upper_leg=92, lower_leg=75):

        self.ru_home, self.rl_home = ru_home, rl_home  # Right leg servo home positions
        self.lu_home, self.ll_home = lu_home, ll_home  # Left leg servo home positions

        self.a_const_0 = (upper_leg ** 2) - (lower_leg ** 2)  # b^2 - a^2
        self.a_const_1 = 2 * upper_leg  # 2*b (c is unknown)

        self.b_const_0 = (upper_leg ** 2) + (lower_leg ** 2)  # b^2 + c^2
        self.b_const_1 = 2 * upper_leg * lower_leg  # 2bc

    def translate_xy(self, x, y, flip=False):  # translate_xy(x, y) Calculates the required angles to move to (x, y) mm
        if x == 0 and y == 0:  # (0, 0) resets servos to home position
            return self.ru_home, self.rl_home, self.lu_home, self.ll_home
        else:
            try:
                angle_a = degrees(acos(((self.a_const_0 + (x ** 2)) /  # A = Cos^-1((b^2+c^2-a^2)/2bc)
                                        (self.a_const_1 * x))))
                angle_a += degrees(atan2(y, x))  # Tan^-1(y/x)
                angle_b = 180 - degrees(acos((self.b_const_0 - (x ** 2 + y ** 2)) /  # A = Cos^-1((b^2+c^2-a^2)/2bc)
                                             self.b_const_1))
            except ValueError:
                print('Value specified is outside range of capable movement. Please specify a different value...')
                return
        if flip is not False:
            return (self.ru_home - round(angle_a)), (self.rl_home + round(angle_b)), (
                    self.lu_home + round(angle_a)), (self.ll_home - round(angle_b))
        else:
            return (self.ru_home + round(angle_a)), (self.rl_home - round(angle_b)), (
                    self.lu_home - round(angle_a)), (self.ll_home + round(angle_b))


IKSolve = IKSolve()

print(IKSolve.translate_xy(...