wobble

Unnamed repository; edit this file 'description' to name the repository.
Log | Files | Refs | README

wobble.py (4945B)

      1 import  BLE_GATT
      2 from gi.repository import GLib
      3 
      4 import pdb
      5 import struct
      6 
      7 import sys
      8 import time
      9 from datetime import datetime, timedelta
     10 import os
     11 
     12 import pygame
     13 import pygame.freetype
     14 from pygame.locals import *
     15 
     16 class CadenceListener:
     17     prevCtime = 0
     18     prevWtime = 0
     19     prevWrevs = 0
     20     prevCrevs = 0
     21     wheelSpeed = 0
     22     crankSpeed = 0
     23     crankSpeedMax = 120
     24     elapsedTime = None
     25     firstMessage = True
     26 
     27     def getWheelSpeed(self):
     28         return self.wheelSpeed
     29 
     30     def process(self, value):
     31         flags = value[0]
     32         offset = 1
     33         if flags & 1:
     34             wheelRevolutions,wheelTime = struct.unpack('<IH', bytes(value)[offset:offset+6])
     35             offset = offset + 6
     36         if flags & 2:
     37             crankRevolutions,crankTime = struct.unpack('<HH', bytes(value)[offset:offset+4])        
     38 
     39         if(self.firstMessage):
     40             self.prevCtime = crankTime
     41             self.prevWtime = wheelTime
     42             self.prevCrevs = crankRevolutions
     43             self.prevWrevs = wheelRevolutions
     44             self.firstMessage = False
     45             return
     46         
     47         # handle wraparound
     48         if(crankTime < self.prevCtime):
     49             self.prevCtime = self.prevCtime - 65536
     50         if(wheelTime < self.prevWtime):
     51             self.prevWtime = self.prevWtime - 65536
     52         if(crankRevolutions < self.prevCrevs):
     53             self.prevCrevs = self.prevCrevs - 65536
     54         if(wheelRevolutions < self.prevWrevs):
     55             self.prevWrevs = self.prevWrevs - 65536 * 65536
     56 
     57         if(crankTime > self.prevCtime):
     58             self.crankSpeed = 60 * 1024 * ((crankRevolutions - self.prevCrevs) /
     59                                            float(crankTime - self.prevCtime))
     60             print("crank", self.crankSpeed)
     61             self.prevCtime = crankTime
     62             self.prevCrevs = crankRevolutions
     63 
     64         if(wheelTime > self.prevWtime):
     65             millis = float(wheelTime - self.prevWtime)
     66             self.rolling = True
     67             revs = (wheelRevolutions - self.prevWrevs) / millis
     68 
     69             self.wheelSpeed = revs * 2205 * 3600 / 1024.0
     70             print("wheel", millis, self.wheelSpeed)
     71             self.prevWtime = wheelTime
     72             self.prevWrevs = wheelRevolutions
     73             self.elapsedTime = (self.elapsedTime or 0) + (millis / 1024.0)
     74 
     75 class Wobble:
     76     listener = CadenceListener()
     77 
     78     def __init__(self):
     79         pygame.init()
     80         self.surface = pygame.display.set_mode((600,300), pygame.RESIZABLE)
     81         self.width = self.surface.get_width();
     82         self.height = self.surface.get_height();
     83         self.font = None
     84 
     85     def on_update(self):
     86         scale = int(min(self.width/7, (self.height-20)/3))
     87         if(self.font == None):
     88             self.font = pygame.freetype.Font(os.environ['FONT'], scale)
     89 
     90         self.surface.fill((10,20,10))
     91         self.font.render_to(self.surface,
     92                             (scale/16,10),
     93                             "{: 5.1f} km/h".format(self.listener.wheelSpeed),
     94                             (0,255,0))
     95         self.font.render_to(self.surface,
     96                             (scale/16,scale + 20),
     97                             "{: 5.1f} rpm".format(self.listener.crankSpeed),
     98                             (0,255,0))
     99         if self.listener.elapsedTime !=  None:
    100             runTime = timedelta(seconds=self.listener.elapsedTime)
    101             timelabel = str(runTime)[0:9]
    102         else:
    103             timelabel = datetime.now().strftime("%H:%M:%S")
    104         self.font.render_to(self.surface,
    105                             (scale, scale * 2 + 30),
    106                             timelabel,
    107                             (0,255,0))
    108         pix = self.width *((self.listener.crankSpeed-40)/ self.listener.crankSpeedMax)
    109         pix = max(pix, 0)
    110         pygame.draw.rect(self.surface, (180,0,0), pygame.Rect(0,self.height/2-60, pix, 50))
    111         pygame.draw.rect(self.surface, (0,0,180), pygame.Rect(pix,self.height/2-60, 20, 50))
    112         pygame.display.flip()
    113 
    114 
    115 bike = BLE_GATT.Central(sys.argv[1])
    116 bike.connect()
    117 app = Wobble()
    118 
    119 csc_measurement = '00002a5b-0000-1000-8000-00805F9B34FB' 
    120 bike.on_value_change(csc_measurement, app.listener.process)
    121 
    122 running = True
    123 
    124 def handle_pygame_event(event):
    125     if event.type == pygame.QUIT:
    126         running = False
    127     if event.type == pygame.KEYUP and event.key == pygame.K_q:
    128         running = False
    129     if event.type == pygame.KEYUP and event.key == pygame.K_SPACE:
    130         if app.listener.startTime == None:
    131             app.listener.startTime = datetime.now()
    132         else:
    133             app.listener.startTime = None
    134     if event.type == pygame.VIDEORESIZE:
    135         app.width = event.w
    136         app.height = event.h
    137         app.font = None
    138 
    139 def on_timer():
    140     handle_pygame_event(pygame.event.poll())
    141     app.on_update()
    142     return True
    143     
    144 
    145 GLib.timeout_add(100, on_timer)
    146     
    147 try:
    148     # Time to go live
    149     print("Listening for events...")
    150     bike.mainloop.run()
    151 
    152 finally:
    153     pygame.quit()
    154