added compensation

Author: turbinenreiter

attitude.py

@@ -0,0 +1,7 @@
+import math
+
+def angle(imu_data):
+ x,y,z = imu_data
+ roll = math.atan(x/math.sqrt(y**2+z**2))
+ pitch = math.atan(y/math.sqrt(x**2+z**2))
+ return (roll, pitch)

bma2x2.py

@@ -26,6 +26,7 @@
 '''
 
 import machine
+from time import sleep_ms as sleep
 
 # from stackoverflow J.F. Sebastian
 def _twos_comp(val, bits=8):
@@ -54,7 +55,7 @@ def __init__(self, i2c):
  except OSError:
  raise OSError('no BMA2X2 connected')
  self.set_range(16) # default range 16g
- self.set_filter_bw(125) # default filter bandwith 125Hz
+ self.set_filter_bw(128) # default filter bandwith 125Hz
 
  def _read_accel(self, addr):
  """return accel data from addr"""
@@ -78,19 +79,45 @@ def set_range(self, accel_range):
  return self.get_range()
 
  def get_range(self):
- return self.i2c.readfrom_mem(self.acc_addr, 0x0F, 1)
+ return {3:2,5:4,8:8,12:16}[self.i2c.readfrom_mem(self.acc_addr, 0x0F, 1)[0]]
 
  def set_filter_bw(self, freq):
- freqs = {8:b'\x08', 16:b'\x09', 32:b'\x0A', 63:b'\x0B', 125:b'\x0C', 250:b'\x0D', 500:b'\x0E', 1000:b'\x0F'}
+ freqs = {8:b'\x08', 16:b'\x09', 32:b'\x0A', 64:b'\x0B', 128:b'\x0C', 256:b'\x0D', 512:b'\x0E', 1024:b'\x0F'}
  try:
  freq_byte = freqs[freq]
  except:
- raise ValueError('invalid filter bandwith, use 8, 16, 32, 63, 125, 250, 500 or 1000')
+ raise ValueError('invalid filter bandwith, use 8, 16, 32, 64, 128, 256, 512 or 1024')
  self.i2c.writeto_mem(self.acc_addr, 0x10, freq_byte)
  return self.get_filter_bw()
 
  def get_filter_bw(self):
- return self.i2c.readfrom_mem(self.acc_addr, 0x10, 1)
+ return 2**(self.i2c.readfrom_mem(self.acc_addr, 0x10, 1)[0]-5)
+
+ def compensation(self, active=None):
+ accel_range = self.get_range()
+ self.set_range(2)
+ self.i2c.writeto_mem(self.acc_addr, 0x37, b'\x21') # settings x0y0z1 10Hz
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x80') # reset
+ if active is None: # trigger fast comp
+ active = 'compensated'
+ print(self.i2c.readfrom_mem(self.acc_addr, 0x36, 1))
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x20') # x
+ sleep(100)
+ print(self.i2c.readfrom_mem(self.acc_addr, 0x36, 1))
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x40') # y
+ sleep(100)
+ print(self.i2c.readfrom_mem(self.acc_addr, 0x36, 1))
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x60') # z
+ sleep(100)
+ print(self.i2c.readfrom_mem(self.acc_addr, 0x36, 1))
+ elif active: # activate slow comp
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x07')
+ elif not active: # deactivate slow comp
+ self.i2c.writeto_mem(self.acc_addr, 0x36, b'\x00')
+ else:
+ raise TypeError('pass a boolean or no argument')
+ self.set_range(accel_range)
+ return active
 
  def x(self):
  return self._read_accel(0x02)

bmx055.py

@@ -25,6 +25,7 @@
 SOFTWARE.
 '''
 
+import machine
 from bma2x2 import BMA2X2
 from bmg160 import BMG160
 from bmm050 import BMM050
@@ -35,5 +36,5 @@ class BMX055():
  def __init__(self, i2c):
 
  self.accel = BMA2X2(i2c)
-# self.gyro = BMG160(i2c)
+ self.gyro = BMG160(i2c)
 # self.mag = BMM050(self.i2c)

boot.py

@@ -0,0 +1,3 @@
+# boot.py -- run on boot-up
+# can run arbitrary Python, but best to keep it minimal
+

main.py

@@ -1,40 +1,15 @@
-# main.py -- put your code here!
-
-from machine import I2C, Pin
-from bmx055 import BMX055
-import time
-
-# for atsamd21
-#i2c_bus = I2C(scl=Pin.board.SCL, sda=Pin.board.SDA, freq=400000)
-#i2c_bus.init()
-
-# for esp8266 (if this doesn't work switch 4 and 5)
-i2c_bus = I2C(scl=Pin(5), sda=Pin(4))
-print(i2c_bus.scan())
-try:
- print('1:', i2c_bus.readfrom_mem(0x18, 0x00, 1))
-except:
- print('2:', i2c_bus.readfrom_mem(0x18, 0x00, 1))
-
-imu = BMX055(i2c_bus)
-
-print(imu.accel.i2c.readfrom_mem(imu.accel.acc_addr, 0, 63))
-
-print('temperature:', imu.accel.temperature())
-print('resolution:', imu.accel._resolution)
-print('accel:', imu.accel.xyz())
-#print('gyro:', imu.gyro.xyz())
-print('set range 4:', imu.accel.set_range(4))
-print('get range:', imu.accel.get_range())
-print('set range 8:', imu.accel.set_range(8))
-print('set filter 8:', imu.accel.set_filter_bw(8))
-print('get filter:', imu.accel.get_filter_bw())
-print('set filter 250:', imu.accel.set_filter_bw(250))
-print('accel:', imu.accel.xyz())
-
-t_end = time.ticks_ms() + 10*1000
-while time.ticks_ms() < t_end:
- print(imu.accel.xyz())
- time.sleep_ms(100)
-
-i2c_bus.deinit()
+from bmx055 import BMX055
+from servo import Servo
+from machine import Pin, I2C
+
+#imu = BMX055(I2C(sda=Pin(4), scl=Pin(5)))
+
+#right = Servo(Pin(2))
+#left = Servo(Pin(15))
+
+for i in range(17):
+ try:
+ Servo(Pin(i)).write_angle(180)
+ except:
+ print(i)
+#sgeht