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 process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type if msg_type == rc.MT_POLARIS_POSITION: # 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[:])) # np.testing.assert_array_equal(positions[:,0], orientations[:,0], err_msg='Samples are not aligned') if self.calibrated: if in_mdf.tool_id == (self.marker + 1): # calculating output self.Qk = qa.norm( orientations ) # need to find a way to discriminate the tools files in the messages??? Qr = qa.mult(self.Qk, qa.inv(self.Qi)).flatten() Tk = positions hotspot_position = (qa.rotate(Qr, self.Xi) + Tk).flatten() hotspot_vector_head = qa.rotate(Qr, plate_vector) if np.any(np.isnan(hotspot_position)) == True: print "x", # print ' *****nan present, check coil is within frame!*****' # creating output message out_mdf = rc.MDF_HOTSPOT_POSITION() out_mdf.xyz[:] = hotspot_position out_mdf.ori[:3] = hotspot_vector_head # Qk - coil active orientation out_mdf.sample_header = in_mdf.sample_header msg = CMessage(rc.MT_HOTSPOT_POSITION) copy_to_msg(out_mdf, msg) self.mod.SendMessage(msg) sys.stdout.write("o") else: if np.any(np.isnan(positions)) == True: raise Exception, "nan present" if np.any(np.isnan(orientations)) == True: raise Exception, "nan present" if ( (self.store_plate >= self.store_plate_pos.shape[0]) & (self.store_plate >= self.store_plate_ori.shape[0]) & (self.store_coil >= self.store_coil_pos.shape[0]) & (self.store_coil >= self.store_coil_ori.shape[0]) ): self.calibrating = False self.make_calibration_vector() elif in_mdf.tool_id == (self.marker + 1): self.store_coil_pos[self.store_coil, :] = positions self.store_coil_ori[self.store_coil, :] = orientations self.store_coil += 1 elif in_mdf.tool_id == (self.plate + 1): self.store_plate_pos[self.store_plate, :] = positions self.store_plate_ori[self.store_plate, :] = orientations self.store_plate += 1
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_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 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_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 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 respond_to_ping(mod, msg, module_name): dest_mod_id = msg.GetHeader().dest_mod_id p = rc.MDF_PING() copy_from_msg(p, msg) # print "PING received for '{0}'".format(p.module_name) if (p.module_name.lower() == module_name.lower()) or (p.module_name == "*") or (dest_mod_id == mod.GetModuleID()): mdf = rc.MDF_PING_ACK() mdf.module_name = module_name msg_out = CMessage(rc.MT_PING_ACK) copy_to_msg(mdf, msg_out) mod.SendMessage(msg_out)
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 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 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")
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()
MID_REPLY = 10 # Note: Reply must be started first if __name__ == "__main__": mod = PyDragonfly.Dragonfly_Module(MID_REPLY, 0) mod.ConnectToMMM() mod.Subscribe(mdefs.MT_REQUEST_TEST_DATA) print("Reply running...\n") run = True while run: in_msg = PyDragonfly.CMessage() print("Waiting for message") rcv = mod.ReadMessage(in_msg, 1.0) if rcv == 1: print("Received message", in_msg.GetHeader().msg_type) out_msg = PyDragonfly.CMessage(mdefs.MT_TEST_DATA) data = mdefs.MDF_TEST_DATA() if in_msg.GetHeader().msg_type == mdefs.MT_REQUEST_TEST_DATA: data.a = -20 data.b = 47 data.x = 123.456 copy_to_msg(data, out_msg) mod.SendMessage(out_msg) print("Sent message", out_msg.GetHeader().msg_type) elif in_msg.GetHeader().msg_type in (MT_EXIT, MT_KILL): run = False
import PyDragonfly from PyDragonfly import copy_to_msg import message_defs as md import sys MID_PRODUCER = 10 if __name__ == "__main__": mod = PyDragonfly.Dragonfly_Module(MID_PRODUCER, 0) mod.ConnectToMMM("localhost:7111") print "Producer running...\n" a = 0 run = True while run: out_msg = PyDragonfly.CMessage(md.MT_TEST_DATA) data = md.MDF_TEST_DATA() data.a = a data.b = -3 data.x = 1.234 copy_to_msg(data, out_msg) mod.SendMessage(out_msg) print "Sent message ", out_msg.GetHeader().msg_type a += 1 time.sleep(1)
def find_hotspot_to_cz( self, HS_GCS, glasses_orientation, glasses_position, coil_vector): #Saves 0, 0, 0 array to file when object called check_HS_GCS = self.check(HS_GCS) check_glasses_orientation = self.check(glasses_orientation) check_glasses_position = self.check(glasses_position) check_coil_vector = self.check(coil_vector) if np.any( np.array([ check_HS_GCS, check_glasses_orientation, check_glasses_position, check_coil_vector ])): print np.array([ check_HS_GCS, check_glasses_orientation, check_glasses_position, check_coil_vector ]) HS_LCS = (np.zeros(3) * np.NaN)[None] print HS_LCS vector_LCS = (np.zeros(3) * np.NaN)[None] with open(self.location, 'a') as location: np.savetxt(location, np.array(HS_LCS), fmt='%f', delimiter=',', newline='\r\n') location.close() with open(self.vector, 'a') as data: np.savetxt(data, np.array(vector_LCS), fmt='%f', delimiter=',', newline='\r\n') data.close() sys.stdout.write("No position\n") else: cz_pos, cz_rot = self.glasses_to_cz.get_pos( glasses_orientation, glasses_position) LT_pos, LT_rot = self.glasses_to_LT.get_pos( glasses_orientation, glasses_position) RT_pos, RT_rot = self.glasses_to_RT.get_pos( glasses_orientation, glasses_position) Nasion_pos, Nasion_rot = self.glasses_to_Nasion.get_pos( glasses_orientation, glasses_position) calibration_matrix = self.make_head_axis(LT_pos, RT_pos, Nasion_pos) HS_LCS = qa.rotate(calibration_matrix, (HS_GCS - cz_pos)) vector_LCS = qa.rotate(calibration_matrix, coil_vector) #HS_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (HS_GCS - cz_pos))) #vector_LCS = qa.rotate(rot, qa.rotate(self.calibration_matrix, (coil_vector))) with open(self.location, 'a') as location: np.savetxt(location, np.array(HS_LCS), fmt='%f', delimiter=',', newline='\r\n') location.close() with open(self.vector, 'a') as data: np.savetxt(data, np.array(vector_LCS), fmt='%f', delimiter=',', newline='\r\n') data.close() self.stimulus_no += 1 print self.stimulus_no in_mdf = rc.MDF_POLARIS_POSITION() out_mdf = rc.MDF_PLOT_POSITION() out_mdf.xyz[:] = HS_LCS[0] out_mdf.ori[:] = np.append(vector_LCS[0], 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")