class Maus(object):
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
def walk(self, steps):
left_x_amp = 20 #millimeters
right_x_amp = 20 #millimeters
z_amp = 30 #millimeters
swing_amp = 70 #degrees
T = 900 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-10, -75, -10, -75, 0]
phase = [90, 180, 270, 0, 335+90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() +float(T*steps/1000)
while time.time() < final:
try:
roll_data=self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def slowWalk(self, steps):
left_x_amp = 10 #millimeters
right_x_amp = 10 #millimeters
z_amp = 20 #millimeters
swing_amp = 15 #degrees
T = 700 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 170]
#self.osc[4].signal=signal.triangle
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T*steps/1000)
modded_phase=0
while time.time() < final:
try:
roll_data=0#self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output-roll_data)
modded_phase+=0.5
#self.osc[4].phase=modded_phase
print self.osc[4].phase
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def run(self, steps):
left_x_amp = 10 #millimeters
right_x_amp = 10 #millimeters
z_amp = 20 #millimeters
swing_amp = 0 #degrees
T = 150 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -60, -20, -60, 0]
phase = [0, 270, 0, 270, 0]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() +float(T*steps/1000)
while time.time() < final:
try:
roll_data=self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def walkBackwards(self, steps):
left_x_amp = 20 #millimeters
right_x_amp = 20 #millimeters
z_amp = 30 #millimeters
swing_amp = 70 #degrees
T = 900 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 0, 270, 180, 335-90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() +float(T*steps/1000)
while time.time() < final:
try:
roll_data=0#self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def turnLeft(self, steps):
left_x_amp = 0 #millimeters
right_x_amp = 20 #millimeters
z_amp = 30 #millimeters
swing_amp = 70 #degrees
T = 900 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, 0, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 335+90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() +float(T*steps/1000)
while time.time() < final:
try:
roll_data=0#self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def turnRight(self, steps):
left_x_amp = 20 #millimeters
right_x_amp = 0 #millimeters
z_amp = 30 #millimeters
swing_amp = 70 #degrees
T = 900 #milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, 0, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 335+90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() +float(T*steps/1000)
while time.time() < final:
try:
roll_data=0#self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output-roll_data/7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output+roll_data/7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output-0.5*roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
#def jump(self):
def moveJoints(self, joint_target, execution_time=0, sample_time=18):
if execution_time > sample_time:
increment = []
initial_position = []
for i in range(len(self._servo_pins)):
initial_position.append(self.control.getPosition(self._servo_pins[i]))
increment.append(float(joint_target[i] - initial_position[i]) / (execution_time / sample_time))
final_time = time.time()*1000 + execution_time
iteration = 1
while time.time()*1000 < final_time:
partial_time = time.time()*1000 + sample_time
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], initial_position[i] + (iteration * increment[i]))
iteration += 1
while (time.time()*1000 < partial_time):
pass
else:
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], joint_target[i])
def moveCart(self, x_left, z_left, x_right, z_right):
pass
def zero(self):
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], 0)
def sleep(self):
self.control.sleep()
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)
class Maus(object):
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
def walk(self, steps):
left_x_amp = 20 # millimeters
right_x_amp = 20 # millimeters
z_amp = 30 # millimeters
swing_amp = 70 # degrees
T = 900 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-10, -75, -10, -75, 0]
phase = [90, 180, 270, 0, 335 + 90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
while time.time() < final:
try:
roll_data = self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output - roll_data / 7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output + roll_data / 7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output - 0.5 * roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def slowWalk(self, steps):
left_x_amp = 10 # millimeters
right_x_amp = 10 # millimeters
z_amp = 20 # millimeters
swing_amp = 15 # degrees
T = 700 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 170]
# self.osc[4].signal=signal.triangle
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
modded_phase = 0
while time.time() < final:
try:
roll_data = 0 # self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output - roll_data)
modded_phase += 0.5
# self.osc[4].phase=modded_phase
print self.osc[4].phase
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def run(self, steps):
left_x_amp = 10 # millimeters
right_x_amp = 10 # millimeters
z_amp = 20 # millimeters
swing_amp = 0 # degrees
T = 150 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -60, -20, -60, 0]
phase = [0, 270, 0, 270, 0]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
while time.time() < final:
try:
roll_data = self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output - roll_data / 7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output + roll_data / 7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def walkBackwards(self, steps):
left_x_amp = 20 # millimeters
right_x_amp = 20 # millimeters
z_amp = 30 # millimeters
swing_amp = 70 # degrees
T = 900 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 0, 270, 180, 335 - 90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
while time.time() < final:
try:
roll_data = 0 # self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output - roll_data / 7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output + roll_data / 7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output - 0.5 * roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def turnLeft(self, steps):
left_x_amp = 0 # millimeters
right_x_amp = 20 # millimeters
z_amp = 30 # millimeters
swing_amp = 70 # degrees
T = 900 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, 0, right_x_amp, z_amp, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 335 + 90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
while time.time() < final:
try:
roll_data = 0 # self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output - roll_data / 7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output + roll_data / 7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output - 0.5 * roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
def turnRight(self, steps):
left_x_amp = 20 # millimeters
right_x_amp = 0 # millimeters
z_amp = 30 # millimeters
swing_amp = 70 # degrees
T = 900 # milliseconds
period = [T, T, T, T, T]
amplitude = [left_x_amp, z_amp, right_x_amp, 0, swing_amp]
offset = [-20, -75, -20, -75, 0]
phase = [90, 180, 270, 0, 335 + 90]
for i in range(len(self.osc)):
self.osc[i].period = period[i]
self.osc[i].amplitude = amplitude[i]
self.osc[i].phase = phase[i]
self.osc[i].offset = offset[i]
final = time.time() + float(T * steps / 1000)
while time.time() < final:
try:
roll_data = 0 # self.sensor.getPitch()
for i in range(len(self.osc)):
self.osc[i].refresh()
left_joints = self.ik.getJoints(self.osc[0].output, 0, self.osc[1].output - roll_data / 7)
right_joints = self.ik.getJoints(self.osc[2].output, 0, self.osc[3].output + roll_data / 7)
self.control.move(self._servo_pins[0], left_joints[0])
self.control.move(self._servo_pins[1], right_joints[0])
self.control.move(self._servo_pins[2], left_joints[1])
self.control.move(self._servo_pins[3], right_joints[1])
self.control.move(self._servo_pins[4], self.osc[4].output - 0.5 * roll_data)
except IOError:
self._bus = smbus.SMBus(self._i2c_bus)
# def jump(self):
def moveJoints(self, joint_target, execution_time=0, sample_time=18):
if execution_time > sample_time:
increment = []
initial_position = []
for i in range(len(self._servo_pins)):
initial_position.append(self.control.getPosition(self._servo_pins[i]))
increment.append(float(joint_target[i] - initial_position[i]) / (execution_time / sample_time))
final_time = time.time() * 1000 + execution_time
iteration = 1
while time.time() * 1000 < final_time:
partial_time = time.time() * 1000 + sample_time
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], initial_position[i] + (iteration * increment[i]))
iteration += 1
while time.time() * 1000 < partial_time:
pass
else:
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], joint_target[i])
def moveCart(self, x_left, z_left, x_right, z_right):
pass
def zero(self):
for i in range(len(self._servo_pins)):
self.control.move(self._servo_pins[i], 0)
def sleep(self):
self.control.sleep()
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)