def cord_init():
# create sensorimotor communication bus
cord = Sensorimotor(
number_of_motors=6, # how many motors
verbose=False, # print debug info
update_rate_Hz=100) # low-level update frequency
try:
# checking for motors
N = cord.ping()
print("Found {0} sensorimotors.".format(N))
time.sleep(0.25)
# starting motorcord
cord.start()
i = 0
except:
# Script crashed?
#print("\rException thrown, stopping cord.")
cord.stop()
raise
return cord
def main(args):
conf = conflib[args.conf]
args_main = argparse.Namespace()
for conf_key in conf: # ['number_of_motors', 'update_rate_Hz', 'smnode_ids']:
# update from command line
if hasattr(args, conf_key) and getattr(args, conf_key) is not None:
setattr(args_main, conf_key, getattr(args, conf_key))
else:
setattr(args_main, conf_key, conf[conf_key])
# convenience
args_main.update_period_s = 1. / args_main.update_rate_Hz
# create queue
qu = queue.Queue(maxsize=10)
# def cb_hexagon_motors(path, args, types, target, unk):
# # def cb_hexagon_motors(*args, **kwargs):
# # i, f = args
# # print('args = {0}, kwargs = {1}'.format(args, kwargs))
# # print('qu = {0}'.format(qu))
# print("cb_hexagon_motors message {0} with arguments {1}".format(path, args))
# qu.put((path, args))
# # print('received args {0}'.format(args))
osc_address_motors = f"/{args_main.name}_motors"
osc_address_sensors = f"/{args_main.name}_sensors"
# create server, listening on port 1234
oscsrv = OSCsrv(port=args_main.port, queue=qu)
oscsrv.add_method(
path=osc_address_motors,
# types='f'*number_of_motors,
types='iiii',
# use a partial here to bind the qu argument
callback=partial(cb_hexagon_motors, qu)
# callback=cb_hexagon_motors,
)
# # fix pd
# target = liblo.Address(1337)
# liblo.send(target, "/reconnect", 'bang')
osc_target_hub = liblo.Address(1237)
osc_target_trigrid = liblo.Address('localhost', 1235, liblo.UDP)
# init motors
if not args_main.dummy:
cord = Sensorimotor(number_of_motors=args_main.number_of_motors,
verbose=DEBUG,
update_rate_Hz=args_main.update_rate_Hz)
else:
cord = SensorimotorDummy(number_of_motors=args_main.number_of_motors,
verbose=DEBUG,
update_rate_Hz=args_main.update_rate_Hz)
try:
# checking for motors
N = cord.ping()
print("osc2sensorimotor.main found {0} sensorimotors".format(N))
sleep(0.2)
# # TODO: set this according to your supply voltage and desired max. motor speed
# cord.set_voltage_limit([0.8, 0.8, 0.8])
# starting motorcord
cord.start()
# beat = tst0
except (Exception):
print('osc2sensorimotor.main failed to start motors')
pass
# start loop
beats = [[0.6, 0.0, 0.0], [0.0, 0.6, 0.0], [0.0, 0.0, 0.6]]
cnt = 0
try:
while True:
# print('main loop cnt = {0}'.format(cnt))
qud = None
while qu.qsize() > 0:
qud = qu.get()
# qud[0] = 0
# print('qu {0}'.format(qud))
# QUD[0,:] = np.array(qud)
# QUD = np.roll(QUD, shift=1, axis=0)
# print('qud = {0}'.format(qud))
# USER CODE HERE BEGIN
# motors.apply_impulse(b[0])
# b = beats[cnt%3]
if qud is not None:
loopfunc_hexagon(qud[1], cord)
# time.sleep(1e-3)
cnt += 1
# print('cnt={0}'.format(cnt))
if cnt % 1000 == 0:
# gng.stop_training()
# f = open('gng.bin', 'wb')
# pickle.dump(gng, f)
# f.close()
print('cnt = {0}'.format(cnt))
# pass
for i in args_main.smnode_ids:
x = cord.get_raw_data_recv(i, 11)
if i == 1:
print(i, x)
# print(x[5], x[6])
l_ = [i] + x
oscsrv.server.send(osc_target_trigrid, osc_address_sensors,
*l_)
time.sleep(args_main.update_period_s)
except (KeyboardInterrupt, SystemExit):
print("key fin")
oscsrv.isrunning = False
# stopping motor cord
print("\rAborted, stopping motors")
cord.stop()
# plt.ioff()
# gng.stop_training()
except:
# Script crashed?
print("\rException thrown, stopping cord.")
cord.stop()
raise
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=False)
try:
# Check for motors
N = motors.ping()
print("Found {0} motors.".format(N))
sleep(1.0)
#TODO: set this according to your supply voltage and desired max. motor speed
motors.set_voltage_limit([0.16, 0.16])
# Start motors
motors.start()
counter = 0
while (counter < 10):
#moves left
motors.apply_impulse([0.1, 0.1])
print("tak")
counter = counter + 1
sleep(0.1)
counter = 0
while (counter < 10):
#move right
motors.apply_impulse([0.1, -0.1])
print("tak")
counter = counter + 1
sleep(0.1)
counter = 0
motors.stop()
except (KeyboardInterrupt, SystemExit):
# Stop motors
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=False)
try:
# Check for motors
N = motors.ping()
print("Found {0} motors.".format(N))
sleep(1.0)
#voltage limit for supply voltage and desired max. motor speed
motors.set_voltage_limit([0.16, 0.16])
# Start motors
motors.start()
#TODO: set this parameters according to your desired motor positon control behaviour
motors.set_pos_ctrl_params(0,
Kp=0,
Ki=0,
Kd=0,
deadband=0,
pulse_threshold=0)
motors.set_pos_ctrl_params(1,
Kp=0,
Ki=0,
Kd=0,
deadband=0,
pulse_threshold=0)
motors.set_position([0.3, 0.3])
sleep(1.5)
# Print the current position
print_position(motors.get_position())
motors.set_position([-0.3, -0.3])
sleep(1.5)
# Print the current position
print_position(motors.get_position())
motors.stop()
except (KeyboardInterrupt, SystemExit):
# Stop motors
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=3, verbose=False)
# checking for motors
N = motors.ping()
print("Found {0} sensorimotors.".format(N))
sleep(1.0)
# starting motorcord
motors.set_voltage_limit([0.6, 0.6, 0.6])
motors.start()
beat = tst0
try:
while (True):
# USER CODE HERE BEGIN
print("tak")
for b in beat:
motors.apply_impulse(b[0])
sleep(b[1] * 1.2)
# USER CODE HERE END
except (KeyboardInterrupt, SystemExit):
# stopping motor cord
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
raise
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=False)
try:
# Check for motors
N = motors.ping()
print("Found {0} motors.".format(N))
sleep(1.0)
#TODO: set the supply voltage limit
motors.set_voltage_limit([0.11, 0.11])
# Start motors
motors.start()
motors.set_pos_ctrl_params(0,
Kp=1.8,
Ki=0.2,
Kd=0.1,
deadband=0.1,
pulse_threshold=0.1)
motors.set_pos_ctrl_params(1,
Kp=1.8,
Ki=0.2,
Kd=0.1,
deadband=0.1,
pulse_threshold=0.1)
motors.set_position([0.5, 0.5])
sleep(1.5)
# Print the current position
print_position(motors.get_position())
motors.set_position([-0.5, -0.5])
sleep(1.5)
# Print the current position
print_position(motors.get_position())
motors.stop()
except (KeyboardInterrupt, SystemExit):
# Stop motors
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=False)
try:
# checking for motors
N = motors.ping()
print("Found {0} motors.".format(N))
sleep(1.0)
#TODO: set this according to your supply voltage and desired max. motor speed
motors.set_voltage_limit([0.18, 0.18])
# starting motorcord
motors.start()
#TODO: set this parameters according to your desired motor positon control behaviour
motors.set_pos_ctrl_params(0,
Kp=0.9,
Ki=0.0,
Kd=0.05,
deadband=0.0,
pulse_threshold=0.1)
motors.set_pos_ctrl_params(1,
Kp=0.9,
Ki=0.0,
Kd=0.05,
deadband=0.0,
pulse_threshold=0.1)
while (True):
print_position(motors.get_position())
sleep(0.2)
#prints the current position
motors.stop()
except (KeyboardInterrupt, SystemExit):
# stopping motor cord
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
raise
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=False)
try:
# checking for motors
N = motors.ping()
print("Found {0} sensorimotors.".format(N))
sleep(1.0)
#TODO: set this according to your supply voltage and desired max. motor speed
#motors.set_voltage_limit([0.25, 0.25])
# starting motorcord
motors.start()
#TODO: set this parameters according to your desired motor positon control behaviour
#motors.set_pos_ctrl_params(0, Kp = 1.0, Ki = 0.0, Kd = 0.0, deadband = 0.00, pulse_threshold = 0.00)
while (True):
# USER CODE HERE BEGIN
motors.set_position([0.2, 0.2])
for i in range(100):
sleep(0.01)
print_position(motors.get_position())
motors.set_position([-0.2, 0.2])
sleep(1.0)
# USER CODE HERE END
except (KeyboardInterrupt, SystemExit):
# stopping motor cord
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
raise
print("____\nDONE.")
def main():
motors = Sensorimotor(number_of_motors=2, verbose=True)
# checking for motors
N = motors.ping()
print("Found {0} sensorimotors.".format(N))
sleep(1.0)
# starting motorcord
motors.set_voltage_limit([0.1, 0.1])
motors.start()
try:
while (True):
# USER CODE HERE BEGIN
motors.set_position([0.2, 0.2])
#print_position(motors.get_position())
sleep(0.25)
motors.set_position([0.2, -0.2])
#print_position(motors.get_position())
sleep(0.25)
motors.set_position([-0.2, -0.2])
#print_position(motors.get_position())
sleep(0.25)
motors.set_position([-0.2, +0.2])
#print_position(motors.get_position())
sleep(0.25)
# USER CODE HERE END
except (KeyboardInterrupt, SystemExit):
# stopping motor cord
print("\rAborted, stopping motors")
motors.stop()
except:
# Script crashed?
print("\rException thrown, stopping motors")
motors.stop()
raise
print("____\nDONE.")
