projects python

Gyroscopic 3D wireframe cube

Using a 3-axis gyro for live 3D perspective

This little project combines the previous accelerometer-gyroscope code with the 3D rotating OLED cube to produce a 3D cube which responds to gyro input, making it possible to "peek around" the cube with simulated perspective, or make it spin with a flick of the wrist.

Take a look at those earlier articles if you're interested in the background basics.

Requirements
Wemos D1 v2.2+ or good imitations. amazon
3-axis Gyroscope Based on MPU6050 chip amazon
0.96in OLED Screen 128x64 pixels, I2c interface. amazon
Breadboard Any size will do. amazon
Wires Loose ends, or jumper leads.

Libraries

We need two Python drivers for this project — one for the 128x64 OLED display, and one for the gyroscope.

The display in this example uses the ssd1306 chip, so we can use the module available in the MicroPython repository.

The gyroscope is a MPU6050, a Python library for which is available from @adamjezek98 here on Github.

Download both files and upload them to your controller using ampy or the web REPL.

Once the libraries are in place, connect to your controller and try and import both packages. If the imports work, you should be good to go.

python
import ssd1306
import mpu6050

Wiring

Both the ssd1306 display and the MPU6050 gyroscope-accelerometer communicte via I2C. Helpfully they're also on different channels, so we don't need to do any funny stuff to talk to them both at the same time.

The wiring is therefore quite simple, hooking them both up to +5V/GND and connecting their SCL and SDA pins to D1 and D2 respectively.

I2C OLED display and Gyro wired to Wemos D1

On the boards I have the the SDA, SCL, GND and 5V pins are in reverse order when the boards are placed pins-top. Double check what you're wiring where.

Tiny games for your BBC micro:bit.

To support developers in [[ countryRegion ]] I give a [[ localizedDiscount[couponCode] ]]% discount on all books and courses.

[[ activeDiscount.description ]] I'm giving a [[ activeDiscount.discount ]]% discount on all books and courses.

Code

The project is made up of 3 parts — 

  1. the gyroscope code to calibrate, retrieve and smooth the data
  2. the 3D point code to handle the positions of cube in space
  3. the simulation code to handle the inputs, and apply them to the 3D scene, outputting the result

First, the basic imports for I2C and the two libraries used for the display and gyro.

python
from machine import I2C, Pin
import ssd1306
import mpu6050
import math

i2c = I2C(scl=Pin(5), sda=Pin(4))
display = ssd1306.SSD1306_I2C(128, 64, i2c)
accel = mpu6050.accel(i2c)

Gyroscope

The gyroscope values can be a little noisy, and because of manufacturing variation (and gravity) need calibrating at rest before use.

Some standard smoothing and calibration code is shown below — to see a more thorough explanation of this see the introduction to 3-axis gyro-accelerometers in MicroPython.

First the smoothed sampling code which takes a number of samples and returns the mean average. It accepts a calibration input which provides a base value to remove from the resulting measurement.

python
def get_accel(n_samples=10, calibration=None):
    # Setup a dict of measure at 0
    result = {}
    for _ in range(n_samples):
        v = accel.get_values()

        for m in v.keys():
            # Add on value / n_samples (to generate an average)
            result[m] = result.get(m, 0) + v[m] / n_samples

    if calibration:
        # Remove calibration adjustment
        for m in calibration.keys():
            result[m] -= calibration[m]

    return result

The calibration code takes a number of samples, waiting for the variation to drop below threshold. It then returns this base offset for use in future calls to get_accel.

python
def calibrate(threshold=50):
    print('Calibrating...', end='')
    while True:
        v1 = get_accel(100)
        v2 = get_accel(100)
        if all(abs(v1[m] - v2[m]) < threshold for m in v1.keys()):
            print('Done.')
            return v1

Point3D objects

The simplest way to model objects in 3D space is to store and manipulate their vertices only — for a cube, that means the 8 corners.

To rotate the cube we manipulate these points in 3 dimensional space. To draw the cube, we project these points onto a 2-dimensional plane, to give a set of x,y coordinates, and connect the vertices with our edge lines.

The code here is based on this example for Pygame. The initial conversion of that code to MicroPython with an OLED screen and some background on the theory can be found here.

