accelero.py

   1 #!/usr/bin/python
   2 # -*- coding: UTF-8 -*-
   3
   4 """
   5 in: Death Valley X
   6
   7     Accelerometer code: Martin Senkerik <martinsenkerik@gmail.com>
   8     (BSD licence)
   9
  10 """
  11
  12 import struct
  13 import sys
  14 from math import sqrt
  15 import threading, time, gtk
  16 from math import *
  17
  18
  19 debug = False
  20
  21 firstsensorfile = "/dev/input/event3"
  22
  23 #int, int, short, short, int
  24 fmt = 'iihhi'
  25
  26 #open file in binary mode
  27 accel1 = open(firstsensorfile,"rb")
  28
  29 def sign(val):
  30     try:
  31         return abs(val) / val
  32     except ZeroDivisionError:
  33         return 1
  34
  35 class Worker(threading.Thread):
  36     form = None
  37     bRun = True
  38     def __init__(self, form):
  39         if debug: print "DEBU:","Accelerometer thread initialized"
  40         self.form = form
  41         self.x = axis()
  42         self.y = axis()
  43         threading.Thread.__init__ ( self, name="Worker" )
  44
  45     def run(self):
  46         event = accel1.read(16)
  47
  48         if debug: "DEBUG:","Accelerometer thread running"
  49         self.form.ready_semaphore.acquire()
  50
  51         ## Repeat while not received signal to stop (through bRun)
  52         while event and self.bRun:
  53
  54             (time1, time2, type, code, value) = struct.unpack(fmt,event)
  55             ## Only reading X, Y now
  56             if type == 2:
  57                 if code == 0:
  58                     self.x.low_pass(time1, time2, value)
  59                     self.y.low_pass(time1, time2)
  60                     #self.z.low_pass(time1, time2)
  61                 if code == 1:
  62                     self.x.low_pass(time1, time2)
  63                     self.y.low_pass(time1, time2, value)
  64                     #self.z.low_pass(time1, time2)
  65                 if code == 2:
  66                     #self.x.low_pass(time1, time2)
  67                     #self.y.low_pass(time1, time2)
  68                     #self.z.low_pass(time1, time2, value)
  69                     pass
  70             if type == 0 and code == 0:
  71                  ## When new values read, change ship speed in game
  72                  self.form.drawing_area.shipratio = self.y.val/-1000.0
  73             event = accel1.read(16)
  74
  75         ## If finished, clean up
  76         accel1.close()
  77         gtk.main_quit()
  78
  79     def calibrate(self):
  80         """ Calibrate if sensor readings are not centered where they ought to """
  81         self.x.calib = self.x.val
  82         self.y.calib = self.y.val
  83
  84
  85 class axis:
  86     last_val_lp = 0
  87     old_time1 = 0
  88     old_time2 = 0
  89     RC = 5
  90     val_lp = 0
  91     val = 0
  92     calib = 0
  93     def _get_delta(self, time1, time2):
  94         new_time1 = long(time1) - long(self.old_time1)
  95         new_time2 = long(time2) - long(self.old_time2)
  96
  97         if new_time2 < 0.0:
  98             new_time1 -= 1
  99             new_time2 += 1000000
 100
 101         self.old_time1 = time1
 102         self.old_time2 = time2
 103
 104         return float("%s.%s" % (str(new_time1), str(new_time2)))
 105
 106     def low_pass(self, time1, time2, value = None):
 107
 108         # currently bypassed
 109         #---------------------------
 110
 111         if value == None:
 112             value = self.val - self.calib
 113         else:
 114             self.val = value
 115             value = value - self.calib
 116
 117         #dt = self._get_delta(time1, time2)
 118         #alpha = dt / float(self.RC + dt)
 119
 120         #self.val_lp = self.last_val_lp + alpha * (value - self.last_val_lp)
 121         self.val_lp = value
 122
 123         #self.last_val_lp = self.val_lp
 124
 125
 126     def get_angle(self):
 127         return (float(self.val_lp) / 1000) * (pi/2)
 128
 129     def __repr__(self):
 130         return "<axis %s (%s)>" % (str(int(self.val_lp)), str(self.val))
 131
 132