graph_maker.py

# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt
import pygame
from pygame.locals import *
import serial
import sys
import re
import time

D_FORMAT = re.compile('Roll:.+Pitch:.+Yaw:.+deg')
D_PATTERN = re.compile('Roll:|Pitch:|Yaw:|deg')

class Degree():

    def __init__(self, name):
        self.values = [0] * 100
        self.name = name

    def update(self, value):
        self.values.pop(99)
        self.values.insert(0,value)

    def dup(self):
        value = self.values[0]
        self.values.pop(99)
        self.values.insert(0, value)
        return str(self.values[0])


class DegreeGraphMaker():

    def __init__(self):
        self.serial_connection = serial.Serial('/dev/ttyACM0', 9600)
        self.t = np.arange(0, 100, 1)
        self.pitch = Degree('Pitch')
        self.roll = Degree('Roll')
        self.yaw = Degree('Yaw')

        plt.ion()
        pygame.init()
        self.screen = pygame.display.set_mode((600, 600))
        pygame.display.set_caption('mpu-6050 dmp')
        self.font = pygame.font.Font(None, 26)

    def main(self):

        def render(angle, raw_value):
            label_coordinates = {
                 'Pitch': (10, 580),
                'Roll':  (160, 580),
                'Yaw': (310, 580)
            }
            try:
                angle.update(float(value))
            except:
                angle.dup()
            label = self.font.render( angle.name + ': ' + raw_value , False, (255,255,255))
            self.screen.blit(label, label_coordinates[angle.name])
            
        while True:
            self.screen.fill((0,0,0))
            try:
                raw_read_degrees = self.serial_connection.readline().rstrip().decode('ascii')
            except:
                continue

            if D_FORMAT.match(raw_read_degrees):
                degrees = re.sub(D_PATTERN, '', raw_read_degrees).split(',')
                r, p, y = degrees[0], degrees[1], degrees[2]

                render(self.roll, r)
                render(self.pitch, p)
                render(self.yaw, y)

                pygame.display.flip()

                # x = np.arange(100)
                # plt.plot(x, self.roll.values)
                # plt.plot(x, self.pitch.values)
                # plt.plot(x, self.yaw.values)

                plt.draw()

                for event in pygame.event.get():
                    if event.type == QUIT:
                        pygame.quit()
                        self.serial_connection.close()
                        plt.close()
                        sys.exit()


if __name__ == '__main__':
    dgm = DegreeGraphMaker()
    dgm.main()