python
class Point3D:
    def __init__(self, x = 0, y = 0, z = 0):
        self.x, self.y, self.z = x, y, z

    def rotateX(self, deg):
        """ Rotates this point around the X axis the given number of degrees. """
        rad = deg * math.pi / 180
        cosa = math.cos(rad)
        sina = math.sin(rad)
        y = self.y * cosa - self.z * sina
        z = self.y * sina + self.z * cosa
        return Point3D(self.x, y, z)

    def rotateY(self, deg):
        """ Rotates this point around the Y axis the given number of degrees. """
        rad = deg * math.pi / 180
        cosa = math.cos(rad)
        sina = math.sin(rad)
        z = self.z * cosa - self.x * sina
        x = self.z * sina + self.x * cosa
        return Point3D(x, self.y, z)

    def rotateZ(self, deg):
        """ Rotates this point around the Z axis the given number of degrees. """
        rad = deg * math.pi / 180
        cosa = math.cos(rad)
        sina = math.sin(rad)
        x = self.x * cosa - self.y * sina
        y = self.x * sina + self.y * cosa
        return Point3D(x, y, self.z)

    def project(self, win_width, win_height, fov, viewer_distance):
        """ Transforms this 3D point to 2D using a perspective projection. """
        factor = fov / (viewer_distance + self.z)
        x = self.x * factor + win_width / 2
        y = -self.y * factor + win_height / 2
        return Point3D(x, y, self.z)

Gyro-locked Perspective Simulation

The first demo uses the accelerometer to produce a simulated perspective view of the cube. Tilting the board allows us to see "around" the edges of the cube, as if we were looking into the scene through a window.

To detect the angle of the device we're using the accelerometer. You might think to use the gyroscope first — I did — but remember the gyroscope detects angular velocity, not angle. Measurements are zero at rest, in any orientation. You can track the velocity changes and calculate the angle from this yourself, but gradually the error will build up and the cube will end up pointing the wrong way.

Using the accelerometer we have a defined rest point (flat on the surface) from which to calculate the current rotation. Placing the device flat will always return to the initial state.

python
class Simulation:
    def __init__(self, width=128, height=64, fov=64, distance=4):

        self.vertices = [
            Point3D(-1,1,-1),
            Point3D(1,1,-1),
            Point3D(1,-1,-1),
            Point3D(-1,-1,-1),
            Point3D(-1,1,1),
            Point3D(1,1,1),
            Point3D(1,-1,1),
            Point3D(-1,-1,1)
        ]

        # Define the edges, the numbers are indices to the vertices above.
        self.edges  = [
            # Back
            (0, 1), (1, 2), (2, 3), (3, 0),
            # Front
            (5, 4), (4, 7), (7, 6), (6, 5),
            # Front-to-back
            (0, 4), (1, 5), (2, 6), (3, 7),
        ]

        # Dimensions
        self.projection = [width, height, fov, distance]

    def run(self):
        # Starting angle (unrotated in any dimension)
        angleX, angleY, angleZ = 0, 0, 0

        calibration = calibrate()

        while 1:

            data = get_accel(10, calibration)

            angleX = data['AcX'] / 256
            angleY = data['AcY'] / 256

            t = []
            for v in self.vertices:
                # Rotate the point around X axis, then around Y axis, and finally around Z axis.
                r = v.rotateX(angleX).rotateY(angleY).rotateZ(angleZ)

                # Transform the point from 3D to 2D
                p = r.project(*self.projection)

                # Put the point in the list of transformed vertices
                t.append(p)

            display.fill(0)

            for e in self.edges:
                display.line(*to_int(t[e[0]].x, t[e[0]].y, t[e[1]].x, t[e[1]].y, 1))

            display.show()

We use a simple helper function to convert lists of float into lists of int to make updating the OLED display simpler.

python
def to_int(*args):
    return [int(v) for v in args]

We can create a Simulation and run it with the following.

python
s = Simulation()
s.run()

Leave it on a flat surface as you start it up, so the calibration can complete quickly.

Once running it should look something like the following. If you pick up the device and tilt it you should notice the perspective of the cube change, as if you were 'looking around' the side of a real 3D cube.

{% youtube uesh3CcE0RA %}

Making it Spin

So far we've only used the accelerometer, and the cube has remained locked in a single position. This second demo uses the gyroscope to detect angular velocity allowing you to make the cube spin by flicking the device in one direction or another.

