Pi Wars 2019 Part 5 – Quick Maths

Once the Chassis was complete, my next task was to initially focus on a stable driving system and then look at a way of using inputs to move the robot.

Initially, I wanted to find a stable and easy to use Python3 library to enable input from a controller. My first port of call was Pygame, having used it for the majority of my GCSE Computer Science coursework. After looking into it further however, I found the best library seemed to be the Approximate Engineering Input Library. This takes the input of various different controllers, putting the values into a class with the universal button mappings. This means that if for some reason i needed to change my controller, i could easily do so with very minimal code changes.

I started off by following their guide on pairing a PS3 DualShock controller, which was very  informative pairing with no hassle at all! A link to the guide can be found here.

Once the Controller was connected, I then set about learning how the library works, which turned out to be very simple. Using the example code on the documentation site, I was able to easily able to gain data from the inputs on the controller. The basis of the code was an infinite loop checking to see whether the controller was connected. Once connected, it defined the Controller which whilst connected returned a class that can be read for values.

from approxeng.input.selectbinder import ControllerResource

while True:
    try:
        with ControllerResource() as joystick:
            print('Found a joystick and connected')
            while joystick.connected:
                # Do stuff with your joystick here!
                # ....
                # ....
        # Joystick disconnected...
        print('Connection to joystick lost')
    except IOError:
        # No joystick found, wait for a bit before trying again
        print('Unable to find any joysticks')
        sleep(1.0)

Once I had got this stably working, I then moved onto the maths of the general movement. There are a few library’s available for this, such as this one, however I really wanted to try and make my own one, being the main feature of my bot. I started off by sketching out the logic. The basic fundamentals I wanted to work along were that the joystick controlled both direction and speed. Using the axis of the joystick, I would be able to get both a angle and percentage of speed. An example of this can be seen below:

 

I started off by trying to translate the X & Y positions of the joystick to an angle. Using trigonometry I can take the X & Y and convert it into a triangle to work out the angle. To make tings easier, I coveted all coordinates into positive values and then added the respective values afterwards to find the exact angle.

In coding terms,

def CalcAngle(x, y):
    if abs(x) < 0.2 and abs(y) < 0.2: # Checks if axis is less than 20% on both XY stop robot. Stops glitching
        motorSpeed(0, 0) #Sets motor speed to 0 on all motors to stop movement
    else:
        #Checking for exact angles
        if x > 0.0 and y == 0.0: #
            angle = 90
        elif x == 0.0 and y < 0.0:
            angle = 180
        elif x < -0.0 and y == 0.0:
            angle = 270
        elif x == 0.0 and y > 0.0:
            angle = 0
        else:
            #Working out angle from coordinates using inverse TAN rule. abs() converts negative values to positive
            angle = math.degrees(math.atan(abs(y) / abs(x)))
              
            #Adding appropriate values to angle depending on the values of coordinates to give 360° direction
            if x > 0.0 and y > 0.0:
                angle = 90 - angle
            elif x > 0.0 and y < 0.0:
                angle = 90 + angle
            elif x < 0.0 and y < 0.0:
                angle = 270 - angle
            elif x < 0.0 and y > 0.0:
                angle = 270 + angle


        return angle #Returns value of angle

Working out the speed is simply done by using Pythagoras, A^2 + B^2 = C^2, in my case, 

Image result for pythagoras theorem

This could then be converted into python using the following line of code:

speed = min(math.sqrt((x * x) + (y * y)), 1)

Sometimes the value supplied was a little over 1, so adding the min() function simply meant that if this was the case, it would supply a value of 100%. This can then be applied with the motor control software to calculate the speed of each motor to get the desired movement.

The movement of the bot can be split into 6 different zones. a pair of wheels can work together in both directions. These are separated by 60° and are shown in the below diagram. (Beautiful I know!)

I then used the angle from above to calculate the speed of each motor, adding in a graduation between the different positions to enable a smooth drive.

def motorSpeed(angle, speed):
    global TB1, TB2
    val = 0.0166666666667 #1/60 to calculate the gradiation
    m1 = 0 # Initially setting motors to 0
    m2 = 0 # Initially setting motors to 0
    m3 = 0 # Initially setting motors to 0
    if angle >= -1 and angle < 60: # Between 0° and 59°
        dif = angle # Working out the multiplier for gradual decrease in speed
        m1 = 1
        m2 = -1 + (dif * val)
        m3 = 0 - (dif * val)
    elif angle >= 60 and angle < 120:# Between 60° and 119°
        m1 = 1
        m2 = 0 
        m3 = -1
    elif angle >= 120 and angle < 180: # Between 120° and 179°
        dif = angle - 120 # Working out the multiplier for gradual decrease in speed
        m1 = 0 - (dif * val)
        m2 = 1
        m3 = -1 + (dif * val)
    elif angle >= 180 and angle < 240: # Between 180° and 239°
        dif = angle - 180 # Working out the multiplier for gradual decrease in speed
        m1 = -1
        m2 = 1 - (dif * val)
        m3 = 0 + (dif * val)
    elif angle >= 240 and angle < 300:# Between 240° and 299°
        m1 = -1
        m2 = 0
        m3 = 1
    elif angle >= 300 and angle <= 360: # Between 300° and 360°
        dif = angle - 300 # Working out the multiplier for gradual decrease in speed
        m1 = 0 + (dif * val)
        m2 = -1
        m3 = 1 - (dif * val)

    m1 = m1 * speed # Multiplying the drive for angle by the speed (value is between 0 and 1)
    m2 = m2 * speed # Multiplying the drive for angle by the speed (value is between 0 and 1)
    m3 = m3 * speed # Multiplying the drive for angle by the speed (value is between 0 and 1)

    TB1.SetMotor1(m1) # Setting speed for motors
    TB1.SetMotor2(m2) # Setting speed for motors
    TB2.SetMotor1(m3) # Setting speed for motors

Once all the above are put together, they can be added to the controller function to get a smooth direct drive on 3 wheels 🙂

while True:
    if stopThread == True:
        break
    try:
        with ControllerResource() as joystick:
            print('Found a joystick and connected')
            while joystick.connected:
                Drive(round(joystick.rx, 2), round(joystick.ry, 2))
        print('Connection to joystick lost')
    except IOError:
        # No joystick found, wait for a bit before trying again
        print('Unable to find any joysticks')
        sleep(1.0)

Changes and modifications

After testing, I have mad 1 modification to the code. Previously the angle was being calculated to the nearest degree, however after testing it caused the robot to become quite jumpy. To resolve this, I have added an extra line of code to round the angle to the nearest 5.

angle = int(5 * round(float(angle) / 5))

Leave a Reply

Your email address will not be published. Required fields are marked *