def __init__(self, name='maus', i2c_bus=0, servo_trims=[0, 0, 0, 0, 0], servo_pins=[8, 9, 10, 11, 4], pca9685_address=0x40, bno055_address=0x29): #Configuration self._name = name self._i2c_bus = i2c_bus self._servo_trims = servo_trims self._servo_pins = servo_pins self._pca9685_address = pca9685_address self._bno055_address = bno055_address #Setting up hardware self._bus = smbus.SMBus(self._i2c_bus) if not self._bus: raise Exception('I2C bus connection failed!') self.control = ServoController(self._bus, self._pca9685_address) self.sensor = Inclinometer(self._bus, self._bno055_address) self.sensor.pitch_trim=-8.5 #Setting up OctoSnake self.osc = [] self.osc.append(octosnake.Oscillator()) self.osc.append(octosnake.Oscillator(octosnake.semiSin)) self.osc.append(octosnake.Oscillator()) self.osc.append(octosnake.Oscillator(octosnake.semiSin)) self.osc.append(octosnake.Oscillator()) self.osc[1].ref_time = self.osc[0].ref_time self.osc[2].ref_time = self.osc[0].ref_time self.osc[3].ref_time = self.osc[0].ref_time self.osc[4].ref_time = self.osc[0].ref_time #Setting up PyKDL self.ik = MausKinematics() #Setting up servo controller for i in range(len(self._servo_pins)): self.control.addServo(self._servo_pins[i], self._servo_trims[i]) self.control.servos[self._servo_pins[1]].reverse = True self.control.servos[self._servo_pins[3]].reverse = True
import os import sys sys.path.append( os.path.dirname(os.path.abspath(__file__).replace('examples/bno055', ''))) import smbus from hardware.bno055.bno055 import Inclinometer import time bus = smbus.SMBus(0) device_address = 0x29 if not bus: raise Exception('I2C bus connection failed!') sensor = Inclinometer(bus, device_address) while True: print 'Pitch: ', sensor.getPitch(), '\tRoll: ', sensor.getRoll( ), '\tYaw: ', sensor.getYaw() time.sleep(0.1)