We do this by reading the velocity and adding it along a given axis. By reducing the velocity gradually over time, we can add a sense of friction to the rotation. The result is a cube that you can flick to rotate, that will gradually come to a rest.

The idea is to mimick the effect of a cube (e.g. a dice) floating inside a ball of liquid. Rotating it quickly adds momentum, which is gradually reduced by friction.

The simulation code is given below.

python
class Simulation:
    def __init__(
        self,
        width=128,
        height=64,
        fov=64,
        distance=4,
        inertia=10,
        acceleration=25,
        friction=1
        ):

        self.vertices = [
            Point3D(-1,1,-1),
            Point3D(1,1,-1),
            Point3D(1,-1,-1),
            Point3D(-1,-1,-1),
            Point3D(-1,1,1),
            Point3D(1,1,1),
            Point3D(1,-1,1),
            Point3D(-1,-1,1)
        ]

        # Define the edges, the numbers are indices to the vertices above.
        self.edges  = [
            # Back
            (0, 1), (1, 2), (2, 3), (3, 0),
            # Front
            (5, 4), (4, 7), (7, 6), (6, 5),
            # Front-to-back
            (0, 4), (1, 5), (2, 6), (3, 7),
        ]

        # Dimensions
        self.projection = [width, height, fov, distance]

        # Configuration
        self.friction = friction
        self.acceleration = acceleration
        self.inertia = inertia

    def run(self):
        velocityX, velocityY, velocityZ = 0, 0, 0
        calibration = calibrate()

        while 1:
            t = []

            # Get current rotational velocity from sensor.
            data = get_accel(10, calibration)
            gyroX = -data['GyY'] / 1024
            gyroY = data['GyX'] / 1024
            gyroZ = -data['GyZ'] / 1024

            # Apply velocity, with slide for friction.
            if abs(gyroX) > self.inertia:
                velocityX = slide_to_value(velocityX, gyroX, self.acceleration)

            if abs(gyroY) > self.inertia:
                velocityY = slide_to_value(velocityY, gyroY, self.acceleration)

            if abs(gyroZ) > self.inertia:
                velocityZ = slide_to_value(velocityZ, gyroZ, self.acceleration)

            rotated = []
            for v in self.vertices:
                r = v.rotateX(velocityX).rotateY(velocityY).rotateZ(velocityZ)
                p = r.project(*self.projection)
                t.append(p)
                rotated.append(r)

            self.vertices = rotated
            display.fill(0)

            for e in self.edges:
                display.line(*to_int(t[e[0]].x, t[e[0]].y, t[e[1]].x, t[e[1]].y, 1))

            display.show()

            velocityX = slide_to_value(velocityX, 0, self.friction)
            velocityY = slide_to_value(velocityY, 0, self.friction)
            velocityZ = slide_to_value(velocityZ, 0, self.friction)

We need another helper function which handles the gradual "slide" of a given value towards it's target. This is used to both smooth acceleration and to gradually bleed off velocity via friction. The maximum value of change is specified by slide.

python
def slide_to_value(value, target, slide):
    """
    Move value towards target, with a maximum increase of slide.
    """
    difference = target-value
    if not difference:
        return value
    sign = abs(difference) / difference  # -1 if negative, 1 if positive
    return target if abs(difference) < slide else value + slide * sign

The simulation works as follows —

  1. Read the rotational velocity from the gyroscope for each axis (X and Z axes are reversed because of the orientation of the sensor).
  2. If the measured velocity in a given axis is higher than inertia we add move the current velocity towards the measured value, in steps of acceleration max.
  3. The current velocities are used to update the vertices rotating them in 3D space, and storing the resulting updated positions. This is neccessary to ensure that the orientation of the axes for the view remain aligned with the frame of the gyroscope.
  4. The display is drawn as before.
  5. Finally we move all values towards zero by sliding towards zero, in steps of friction.

The end result is a 3D cube which responds to user input through the gyroscope, rotating along the appropriate axis. The inertia means small movements are ignored, so you can flick it in a given direction and then return it slowly to the original place and it will continue to spin.

{% youtube JJJxuqNs_f8 %}

You can experiment with the inertia, acceleration and friction values to see what effect they have. There is no real physics at work here, so you can create some quite weird behaviours.