def on_daq_callback(self, data):
mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
mdf.tool_id = 0
mdf.missing = 0
self.variable += 1
mdf.xyz[:] = np.array([self.variable] * 3)
mdf.ori[:] = np.array(
[self.variable] * 4
) # will work but need!!! reading modules to know the format of buffer
#mdf.buffer[data.size:] = -1
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print self.variable
sys.stdout.write('|')
sys.stdout.flush()
# now check for exit message
in_msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
hdr = msg.GetHeader()
msg_type = hdr.msg_type
dest_mod_id = hdr.dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print "Received MT_EXIT, disconnecting..."
self.daq_task.StopTask()
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
self.stop()
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'RandomGen')
def timer_event(self):
done = False
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, msg)
x = mdf.target[0]
y = mdf.target[1]
self.setTargetPos(x, y)
#print "x: ", x, "|", self.tgt2pix(x) , " y: ", y, "|", self.tgt2pix(y)
elif msg_type == rc.MT_ROBOT_CONTROL_SPACE_ACTUAL_STATE:
mdf = rc.MDF_ROBOT_CONTROL_SPACE_ACTUAL_STATE()
copy_from_msg(mdf, msg)
x = mdf.pos[0]
y = mdf.pos[1]
self.setCursorPos(x, y)
#print "x: ", mdf.pos[0], "y: ", mdf.pos[1]
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'CursorDisplay')
elif msg_type == MT_EXIT:
self.exit()
done = True
else:
done = True
def get_frequency(self):
# loop over receiving messages until we get a POLARIS_POSITION message
# get a POLARIS_POSITION message, read sample_header.DeltaTime to get
# message frequency
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break;
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'CoilPlotter')
else:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(mdf, msg)
self.fsamp = 1/mdf.sample_header.DeltaTime
if self.fsamp != 0:
break
self.user_start_calibrate()
def __init__(self):
super(Display, self).__init__(180, 180, 180, 255)
self.mod = Dragonfly_Module(0, 0)
self.mod.ConnectToMMM("everest:7111")
self.mod.Subscribe(rc.MT_TASK_STATE_CONFIG)
self.mod.Subscribe(rc.MT_INPUT_DOF_DATA)
self.mod.Subscribe(rc.MT_COMBO_WAIT)
self.mod.Subscribe(rc.MT_TRIAL_CONFIG)
self.mod.Subscribe(rc.MT_END_TASK_STATE)
self.mod.Subscribe(rc.MT_PING)
self.msg = CMessage()
self.timer_sec = 0
self.timer_min = 0
self.position_bar = Polygon(v=[(20, 455), (20, 565), (1260, 565), (1260, 455)], color=(0.05, 0.05, 0.05, 1), stroke=0)
self.tgt_window = Polygon(v=[(0, 0), (0, 0), (0, 0), (0, 0)], color=(0, 0.6, 0.2, 1), stroke=0)
self.pos_fdbk = Polygon(v=[(20, 405), (20, 615), (28, 615), (28, 405)], color=(0, 0.2, 1.0, 1), stroke=0)
self.pos_fdbk_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0,0,0,255),
width=250,
bold=True,
x=1140, y=40)
self.combo_wait_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0,0,0,255),
width=250,
bold=True,
x=478, y=840)
self.score_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=36,
color=(0,0,0,255),
width=250,
bold=True,
x=533, y=240)
self.reward_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=22,
color=(0,0,0,255),
width=250,
bold=True,
x=475, y=120)
self.reset_score()
self.schedule_interval(self.update, 0.01)
def run(self):
mod = Dragonfly_Module(self.mid, 0)
mod.ConnectToMMM(self.server)
for sub in self.subs:
print "subscribing to %s" % (sub)
mod.Subscribe(sub)
mod.SendModuleReady()
while (self.status()):
msg = CMessage()
rcv = mod.ReadMessage(msg, 0)
if rcv == 1:
if msg.GetHeader().msg_type in self.subs:
self.recv_msg(msg)
sleep(.001)
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break;
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'ButtonDetector')
else:
self.process_message(msg)
def timer_event(self):
done = False
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_TASK_STATE_CONFIG:
self.tsc_mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(self.tsc_mdf, msg)
elif msg_type == rc.MT_FORCE_FEEDBACK:
mdf = rc.MDF_FORCE_FEEDBACK()
copy_from_msg(mdf, msg)
#self.fdbk_actual_pos = []
self.fdbk_actual_pos = [mdf.x, mdf.y, mdf.z, 0.0, 0.0, 0.0]
self.update_judging_data()
elif msg_type == rc.MT_FORCE_SENSOR_DATA:
mdf = rc.MDF_FORCE_SENSOR_DATA()
copy_from_msg(mdf, msg)
self.fdbk_actual_pos = []
self.fdbk_actual_pos.extend(mdf.data)
self.update_judging_data()
elif msg_type == rc.MT_END_TASK_STATE:
self.ets_mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(self.ets_mdf, msg)
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'SimpleDisplay')
elif msg_type == MT_EXIT:
self.exit()
done = True
else:
done = True
self.update_plot()
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
# read a Dragonfly message
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
return
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'fast_display_rms')
else:
self.process_message(msg)
def timer_event(self, parent):
done = False
sys.stdout.flush()
while not done:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
self.process_message(msg)
else:
done = True
def chk_msg(self):
while True:
in_msg = CMessage()
rcv = self.mod.ReadMessage(in_msg, 0.1)
if rcv == 1:
msg_type = in_msg.GetHeader().msg_type
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, in_msg, 'Metronome')
elif msg_type == self.in_msg_num:
in_mdf = eval('rc.MDF_%s()' % (self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
return in_mdf.sample_header.DeltaTime
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
if msg_type == rc.MT_APP_START:
try:
mdf = rc.MDF_APP_START()
copy_from_msg(mdf, msg)
config = mdf.config
print "Config: %s" % config
# -- to do --
# get a list of all modules in appman.conf for this host
# see if any of the modules above are already/still running
# start non-running modules
# -- to do --
print "Creating scripts"
appman.create_script(config, self.host_name)
print "Starting modules on host: %s" % self.host_name
appman.run_script(self.host_name)
self.mod.SendSignal(rc.MT_APP_START_COMPLETE)
except Exception, e:
print "ERROR: %s" % (e)
elif msg_type == rc.MT_PING:
print 'got ping'
self.respond_to_ping(msg, 'AppStarter')
# we use this msg to stop modules individually
elif msg_type == MT_EXIT:
print 'got exit'
elif msg_type == MT_KILL:
print 'got kill'
appman.kill_modules()
def initial_ping(self):
t = time.time()
while time.time() < t + 10:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'TMS_Mapping_Collection')
break
else:
pass
def send_metronome(self, state):
mdf = rc.MDF_MNOME_STATE()
self.serial_no += 1
mdf.sample_header.SerialNo = self.serial_no
mdf.sample_header.Flags = 0
mdf.sample_header.DeltaTime = (1. / 5)
mdf.state = state
msg = CMessage(rc.MT_MNOME_STATE)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
print "Sent message %d" % state
def send_output(self, sample_header):
mdf = rc.MDF_COMPOSITE_MOVEMENT_COMMAND()
mdf.tag = 'composite'
vel = np.zeros_like(mdf.vel)
vel[:] = self.get_combined_command()
if self.idle_gateable == 1:
vel[:8] *= self.gate
mdf.vel[:] = vel
mdf.sample_header = sample_header
msg = CMessage(rc.MT_COMPOSITE_MOVEMENT_COMMAND)
copy_to_msg(mdf, msg)
self.mod.SendMessage(msg)
def send_sample_generated(self):
sg = rc.MDF_SAMPLE_GENERATED()
self.serial_no += 1
sg.sample_header.SerialNo = self.serial_no
sg.sample_header.Flags = 0
sg.sample_header.DeltaTime = (1. / self.freq)
sg.source_timestamp = self.default_timer() #time.time()
sg_msg = CMessage(rc.MT_SAMPLE_GENERATED)
copy_to_msg(sg, sg_msg)
self.mod.SendMessage(sg_msg)
sys.stdout.write('|')
sys.stdout.flush()
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if self.calibrated:
if self.set_collecting:
if (msg_type == rc.MT_HOTSPOT_POSITION) & (self.got_coil == False):
# handling input message
in_mdf = rc.MDF_HOTSPOT_POSITION()
copy_from_msg(in_mdf, in_msg)
self.current_vtail = np.array(in_mdf.xyz[:]) #Hotspot position
self.current_vhead = np.array(in_mdf.ori[:3]) #Vector head of coil, used to find ori
self.got_coil = True
elif (msg_type == rc.MT_POLARIS_POSITION) & (self.got_head == False):
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
if in_mdf.tool_id == (self.glasses + 1):
# calculating output
self.head, Qr = self.tp.get_pos(orientations, positions)
print(self.head)
self.got_head = True
elif (self.got_head == True) & (self.got_coil == True):
plot_position = self.current_vtail - self.head + self.plot_vertex_vec
in_mdf = rc.MDF_POLARIS_POSITION()
out_mdf = rc.MDF_PLOT_POSITION()
copy_from_msg(in_mdf, in_msg)
out_mdf.xyz[:] = plot_position
out_mdf.ori[:] = np.append(self.current_vhead, 0)# Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
self.count += 1
save_array = np.insert(np.concatenate(((self.current_vtail - self.head), self.current_vhead)), 0, self.count)
self.current_map_data = np.vstack((self.current_map_data, save_array))
self.got_coil = False
self.got_head = False
self.set_collecting = False
if self.count > 100:
location = raw_input("Finished! Where should it save?")
np.savetxt(str(location) +'.txt',self.current_map_data[1:],
delimiter=',', newline='/n')
mlab.savefig(str(location) + '.png', figure=self.fig)
mlab.close(self.fig)
self.count = 0
self.run()
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
self.process_message(msg)
this_time = time()
self.diff_time = this_time - self.last_time
if self.diff_time > 1.:
self.last_time = this_time
self.write()
self.count[:] = 0
def respond_to_ping(self, msg, module_name):
#print "PING received for '{0}'".format(p.module_name)
dest_mod_id = msg.GetHeader().dest_mod_id
p = rc.MDF_PING()
copy_from_msg(p, msg)
if (p.module_name.lower() == module_name.lower()) or (p.module_name == "*") or \
(dest_mod_id == self.mod.GetModuleID()):
mdf = rc.MDF_PING_ACK()
mdf.module_name = module_name + ":" + self.host_name # + ":" + self.host_os
msg_out = CMessage(rc.MT_PING_ACK)
copy_to_msg(mdf, msg_out)
self.mod.SendMessage(msg_out)
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'CoilPlotter')
elif (msg_type == rc.MT_POLARIS_POSITION) & (self.calibrated == False):
self.calibrate_head(msg)
elif msg_type == rc.MT_TMS_TRIGGER:
self.set_collecting = True
self.got_coil = False
self.got_head = False
else:
self.process_message(msg)
def run(self):
self.delta_time_calc = self.default_timer() #time.time()
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
hdr = msg.GetHeader()
msg_type = hdr.msg_type
dest_mod_id = hdr.dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'SampleGenerator')
elif (msg_type == rc.MT_SPM_SPIKECOUNT):
msg_src_mod_id = hdr.src_mod_id
if msg_src_mod_id == rc.MID_SPM_MOD:
print "\n\n ** Detected SPM_SPIKECOUNT messages coming from SPM_MOD! Quitting..\n\n"
sys.exit(0)
else:
if len(self.triggers) > 0:
self.process_msg(msg)
else:
# if no triggers...
if len(self.triggers) == 0:
period = (1. / self.freq)
time_now = self.default_timer()
delta_time = period - (time_now - self.delta_time_calc)
#print "%f %f %f\n\n" % (time_now, self.delta_time_calc, delta_time)
if delta_time > 0:
time.sleep(delta_time)
self.delta_time_calc = self.delta_time_calc + period
self.send_sample_generated()
def send_btn_release(self, btn, controller_id):
print "controller_id %d sending button release %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_RELEASE()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
msg_out = CMessage(rc.MT_PS3_BUTTON_RELEASE)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.1)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'TrialStatus')
else:
self.process_message(msg)
this_time = time()
self.diff_time = this_time - self.last_time
if self.diff_time > 1.:
self.last_time = this_time
self.write()
def send_btn_press(self, btn, controller_id):
print "controller_id %d sending button press %s" % (controller_id, btn)
btn_map = {'l1' : rc.PS3_B_L1,
'l2' : rc.PS3_B_L2,
'r1' : rc.PS3_B_R1,
'x' : rc.PS3_B_X,
'sq' : rc.PS3_B_SQUARE,
'crc': rc.PS3_B_CIRCLE,
'trg': rc.PS3_B_TRIANGLE}
mdf_out = rc.MDF_PS3_BUTTON_PRESS()
mdf_out.whichButton = btn_map[btn]
mdf_out.controllerId = controller_id
# make outgoing message data
msg_out = CMessage(rc.MT_PS3_BUTTON_PRESS)
copy_to_msg(mdf_out, msg_out)
self.mod.SendMessage(msg_out)
def run(self):
self.delta_time_calc = time.time() #time.time()
while True:
if (time.time() - self.delta_time_calc) % 2 == 0:
self.tail[0] = self.tail[0] + 1
self.head[1] = self.head[1] + 1
out_mdf = rc.MDF_PLOT_POSITION()
self.serial_no += 1
out_mdf.sample_header.SerialNo = self.serial_no
out_mdf.sample_header.Flags = 0
out_mdf.sample_header.DeltaTime = (1. / 5)
out_mdf.xyz[:] = self.tail
out_mdf.ori[:] = np.append(self.head, 0)# Qk - coil active orientation
msg = CMessage(rc.MT_PLOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("C")
class Display(ColorLayer):
is_event_handler = True
def __init__(self):
super(Display, self).__init__(180, 180, 180, 255)
self.mod = Dragonfly_Module(0, 0)
self.mod.ConnectToMMM("everest:7111")
self.mod.Subscribe(rc.MT_TASK_STATE_CONFIG)
self.mod.Subscribe(rc.MT_INPUT_DOF_DATA)
self.mod.Subscribe(rc.MT_COMBO_WAIT)
self.mod.Subscribe(rc.MT_TRIAL_CONFIG)
self.mod.Subscribe(rc.MT_END_TASK_STATE)
self.mod.Subscribe(rc.MT_PING)
self.msg = CMessage()
self.timer_sec = 0
self.timer_min = 0
self.blank_display = False
self.position_bar = Polygon(v=[(20, 455), (20, 565), (1260, 565),
(1260, 455)],
color=(0.05, 0.05, 0.05, 1),
stroke=0)
self.tgt_window = Polygon(v=[(0, 0), (0, 0), (0, 0), (0, 0)],
color=(0, 0.6, 0.2, 1),
stroke=0)
self.pos_fdbk = Polygon(v=[(20, 405), (20, 615), (28, 615), (28, 405)],
color=(0, 0.2, 1.0, 1),
stroke=0)
self.pos_fdbk_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=1140,
y=40)
self.combo_wait_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=478,
y=840)
self.score_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=36,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=533,
y=240)
self.reward_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=22,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=475,
y=120)
self.reset_score()
self.schedule_interval(self.update, 0.01)
def timer_count_down(self, dt):
self.timer_sec -= 1
if self.timer_sec < 0:
if self.timer_min > 0:
self.timer_min -= 1
self.timer_sec = 59
else:
self.unschedule(self.timer_count_down)
self.combo_wait_txt.text = ''
return
self.combo_wait_txt.text = 'Relax Time %d:%02d' % (self.timer_min,
self.timer_sec)
if self.timer_min == 0 and self.timer_sec <= 5 and self.timer_sec > 0:
winsound.PlaySound(
os.path.join(os.environ.get('ROBOT_CONFIG'), 'default',
'gocue.wav'),
winsound.SND_FILENAME | winsound.SND_ASYNC)
def reset_score(self):
self.score = 0
self.score_force_level = 0
self.score_target_dist = 999
self.score_force_mult = 0
self.score_target_mult = 0
self.score_txt.text = "Score: %d" % self.score
self.reward_txt.text = ""
def update_reward(self):
reward = self.score_force_mult * self.score_target_mult
pts = "points"
if reward == 1:
pts = "point"
self.reward_txt.text = "Current Reward: %d %s" % (reward, pts)
def increment_score(self):
self.score += self.score_force_mult * self.score_target_mult
self.score_txt.text = "Score: %d" % self.score
def update(self, dt):
while True:
rcv = self.mod.ReadMessage(self.msg, 0)
if rcv == 1:
hdr = self.msg.GetHeader()
msg_type = hdr.msg_type
if msg_type == rc.MT_PING:
self.reset_score()
elif msg_type == rc.MT_INPUT_DOF_DATA:
mdf = rc.MDF_INPUT_DOF_DATA()
copy_from_msg(mdf, self.msg)
if mdf.tag == 'carduinoIO':
fdbk = 5 - mdf.dof_vals[
7] # invert to match phyiscal setup
x_pos = int((fdbk * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
self.pos_fdbk_txt.text = "%.2f V" % fdbk
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
# if msg_type == rc.MT_FORCE_SENSOR_DATA:
# mdf = rc.MDF_FORCE_SENSOR_DATA()
# copy_from_msg(mdf, self.msg)
#
# x_fdbk = mdf.data[0]
# x_fdbk_width = int((x_fdbk / MAX_FDBK) * MAX_WIDTH)
elif msg_type == rc.MT_COMBO_WAIT:
mdf = rc.MDF_COMBO_WAIT()
copy_from_msg(mdf, self.msg)
print mdf.duration
duration = mdf.duration / 1000 # convert to seconds
self.timer_sec = duration % 60
self.timer_min = duration / 60
self.schedule_interval(self.timer_count_down, 1)
elif msg_type == rc.MT_TRIAL_CONFIG:
self.unschedule(self.timer_count_down)
self.combo_wait_txt.text = ''
# start displaying again
self.blank_display = False
self.color = (180, 180, 180)
elif msg_type == rc.MT_END_TASK_STATE:
mdf = rc.MDF_END_TASK_STATE()
copy_from_msg(mdf, self.msg)
if (mdf.id == REWARD_TS) and (mdf.outcome == 1):
self.increment_score()
if (mdf.id in [2, 3, 4]) and (mdf.outcome == 0):
self.color = (0, 0, 0)
self.blank_display = True
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, self.msg)
if mdf.background_color == 'gray':
self.color = (180, 180, 180)
elif mdf.background_color == 'red':
self.color = (150, 12, 12)
elif mdf.background_color == 'green':
self.color = (0, 150, 50)
if mdf.fdbk_display_color == 'gray':
self.tgt_window.color = (0.3, 0.3, 0.3, 1)
elif mdf.fdbk_display_color == 'yellow':
self.tgt_window.color = (0.5, 0.5, 0.0, 1)
elif mdf.fdbk_display_color == 'green':
self.tgt_window.color = (0.0, 0.6, 0.2, 1)
elif mdf.fdbk_display_color == 'red':
self.tgt_window.color = (0.6, 0.05, 0.05, 1)
if not (math.isnan(mdf.target[0])) and not (math.isnan(
mdf.target[1])):
x_tgt_lo = int(
(mdf.target[0] * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
x_tgt_hi = int(
(mdf.target[1] * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
self.tgt_window.v[0] = (x_tgt_lo, 430)
self.tgt_window.v[1] = (x_tgt_lo, 590)
self.tgt_window.v[2] = (x_tgt_hi, 590)
self.tgt_window.v[3] = (x_tgt_hi, 430)
# update multipliers during "ForceRamp" task state
if mdf.id == FORCERAMP_TS:
force_level = mdf.sep_threshold_f[1]
if force_level > self.score_force_level:
self.score_force_level = force_level
self.score_force_mult += 1
# new combo, reset target multipliers
self.score_target_mult = 0
self.score_target_dist = 999
target_level = mdf.target[1] - mdf.target[0]
if target_level < self.score_target_dist:
self.score_target_dist = target_level
self.score_target_mult += 1
self.update_reward()
#print mdf.id
#print force_level, self.score_force_level, self.score_force_mult
#print target_level, self.score_target_dist, self.score_target_mult
#print "\n"
else:
break
#def on_key_release( self, keys, mod ):
def draw(self):
super(Display, self).draw()
if not self.blank_display:
self.position_bar.render()
self.tgt_window.render()
self.pos_fdbk.render()
#self.pos_fdbk_txt.draw()
self.combo_wait_txt.draw()
self.score_txt.draw()
self.reward_txt.draw()
def __init__(self):
super(Display, self).__init__(180, 180, 180, 255)
self.mod = Dragonfly_Module(0, 0)
self.mod.ConnectToMMM()
self.mod.Subscribe(rc.MT_TASK_STATE_CONFIG)
self.mod.Subscribe(rc.MT_INPUT_DOF_DATA)
self.mod.Subscribe(rc.MT_COMBO_WAIT)
self.mod.Subscribe(rc.MT_TRIAL_CONFIG)
self.mod.Subscribe(rc.MT_END_TASK_STATE)
self.mod.Subscribe(rc.MT_PING)
self.mod.Subscribe(rc.MT_RT_POSITION_FEEDBACK)
self.msg = CMessage()
self.timer_sec = 0
self.timer_min = 0
self.transformationType = 0
dims = director.get_window_size()
self.width = dims[0]
self.height = dims[1]
self.oldWidth = 1280
self.oldHeight = 1024
self.blank_display = False
self.position_bar = Polygon(v=[(20, 455), (20, 565), (1260, 565),
(1260, 455)],
color=(0.05, 0.05, 0.05, 1),
stroke=0)
self.resizePolygon(self.position_bar)
self.tgt_window = Polygon(v=[(0, 0), (0, 0), (0, 0), (0, 0)],
color=(0, 0.6, 0.2, 1),
stroke=0)
self.resizePolygon(self.tgt_window)
self.pos_fdbk = Polygon(v=[(20, 405), (20, 615), (28, 615), (28, 405)],
color=(0, 0.2, 1.0, 1),
stroke=0)
self.resizePolygon(self.pos_fdbk)
self.pos_fdbk_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=1140,
y=40)
self.combo_wait_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(255, 255, 255, 255),
width=250,
bold=True,
x=478,
y=840)
self.score_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=36,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=500,
y=240)
self.reward_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=22,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=475,
y=120)
self.reset_score()
self.schedule_interval(self.update, 0.01)
class Display(ColorLayer):
is_event_handler = True
def __init__(self):
super(Display, self).__init__(180, 180, 180, 255)
self.mod = Dragonfly_Module(0, 0)
self.mod.ConnectToMMM()
self.mod.Subscribe(rc.MT_TASK_STATE_CONFIG)
self.mod.Subscribe(rc.MT_INPUT_DOF_DATA)
self.mod.Subscribe(rc.MT_COMBO_WAIT)
self.mod.Subscribe(rc.MT_TRIAL_CONFIG)
self.mod.Subscribe(rc.MT_END_TASK_STATE)
self.mod.Subscribe(rc.MT_PING)
self.mod.Subscribe(rc.MT_RT_POSITION_FEEDBACK)
self.msg = CMessage()
self.timer_sec = 0
self.timer_min = 0
self.transformationType = 0
dims = director.get_window_size()
self.width = dims[0]
self.height = dims[1]
self.oldWidth = 1280
self.oldHeight = 1024
self.blank_display = False
self.position_bar = Polygon(v=[(20, 455), (20, 565), (1260, 565),
(1260, 455)],
color=(0.05, 0.05, 0.05, 1),
stroke=0)
self.resizePolygon(self.position_bar)
self.tgt_window = Polygon(v=[(0, 0), (0, 0), (0, 0), (0, 0)],
color=(0, 0.6, 0.2, 1),
stroke=0)
self.resizePolygon(self.tgt_window)
self.pos_fdbk = Polygon(v=[(20, 405), (20, 615), (28, 615), (28, 405)],
color=(0, 0.2, 1.0, 1),
stroke=0)
self.resizePolygon(self.pos_fdbk)
self.pos_fdbk_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=1140,
y=40)
self.combo_wait_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=32,
color=(255, 255, 255, 255),
width=250,
bold=True,
x=478,
y=840)
self.score_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=36,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=500,
y=240)
self.reward_txt = pyglet.text.Label('',
font_name='times new roman',
font_size=22,
color=(0, 0, 0, 255),
width=250,
bold=True,
x=475,
y=120)
self.reset_score()
self.schedule_interval(self.update, 0.01)
# def timer_count_down(self, dt):
# self.timer_sec -= 1
#
# if self.timer_sec < 0:
# if self.timer_min > 0:
# self.timer_min -= 1
# self.timer_sec = 59
# else:
# self.unschedule(self.timer_count_down)
# self.combo_wait_txt.text = ''
# self.screen_on()
# return
#
# self.combo_wait_txt.text = 'Relax Time %d:%02d' % (self.timer_min, self.timer_sec)
#
# if self.timer_min == 0 and self.timer_sec <= 5 and self.timer_sec > 0:
# winsound.PlaySound(os.path.join(os.environ.get('ROBOT_CONFIG'), 'default', 'gocue.wav'), winsound.SND_FILENAME | winsound.SND_ASYNC)
def resizePolygon(self, polygon):
for i in xrange(4):
polygon.v[i] = (polygon.v[i][0] * self.width / self.oldWidth,
polygon.v[i][1] * self.height / self.oldHeight)
def reset_score(self):
self.score = 0
self.score_force_level = 0
self.score_target_dist = 999
self.score_force_mult = 0
self.score_target_mult = 0
self.score_txt.text = "Score: %d" % self.score
self.reward_txt.text = ""
def update_reward(self):
reward = self.score_force_mult * self.score_target_mult
pts = "points"
if reward == 1:
pts = "point"
self.reward_txt.text = "Current Reward: %d %s" % (reward, pts)
def increment_score(self):
self.score += 1 #self.score_force_mult * self.score_target_mult
self.score_txt.text = "Score: %d" % self.score
def screen_off(self):
self.color = (0, 0, 0)
self.blank_display = True
def screen_on(self):
self.blank_display = False
self.color = (180, 180, 180)
def update(self, dt):
while True:
rcv = self.mod.ReadMessage(self.msg, 0)
if rcv == 1:
hdr = self.msg.GetHeader()
msg_type = hdr.msg_type
if msg_type == rc.MT_PING:
self.reset_score()
elif msg_type == rc.MT_INPUT_DOF_DATA:
mdf = rc.MDF_INPUT_DOF_DATA()
copy_from_msg(mdf, self.msg)
if mdf.tag == 'carduinoIO':
fdbk = 5 - mdf.dof_vals[
7] # invert to match phyiscal setup
x_pos = int((fdbk * (MAX_WIDTH - 2 * OFFSET)) / 5.0)
x_pos += 20
self.pos_fdbk_txt.text = "%.2f V" % fdbk
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
# if msg_type == rc.MT_FORCE_SENSOR_DATA:
# mdf = rc.MDF_FORCE_SENSOR_DATA()
# copy_from_msg(mdf, self.msg)
#
# x_fdbk = mdf.data[0]
# x_fdbk_width = int((x_fdbk / MAX_FDBK) * MAX_WIDTH)
elif msg_type == rc.MT_RT_POSITION_FEEDBACK: # updates real time position of handle on screen receives messages from cube_sphere while loop
mdf = rc.MDF_RT_POSITION_FEEDBACK()
copy_from_msg(mdf, self.msg)
x_pos = mdf.distanceFromCenter
x_pos += 20
self.pos_fdbk.v[0] = (x_pos, 405)
self.pos_fdbk.v[1] = (x_pos, 615)
self.pos_fdbk.v[2] = (x_pos + 8, 615)
self.pos_fdbk.v[3] = (x_pos + 8, 405)
self.resizePolygon(self.pos_fdbk)
self.transformPolygon(self.pos_fdbk,
self.transformationType)
elif msg_type == rc.MT_COMBO_WAIT:
mdf = rc.MDF_COMBO_WAIT()
copy_from_msg(mdf, self.msg)
print mdf.duration
duration = mdf.duration / 1000 # convert to seconds
self.timer_sec = duration % 60
self.timer_min = duration / 60
self.screen_off()
self.schedule_interval(self.timer_count_down, 1)
elif msg_type == rc.MT_TRIAL_CONFIG:
self.unschedule(self.timer_count_down)
self.combo_wait_txt.text = ''
self.screen_on()
elif msg_type == rc.MT_END_TASK_STATE:
mdf = rc.MDF_END_TASK_STATE()
#copy_from_msg(mdf, self.msg)
read_msg_data(mdf, self.msg)
print mdf.id, mdf.outcome
if (mdf.id == REWARD_TS) and (mdf.outcome == 1):
self.increment_score()
if (mdf.id in [2, 3, 4]) and (mdf.outcome == 0):
print "screen off"
self.screen_off()
elif msg_type == rc.MT_TASK_STATE_CONFIG:
mdf = rc.MDF_TASK_STATE_CONFIG()
copy_from_msg(mdf, self.msg)
if mdf.background_color == 'gray':
self.color = (180, 180, 180)
elif mdf.background_color == 'red':
self.color = (150, 12, 12)
elif mdf.background_color == 'green':
self.color = (0, 150, 50)
if mdf.fdbk_display_color == 'gray':
self.tgt_window.color = (0.3, 0.3, 0.3, 1)
elif mdf.fdbk_display_color == 'yellow':
self.increment_score()
self.tgt_window.color = (0.5, 0.5, 0.0, 1)
elif mdf.fdbk_display_color == 'green':
self.tgt_window.color = (0.0, 0.6, 0.2, 1)
elif mdf.fdbk_display_color == 'red':
self.tgt_window.color = (0.6, 0.05, 0.05, 1)
if not math.isnan(
mdf.direction) and mdf.direction in range(
-1, 3
) and not mdf.direction == self.transformationType:
self.position_bar.v = [(20, 455), (20, 565),
(1260, 565), (1260, 455)]
self.resizePolygon(self.position_bar)
self.transformPolygon(self.position_bar, mdf.direction)
self.transformPolygon(self.pos_fdbk, mdf.direction)
self.transformationType = mdf.direction
if not (math.isnan(mdf.target[0])) and not (math.isnan(
mdf.target[1])):
x_tgt_lo = mdf.target[0] + 20
x_tgt_hi = mdf.target[1] + 20
self.tgt_window.v[0] = (x_tgt_lo, 430)
self.tgt_window.v[1] = (x_tgt_lo, 590)
self.tgt_window.v[2] = (x_tgt_hi, 590)
self.tgt_window.v[3] = (x_tgt_hi, 430)
self.resizePolygon(self.tgt_window)
self.transformPolygon(self.tgt_window,
self.transformationType)
else:
break
def transformPolygon(self, polygon, transformationType):
for i in xrange(4):
polygon.v[i] = self.transformPoint(polygon.v[i],
transformationType)
def transformPoint(self, input_point, transformationType):
# tt values are determined by adding XorYorZ and UpOrDown
if transformationType == 0: # goes from left to right XorYorZ=1 UpOrDown=-1
return input_point
elif transformationType == 2: # goes from right to left XorYorZ=1 UpOrDown=1
return (self.width - input_point[0], input_point[1])
elif transformationType == -1: # backward XorYorZ=0 UpOrDown=-1
return (input_point[1], self.width - input_point[0])
elif transformationType == 1: # forwards XorYorZ=0 UpOrDown=1
return (input_point[1], input_point[0])
else:
print "unknown transform type"
#def on_key_release( self, keys, mod ):
def draw(self):
super(Display, self).draw()
if not self.blank_display:
self.position_bar.render()
self.tgt_window.render()
self.pos_fdbk.render()
#self.pos_fdbk_txt.draw()
self.score_txt.draw()
self.reward_txt.draw()
self.combo_wait_txt.draw()
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0)
if rcv == 1:
self.process_message(msg)
def process_msg(self, in_msg):
header = in_msg.GetHeader()
if header.msg_type == rc.MT_FT_DATA:
mdf = rc.MDF_FT_DATA()
copy_from_msg(mdf, in_msg)
rate(self.rate)
self.ball.pos = vector(mdf.F[0:3])
self.shadow_cursor.pos = vector(
[mdf.F[0], -self.length / 2, mdf.F[2]])
self.unit_target = np.array(self.target_vector) / np.linalg.norm(
self.target_vector)
self.target_position = np.array(
self.unit_target) * self.max_factor * self.force_scale
self.target.pos = self.target_position
self.shadow_target.pos = [
self.target_position[0], -self.length / 2,
self.target_position[2]
]
distance = [a - b for a, b in zip(self.ball.pos, self.target.pos)]
if (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) >= self.threshold and self.RTFT_display:
self.ball.color = self.ball_color
self.state = 0
elif (distance[0]**2 + distance[1]**2 + distance[2]**2)**(
1 / 2.) < self.threshold and self.RTFT_display:
if self.state == 0: # if previous sample was outside radius, and now we're inside...
self.start_hold = time.time()
self.state = 1
self.ball.color = color.orange
else:
if time.time() > (self.start_hold + self.hold_time):
self.ball.color = color.green
self.target.visible = False
self.shadow_target.visible = False
self.state = 2
out_mdf = rc.MDF_FT_COMPLETE()
out_mdf.FT_COMPLETE = self.state
out_mdf.sample_header = mdf.sample_header
msg = CMessage(rc.MT_FT_COMPLETE)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
else:
self.state = 1
self.ball.color = color.orange
else:
self.state = -1
if self.state == 2 and self.solo: #if no executive file
self.target.pos = [
float(x) for x in [
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale,
np.random.rand(1, 1) * self.max_factor *
self.force_scale
]
]
self.shadow_target.pos = [
self.target.pos[0], -self.length / 2, self.target.pos[2]
]
sys.stdout.write(
"%7.4f, %5d, %16.2f\n" %
(mdf.F[2], self.state,
(self.start_hold + self.hold_time) - time.time()))
#msg_str = "%7.4f " * 6 + "\n"
#sys.stdout.write(msg_str % (mdf.F[0], mdf.F[1], mdf.F[2],
# mdf.T[0], mdf.T[1], mdf.T[2]))
sys.stdout.flush()
elif header.msg_type == rc.MT_RTFT_CONFIG:
mdf = rc.MDF_RTFT_CONFIG()
copy_from_msg(mdf, in_msg)
self.max_factor = mdf.max_factor
self.RTFT_display = mdf.RTFT_display
self.target_vector = mdf.target_vector[:]
self.ball.visible = mdf.cursor_visible
self.target.visible = mdf.target_visible
self.shadow_target.visible = mdf.shadow_target_visible
self.shadow_cursor.visible = mdf.shadow_cursor_visible
self.ball_color = [1, 0, 0]
self.solo = False
def run(self):
while True:
in_msg = CMessage()
rcv = self.mod.ReadMessage(in_msg, 0.1)
if rcv == 1:
msg_type = in_msg.GetHeader().msg_type
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id
== self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, in_msg, 'Metronome')
elif msg_type == rc.MT_MNOME_STATE:
print 'got message'
in_mdf = rc.MDF_MNOME_STATE()
copy_from_msg(in_mdf, in_msg)
if in_mdf.state == 0:
print 'got stop'
self.pause_state = True
self.count = 0
elif in_mdf.state == 1:
print 'got start'
self.pause_state = False
self.count = 0
elif in_mdf.state == 2:
print 'got pause'
self.pause_state = True
self.count = 0
elif msg_type == self.in_msg_num:
if self.pause_state:
pass
else:
self.count += 1
if self.pretrigger_time > 0:
if self.count == self.metronome_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
if self.count == self.trigger_out_count:
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()
else:
if self.count == self.trigger_out_count:
in_mdf = eval('rc.MDF_%s()' %
(self.in_msg_type.upper()))
copy_from_msg(in_mdf, in_msg)
out_mdf = rc.MDF_TMS_TRIGGER()
out_mdf.sample_header = in_mdf.sample_header
out_msg = CMessage(rc.MT_TMS_TRIGGER)
copy_to_msg(out_mdf, out_msg)
self.mod.SendMessage(out_msg)
if self.count == self.metronome_count:
self.count = 0 - int(
np.random.uniform(0, 1.5, 1)[0] *
self.in_msg_freq)
sound_thread = threading.Thread(
target=self.play_sound)
sound_thread.start()