wobble/csc.py

import  BLE_GATT
from gi.repository import GLib
import pdb
import struct

import sys
import time
from datetime import datetime
import os

import pygame
import pygame.freetype
from pygame.locals import *

class CadenceListener:
    csc_feature = '00002a5c-0000-1000-8000-00805F9B34FB' 

    prevCtime = 0
    prevWtime = 0
    prevWrevs = 0
    prevCrevs = 0
    wheelSpeed = 0
    crankSpeed = 0
    crankSpeedMax = 120
    startTime = None

    def getWheelSpeed(self):
        return self.wheelSpeed

    def process(self, value):
        flags = value[0]
        offset = 1
        if flags & 1:
            wheelRevolutions,wheelTime = struct.unpack('<IH', bytes(value)[offset:offset+6])
            offset = offset + 6
        if flags & 2:
            crankRevolutions,crankTime = struct.unpack('<HH', bytes(value)[offset:offset+4])        
        # print(wheelrevs, wheeltime, crankrevs, cranktime)
        if(not self.startTime):
            self.prevCtime = crankTime
            self.prevWtime = wheelTime
            self.prevCrevs = crankRevolutions
            self.prevWrevs = wheelRevolutions
            self.startTime = datetime.now()
            return
        
        # handle wraparound
        if(crankTime < self.prevCtime):
            self.prevCtime = self.prevCtime - 65536
        if(wheelTime < self.prevWtime):
            self.prevWtime = self.prevWtime - 65536
        if(crankRevolutions < self.prevCrevs):
            self.prevCrevs = self.prevCrevs - 65536
        if(wheelRevolutions < self.prevWrevs):
            self.prevWrevs = self.prevWrevs - 65536 * 65536

        if(crankTime > self.prevCtime):
            self.crankSpeed = 60 * 1024 * ((crankRevolutions - self.prevCrevs) /
                                           float(crankTime - self.prevCtime))
            print("crank", self.crankSpeed)
            self.prevCtime = crankTime
            self.prevCrevs = crankRevolutions

        if(wheelTime > self.prevWtime):
            revs = ((wheelRevolutions - self.prevWrevs) /
                    float(wheelTime - self.prevWtime))
            self.wheelSpeed = revs * 2205 * 3600 / 1024.0
            print("wheel",
                  (float(wheelTime - self.prevWtime)),
                  self.wheelSpeed)
            self.prevWtime = wheelTime
            self.prevWrevs = wheelRevolutions

class Biscuit:
    listener = CadenceListener()

    def __init__(self):
        pygame.init()
        self.surface = pygame.display.set_mode((600,300), pygame.RESIZABLE)
        self.width = self.surface.get_width();
        self.height = self.surface.get_height();
        self.font = None

    def on_update(self):
        scale = int(min(self.width/7, (self.height-20)/3))
        if(self.font == None):
            self.font = pygame.freetype.Font(os.environ['FONT'], scale)

        self.surface.fill((10,20,10))
        self.font.render_to(self.surface,
                            (scale/16,10),
                            "{: 5.1f} km/h".format(self.listener.wheelSpeed),
                            (0,255,0))
        self.font.render_to(self.surface,
                            (scale/16,scale + 20),
                            "{: 5.1f} rpm".format(self.listener.crankSpeed),
                            (0,255,0))
        if self.listener.startTime !=  None:
            runTime = datetime.now() - self.listener.startTime
            timelabel = str(runTime)[0:9]
        else:
            timelabel = datetime.now().strftime("%H:%M:%S")
        self.font.render_to(self.surface,
                            (scale, scale * 2 + 30),
                            timelabel,
                            (0,255,0))
        pix = self.width *((self.listener.crankSpeed-40)/ self.listener.crankSpeedMax)
        pix = max(pix, 0)
        pygame.draw.rect(self.surface, (180,0,0), pygame.Rect(0,self.height/2-60, pix, 50))
        pygame.draw.rect(self.surface, (0,0,180), pygame.Rect(pix,self.height/2-60, 20, 50))
        pygame.display.flip()



bike_address = 'C2:35:5D:F2:F4:0F'
bike = BLE_GATT.Central(bike_address)
bike.connect()
app = Biscuit()

csc_measurement = '00002a5b-0000-1000-8000-00805F9B34FB' 
bike.on_value_change(csc_measurement, app.listener.process)

running = True
try:
    # Time to go live
    context = bike.mainloop.get_context()

    print("Listening for events...")
    while running:
        time.sleep(0.1)
        context.iteration(False)
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
            if event.type == pygame.KEYUP and event.key == pygame.K_q:
                running = False
            if event.type == pygame.KEYUP and event.key == pygame.K_SPACE:
                if app.listener.startTime == None:
                    app.listener.startTime = datetime.now()
                else:
                    app.listener.startTime = None
            if event.type == pygame.VIDEORESIZE:
                app.width = event.w
                app.height = event.h
                app.font = None

        app.on_update()
    
finally:
    pygame.quit()