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)
