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
os.path.dirname(os.path.abspath(__file__).replace('examples/pca9685', '')))
print os.path.dirname(
os.path.abspath(__file__).replace('examples/pca9685', ''))
import smbus
from hardware.pca9685.pca9685 import ServoController
import time
bus = smbus.SMBus(0)
device_address = 0x40
if not bus:
raise Exception('I2C bus connection failed!')
servo_id = 4
servo_trim = 21
control = ServoController(bus, device_address)
control.addServo(servo_id, servo_trim)
while True:
control.move(servo_id, 20)
print 'Envio 20, lectura: ', control.getPosition(servo_id)
time.sleep(3)
control.move(servo_id, -20)
print 'Envio -20, lectura: ', control.getPosition(servo_id)
time.sleep(3)