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_HOTSPOT_POSITION: #rc.MT_RAW_SPIKECOUNT: #rc.MT_SPM_SPIKECOUNT: mdf = rc.MDF_HOTSPOT_POSITION() copy_from_msg(mdf, msg) self.coil_tail = np.array(in_mdf.xyz[:]) self.coil_head = np.array(in_mdf.ori[:3]) self.update_judging_data() elif msg_type == rc.MT_PING: respond_to_ping(self.mod, msg, 'PolarisDragonfly') #elif msg_type == MT_EXIT: # self.exit() # done = True else: done = True self.update_plot()
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 process_message(self, msg): # 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, 'PlotHead') elif msg_type == rc.MT_POLARIS_POSITION: in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, msg) positions = np.asarray(in_mdf.xyz[:]) orientations = self.shuffle_q(np.asarray(in_mdf.ori[:])) if in_mdf.tool_id == (self.pointer + 1): Qf = qa.norm(orientations) Qr = qa.mult(Qf, qa.inv(self.pointer_Qi)).flatten() #find_nans(self.store_head, Qr, 'Qr') Tk = positions #find_nans(self.store_head, Tk, 'Tk') tip_pos = (qa.rotate(Qr, self.pointer_Xi) + Tk).flatten() self.pointer_position = np.append(self.pointer_position, (tip_pos[np.newaxis, :]), axis=0) #self.pl.reset(x=self.pointer_position[:,0], y=self.pointer_position[:,1], z=self.pointer_position[:,2]) print("old=", tip_pos) print("new=", self.tp.get_pos(orientations, positions)[0])
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 process_message(self, msg): msg_type = msg.GetHeader().msg_type if msg_type == rc.MT_INPUT_DOF_DATA: mdf = rc.MDF_INPUT_DOF_DATA() copy_from_msg(mdf, msg) tag = mdf.tag if tag in self.inputs.keys(): dof_vals = np.asarray(mdf.dof_vals[:], dtype=float) cid = int(mdf.tag[-1]) pressed = ~self.was_pressed[cid] & (dof_vals > btn_threshold) started = self.bounce_start[cid] > 0 # start timers on previously unstarted counters self.bounce_start[cid, pressed & ~started] = time.time() dt = time.time() - self.bounce_start[cid] held = dt > bounce_threshold valid_held = pressed & held for vh in np.flatnonzero(valid_held): if vh in name_lookup.keys(): self.was_pressed[cid, vh] = True self.send_btn_press(name_lookup[vh], cid) released = self.was_pressed[cid] & (dof_vals < btn_threshold) valid_released = released & held & ~valid_held for vr in np.flatnonzero(valid_released): if vr in name_lookup.keys(): self.was_pressed[cid, vr] = False self.send_btn_release(name_lookup[vr], cid) self.bounce_start[cid, vr] = -1
def get_frequency(self): # loop over receiving messages until we get a POLARIS_POSITION message 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, "PolarisDragonfly") 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 run(self): while True: msg = PyDragonfly.CMessage() self.mod.ReadMessage(msg) # blocking read print "Received message ", msg.GetHeader().msg_type if msg.GetHeader().msg_type == rc.MT_UR5_MOVEMENT_COMMAND: msg_data = rc.MDF_UR5_MOVEMENT_COMMAND() copy_from_msg(msg_data, msg) #position = np.frombuffer(msg_data.position) #print " Data = [X: %d, Y: %d, Z: %f]" % \ # (msg_data.position[0], msg_data.position[1], msg_data.position[2]) movement_complete = self.ur5.send_movement_command( msg_data.position, msg_data.max_velocity, msg_data.acceleration) if movement_complete: # send movement complete message self.mod.SendSignal(rc.MT_UR5_MOVEMENT_COMPLETE) else: # print ur5 error message print "No movement complete received." elif msg.GetHeader().msg_type == rc.MT_UR5_REQUEST_CONNECTED: if self.ur5.connected: self.mod.SendSignal(rc.MT_UR5_CONNECTED) # ideally we'd capture not connected, but not today... elif msg.GetHeader().msg_type == rc.MT_PING: respond_to_ping(self.mod, msg, 'UR5Control')
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 process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type #print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing))) if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) if in_mdf.tool_id == (self.glasses + 1): positions = np.array(in_mdf.xyz[:]) orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) self.find_pos_to_glasses(positions, orientations)
def process_message(self, in_msg): # read a Dragonfly message 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 = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) if in_mdf.tool_id == (self.pointer + 1): pointer_pos, Qr = self.pointer_tp.get_pos( orientations, positions) print pointer_pos
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 calibrate_head(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.asarray(in_mdf.xyz[:]) orientations = self.shuffle_q(np.asarray(in_mdf.ori[:])) #When arrays have been filled the calibration vector is generated if ( (self.store_glasses >= self.store_glasses_pos.shape[0]) & (self.store_glasses >= self.store_glasses_ori.shape[0]) & (self.store_head >= self.store_head_pos.shape[0]) & (self.store_head >= self.store_head_ori.shape[0]) ): self.calibrating = False self.make_calibration_vector() #Pointer is measured from ball 1, pointer end must be calculated elif in_mdf.tool_id == (self.pointer + 1): if self.store_head < self.store_head_pos.shape[0]: if np.any(np.isnan(positions)) == True: raise Exception, 'nan present' elif np.any(np.isnan(orientations)) == True: raise Exception, 'nan present' Qf = qa.norm(orientations) #Sometimes gets nan, source unknown if np.any(np.isnan(Qf)): print(self.store_head, 'Qf', orientations) #find_nans(self.store_head, Tk, 'Tk') Cz_pos, Qr = self.pointer_tp.get_pos(orientations, positions) #find_nans(self.store_head, Cz_pos, 'Cz') self.store_head_pos[self.store_head, :] = Cz_pos self.store_head_ori[self.store_head, :] = orientations self.store_head += 1 elif in_mdf.tool_id == (self.glasses + 1): if self.store_glasses < self.store_glasses_pos.shape[0]: if np.any(np.isnan(positions)) == True: raise Exception, 'nan present' if np.any(np.isnan(orientations)) == True: raise Exception, 'nan present' self.store_glasses_pos[self.store_glasses, :] = positions self.store_glasses_ori[self.store_glasses, :] = orientations self.store_glasses += 1
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 process_message(self, msg): msg_type = msg.GetHeader().msg_type dest_mod_id = msg.GetHeader().dest_mod_id if msg_type == rc.MT_TMS_TRIGGER: self.ext_trig.run() self.TMS_trigger = True else: # if it is a NiDAQ message from channels 0-7, plot the data #self.counter += 1 if msg_type == rc.MT_DAQ_DATA: #sys.stdout.write("*") #sys.stdout.flush() mdf = rc.MDF_DAQ_DATA() copy_from_msg(mdf, msg) # add data to data buffers (necessary, or just use graphics buffers?) # update plots to new data buffers buf = mdf.buffer self.new_data[:,:-self.config.perchan] = self.old_data[:,self.config.perchan:] for i in xrange(self.config.nchan): #if i == 0: # print mdf.buffer[perchan * i:perchan * (i + 1)].size self.new_data[i, -self.config.perchan:] = buf[i:self.config.nchan * self.config.perchan:self.config.nchan] self.old_data[:] = self.new_data[:] if self.parent.current_tab == 'Collect': if self.TMS_trigger: if self.config.pre_trig_samp <= np.argmax(self.old_data[self.config.trig_chan, :] >= 3) <= (self.config.pre_trig_samp)+200: self.trig_index = np.argmax(self.old_data[self.config.trig_chan, :] >= 3) self.collect_data = self.old_data[:self.config.nemg, self.trig_index - self.config.pre_trig_samp:self.trig_index + self.npt - self.config.pre_trig_samp] self.old_data = self.old_data * 0 self.new_data = self.new_data * 0 self.new_collect_data = True self.TMS_trigger = False if self.parent.current_tab == 'Hotspot': if self.config.pre_trig_samp <= np.argmax(self.old_data[self.config.trig_chan, :] >= 3) <= self.config.pre_trig_samp+200: self.trig_index = np.argmax(self.old_data[self.config.trig_chan, :] >= 3) self.hotspot_data = self.old_data[:self.config.nemg, self.trig_index - self.config.pre_trig_samp:self.trig_index + self.npt - self.config.pre_trig_samp] self.new_hotspot_data = True self.old_data = self.old_data * 0 self.new_data = self.new_data * 0
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 process_message(self, msg): # 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, 'FastDisplay') else: # if it is a NiDAQ message from channels 0-7, plot the data #self.counter += 1 if msg_type == rc.MT_TRIGNO_DATA: sys.stdout.write("*") #sys.stdout.flush() mdf = rc.MDF_TRIGNO_DATA() copy_from_msg(mdf, msg) # add data to data buffers (necessary, or just use graphics buffers?) # update plots to new data buffers buf = mdf.T elif msg_type == rc.MT_SAMPLE_GENERATED: sys.stdout.write("#") sys.stdout.flush() buf = np.random.normal(1, 1, size=(432,)) else: return False self.new_data[:,:-self.config.perchan] = self.old_data[:,self.config.perchan:] print (buf[0:2]) for i in xrange(self.config.nchan): #if i == 0: # print mdf.buffer[perchan * i:perchan * (i + 1)].size self.new_data[i, -self.config.perchan:] = buf[i:self.config.nchan * self.config.perchan:self.config.nchan] self.axes.flat[i].setData(self.new_data[i]) #sys.stdout.write("*") #sys.stdout.flush() self.old_data[:] = self.new_data[:]
def process_message(self, msg): # 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, 'PlotHead') elif msg_type == rc.MT_PLOT_POSITION: in_mdf = rc.MDF_PLOT_POSITION() copy_from_msg(in_mdf, msg) tail = np.array(in_mdf.xyz[:])*0.127 + (self.plot_vertex_vec)#Hotspot position head = np.array(in_mdf.ori[:3])/4 #Vector head of coil, used to find ori if np.any(np.isnan(tail)) == True: pass elif np.any(np.isnan(head)) == True: pass elif np.any(np.isinf(tail)) == True: pass elif np.any(np.isinf(head)) == True: pass else: queue.put(np.vstack((head, tail))) self.count=+1 print 'sent message' elif msg_type == rc.MT_MNOME_STATE: in_mdf = rc.MDF_MNOME_STATE() copy_from_msg(in_mdf, msg) if in_mdf.state == 0: print 'got clear' self.parent.reset = True
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 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 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 listener(self): PACKET_LENGTH = md.HX_EMG_SAMPLES_PER_MSG mmDiff = np.memmap(self.data_diff_path, dtype='float32', mode='r', shape=(NUM_CHANS, BUFFER_LENGTH)) mmDiffTime = np.memmap(self.data_diffTime_path, dtype='uint32', mode='r', shape=(BUFFER_LENGTH)) while True: msg = PyDragonfly.CMessage() self.mod.ReadMessage(msg, 0) if msg.GetHeader().msg_type == md.MT_SHUT_DOWN: msg_data = md.MDF_SHUT_DOWN() self.mod.DisconnectFromMMM() print("Emergency stop; Dragonfly thread closed") elif msg.GetHeader().msg_type == md.MT_OL_SPINALLEAD_STIMCONFIG: msg_data = md.MDF_OL_SPINALLEAD_STIMCONFIG() copy_from_msg(msg_data, msg) acratio = msg_data.acratio elecID = np.array(msg_data.elecID[:]) elec = np.array(msg_data.elec[:]) amp = np.array(msg_data.amp[:]) freq = np.array(msg_data.freq[:]) pw = np.array(msg_data.width[:]) dur = np.array(msg_data.dur[:]) stimParams = list( np.concatenate((elecID, elec, amp, freq, pw, dur))) stimParams = tuple(stimParams) self.currentStim = stimParams if stimParams not in self.stimDict.keys(): self.stimDict[stimParams] = np.zeros((17, 1)) if stimParams not in self.stimMeanPulse.keys(): self.stimMeanPulse[stimParams] = np.zeros( (16, int(1.5 * FS))) ## update labels (veUnique, veUniqueIDX) = np.unique(elecID, return_index=True) elecString = "" ampString = "" freqString = "" pwString = "" durString = "" for i in range(0, len(veUniqueIDX)): veIDX = veUniqueIDX[i] veID = elecID[veIDX] if veID > 0: veElec = elec[np.where(elecID == veID)] veAmp = amp[np.where(elecID == veID)] veFreq = freq[veIDX] vePW = pw[veIDX] veDur = dur[np.where(elecID == veID)] elecString = elecString + str(veID) + ": " + str( veElec) + "\n" ampString = ampString + str(veAmp) + "\n" freqString = freqString + str(veFreq) + "\n" pwString = pwString + str(vePW) + "\n" durString = durString + str(veDur) + "\n" Clock.schedule_once(lambda dt: self.update_labels( elecString, ampString, freqString, pwString, durString)) elif msg.GetHeader().msg_type == md.MT_STIM_TIMES: msg_data = md.MDF_STIM_TIMES() copy_from_msg(msg_data, msg) stimTimes = np.array(msg_data.stimTimes[:]) stimTimes = stimTimes[np.where(stimTimes != 99)] msg_data = md.MDF_STIM_TIMES() copy_from_msg(msg_data, msg) stimTimes = np.array(msg_data.stimTimes[:]) stimTimes = stimTimes[np.where(stimTimes != 99)] time.sleep(1) mmEmgTime = mmDiffTime.copy() emgData = mmDiff.copy() b, a = signal.butter(4, 0.1, 'highpass') filtEMG = signal.filtfilt(b, a, emgData[0:16, :], axis=1) rms_window = self.get_root_window( ).children[-1].ids['rmsWindow'].value / 1000.0 emgRMS = self.stimDict[self.currentStim] emgMean = self.stimMeanPulse[self.currentStim] self.currentPulse = np.zeros( (NUM_PLOT_PULSES, 16, int(1.5 * FS))) for s_idx in range(0, len(stimTimes)): stimTime = stimTimes[s_idx] emgTime = np.argmin(np.abs(mmEmgTime - stimTime)).astype(int) emgStart = int(max(0, emgTime - (FS * rms_window))) emgEnd = int( min(BUFFER_LENGTH, emgTime + (FS * rms_window))) emgPreWindow = filtEMG[:, emgStart:emgTime] emgPostWindow = filtEMG[:, emgTime:emgEnd] pulsePreRMS = np.reshape( np.sqrt(np.mean(np.square(emgPreWindow), 1)), (16, 1)) pulsePostRMS = np.reshape( np.sqrt(np.mean(np.square(emgPostWindow), 1)), (16, 1)) pulseRMS = pulsePostRMS - pulsePreRMS ## multiply running average by previous number of pulses, add current pulse, divide by num_pulses + 1 emgRMS[1:] = ( (emgRMS[1:] * emgRMS[0]) + pulseRMS) / (emgRMS[0] + 1) pulseStart = int(max(emgTime - 0.5 * FS, 0)) pulseEnd = int(min(emgTime + FS, BUFFER_LENGTH)) emgPulse = np.zeros((16, int(1.5 * FS))) emgPulse[:, :pulseEnd - pulseStart] = np.array( filtEMG[:, pulseStart:pulseEnd]) emgMean = ( (emgMean * emgRMS[0]) + emgPulse) / (emgRMS[0] + 1) emgRMS[0] = emgRMS[0] + 1 if s_idx < NUM_PLOT_PULSES: self.currentPulse[s_idx, :, :] = emgPulse self.stimDict[self.currentStim] = emgRMS self.stimMeanPulse[self.currentStim] = emgMean recGraph = self.get_root_window().children[-1].ids['recGraph'] recGraph.set_data(self.stimDict, self.stimMeanPulse, self.currentStim, self.currentPulse) Clock.schedule_once(lambda dt: recGraph.update_plots(), 0.1) time.sleep(0.001)
def process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type if not msg_type in self.msg_nums: return # SESSION_CONFIG => start of session if msg_type == rc.MT_SESSION_CONFIG: self.num_trials = 0 self.reset_counters() # EM_DECODER_CONFIGURATION => end of an adaptation round elif msg_type == rc.MT_EM_DECODER_CONFIGURATION: self.reset_counters() # END_TASK_STATE => end of a task elif msg_type == rc.MT_END_TASK_STATE: mdf = rc.MDF_END_TASK_STATE() copy_from_msg(mdf, in_msg) # need to know: # begin task state code # final task state code # intertrial state code if mdf.id == 1: self.trial_sync = 1 self.shadow_started_window.append(0) if (mdf.id == self.task_state_codes["begin"]) & (mdf.outcome == 1): if self.trial_sync: # print "*** trial started ***" # self.rewards_given += 1 self.shadow_num_trial_started_postcalib += 1 self.shadow_success_window.append(0) self.shadow_givenup_window.append(0) self.shadow_started_window[-1] = 1 if mdf.reason == "JV_IDLE_TIMEOUT": if self.trial_sync: self.shadow_num_trial_givenup_postcalib += 1 self.shadow_givenup_window[-1] = 1 if (mdf.id == self.task_state_codes["final"]) & (mdf.outcome == 1): if self.trial_sync: # print "*** trial complete and successful" self.shadow_num_trial_successful_postcalib += 1 self.shadow_success_window[-1] = 1 if mdf.id == self.task_state_codes["intertrial"]: if self.trial_sync: # do end-of-trial stuff here self.num_trials += 1 self.num_trials_postcalib += 1 self.num_trial_started_postcalib = self.shadow_num_trial_started_postcalib self.num_trial_successful_postcalib = self.shadow_num_trial_successful_postcalib self.num_trial_givenup_postcalib = self.shadow_num_trial_givenup_postcalib if len(self.shadow_success_window) > self.window_len: self.shadow_success_window.pop(0) if len(self.shadow_givenup_window) > self.window_len: self.shadow_givenup_window.pop(0) if len(self.shadow_started_window) > self.window_len: self.shadow_started_window.pop(0) self.success_window = copy.deepcopy(self.shadow_success_window) self.started_window = copy.deepcopy(self.shadow_started_window) self.givenup_window = copy.deepcopy(self.shadow_givenup_window)
def process_message(self, msg): """ Needs to: 1) combine non-conflicting controlledDims e.g. from OPERATOR_MOVEMENT_COMMANDs, into either extrinsic or intrinsic commands 2) combine intrinsic and extrinsic commands into final command """ msg_type = msg.GetHeader().msg_type if msg_type in [ rc.MT_OPERATOR_MOVEMENT_COMMAND, rc.MT_PLANNER_MOVEMENT_COMMAND, rc.MT_EM_MOVEMENT_COMMAND, rc.MT_FIXTURED_MOVEMENT_COMMAND, ]: if msg_type == rc.MT_OPERATOR_MOVEMENT_COMMAND: mdf = rc.MDF_OPERATOR_MOVEMENT_COMMAND() elif msg_type == rc.MT_PLANNER_MOVEMENT_COMMAND: mdf = rc.MDF_PLANNER_MOVEMENT_COMMAND() elif msg_type == rc.MT_EM_MOVEMENT_COMMAND: mdf = rc.MDF_EM_MOVEMENT_COMMAND() elif msg_type == rc.MT_FIXTURED_MOVEMENT_COMMAND: mdf = rc.MDF_FIXTURED_MOVEMENT_COMMAND() # MOVEMENT_COMMAND # ---------------- # controlledDims # pos # sample_header # sample_interval # tag # vel # ---------------- copy_from_msg(mdf, msg) tag = mdf.tag # if not tag in self.accepted_tags: # return dim = np.asarray(mdf.controlledDims, dtype=bool) # .astype(bool) if mdf.tag in self.intrinsic_tags: # intrinsic is AUTO command self.intrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim] # print "intr_vel = " + " ".join(["%5.2f" % (x) for x in self.intrinsic_vel]) elif mdf.tag in self.extrinsic_tags: # print "!" # extrinsic is non-AUTO, i.e. EM, command self.extrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim] # self.extrinsic_vel[:8] *= self.gate if tag == self.timer_tag: self.send_output(mdf.sample_header) elif msg_type == rc.MT_ROBOT_CONTROL_CONFIG: mdf = rc.MDF_ROBOT_CONTROL_CONFIG() copy_from_msg(mdf, msg) self.autoVelControlFraction[:] = mdf.autoVelControlFraction elif msg_type == rc.MT_IDLE: mdf = rc.MDF_IDLE() copy_from_msg(mdf, msg) self.gate = float(np.asarray(mdf.gain, dtype=float).item()) elif msg_type == rc.MT_IDLE_DETECTION_ENDED: self.gate = 1.0 elif msg_type == rc.MT_TASK_STATE_CONFIG: mdf = rc.MDF_TASK_STATE_CONFIG() copy_from_msg(mdf, msg) self.idle_gateable = mdf.idle_gateable
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 emgBuffer(): mm_ip = Global.MM_IP ## Connect to MessageManager module = PyDragonfly.Dragonfly_Module(0, 0) try: module.ConnectToMMM(mm_ip) except: print("Could not connect to Message Manager at : ", mm_ip) print("Connected to Message Manager") PACKET_LENGTH = md.HX_EMG_SAMPLES_PER_MSG FS = Global.FS NUM_CHANS = Global.NUM_CHANS BUFFER_TIME = Global.BUFFER_TIME BUFFER_LENGTH = Global.BUFFER_LENGTH ################################################### ## Set up memory mapped files. ## This is useful because both the EMG plotter and ## recruitment curve shenanigans ################################################### data_diff_path = Global.data_diff_path #os.path.join(base_dir, 'mem_map', 'mmDataDiff.dat') data_diffTime_path = Global.data_diffTime_path #os.path.join(base_dir, 'mem_map', 'mmDataDiffTime.dat') if not os.path.exists(data_diff_path): with open(data_diff_path, 'w') as f: f.write(np.zeros(shape=(NUM_CHANS, BUFFER_LENGTH))) if not os.path.exists(data_diffTime_path): with open(data_diffTime_path, 'w') as f: f.write(np.zeros(shape=(1, BUFFER_LENGTH))) mmDiff = np.memmap(data_diff_path, dtype='float32', mode='w+', shape=(NUM_CHANS, BUFFER_LENGTH)) mmDiffTime = np.memmap(data_diffTime_path, dtype='uint32', mode='w+', shape=(BUFFER_LENGTH)) ## Clear buffers mmDiff[:] = mmDiff[:] * 0 mmDiffTime[:] = mmDiffTime[:] * 0 ## Subscribe to Message Manager Messages module.Subscribe(md.MT_HX_EMG_DATA_DIFF) module.Subscribe(md.MT_SHUT_DOWN) print("Reading Messages") while True: msg = PyDragonfly.CMessage() module.ReadMessage(msg, 0) if msg.GetHeader().msg_type == md.MT_HX_EMG_DATA_DIFF: start = time.time() msg_data = md.MDF_HX_EMG_DATA_DIFF() copy_from_msg(msg_data, msg) mmDiff[:, 0:-PACKET_LENGTH] = mmDiff[:, PACKET_LENGTH:] mmDiff[:, -PACKET_LENGTH:] = np.transpose( np.reshape(msg_data.data, newshape=(PACKET_LENGTH, NUM_CHANS))) mmDiffTime[0:-PACKET_LENGTH] = mmDiffTime[PACKET_LENGTH:] mmDiffTime[-PACKET_LENGTH:] = msg_data.source_timestamp if msg.GetHeader().msg_type == md.MT_SHUT_DOWN: module.DisconnectFromMMM() print("Emergency Stop") print("Dragonfly Thread Closed") break
#!/usr/bin/python import time import PyDragonfly from PyDragonfly import copy_from_msg import message_defs as mdefs import sys MID_CONSUMER = 11 if __name__ == "__main__": mod = PyDragonfly.Dragonfly_Module(MID_CONSUMER, 0) mod.ConnectToMMM("localhost:7111") mod.Subscribe(mdefs.MT_TEST_DATA) print "Consumer running...\n" while (1): msg = PyDragonfly.CMessage() mod.ReadMessage(msg) # blocking read print "Received message ", msg.GetHeader().msg_type if msg.GetHeader().msg_type == mdefs.MT_TEST_DATA: msg_data = mdefs.MDF_TEST_DATA() copy_from_msg(msg_data, msg) print " Data = [a: %d, b: %d, x: %f]" % (msg_data.a, msg_data.b, msg_data.x) mod.DisconnectFromMMM()
def process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type if not msg_type in self.msg_nums: return # SESSION_CONFIG => start of session if msg_type == rc.MT_SESSION_CONFIG: self.num_trials = 0 self.reset_counters() # EM_DECODER_CONFIGURATION => end of an adaptation round elif msg_type == rc.MT_EM_DECODER_CONFIGURATION: self.reset_counters() # END_TASK_STATE => end of a task elif msg_type == rc.MT_END_TASK_STATE: mdf = rc.MDF_END_TASK_STATE() copy_from_msg(mdf, in_msg) # need to know: # begin task state code # final task state code # intertrial state code if (mdf.id == 1): self.trial_sync = 1 self.shadow_started_window.append(0) if (mdf.id == self.task_state_codes['begin']) & (mdf.outcome == 1): if self.trial_sync: #print "*** trial started ***" #self.rewards_given += 1 self.shadow_num_trial_started_postcalib += 1 self.shadow_success_window.append(0) self.shadow_givenup_window.append(0) self.shadow_started_window[-1] = 1 if mdf.reason == "JV_IDLE_TIMEOUT": if self.trial_sync: self.shadow_num_trial_givenup_postcalib += 1 self.shadow_givenup_window[-1] = 1 if (mdf.id == self.task_state_codes['final']) & (mdf.outcome == 1): if self.trial_sync: #print "*** trial complete and successful" self.shadow_num_trial_successful_postcalib += 1 self.shadow_success_window[-1] = 1 if (mdf.id == self.task_state_codes['intertrial']): if self.trial_sync: # do end-of-trial stuff here self.num_trials += 1 self.num_trials_postcalib += 1 self.num_trial_started_postcalib = self.shadow_num_trial_started_postcalib self.num_trial_successful_postcalib = self.shadow_num_trial_successful_postcalib self.num_trial_givenup_postcalib = self.shadow_num_trial_givenup_postcalib if len(self.shadow_success_window) > self.window_len: self.shadow_success_window.pop(0) if len(self.shadow_givenup_window) > self.window_len: self.shadow_givenup_window.pop(0) if len(self.shadow_started_window) > self.window_len: self.shadow_started_window.pop(0) self.success_window = copy.deepcopy( self.shadow_success_window) self.started_window = copy.deepcopy( self.shadow_started_window) self.givenup_window = copy.deepcopy( self.shadow_givenup_window)
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()
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 # SESSION_CONFIG => start of session if msg_type == rc.MT_SESSION_CONFIG: #self.msg_cnt += 1 self.num_trials = 0 self.reset_counters() self.update_gui_label_data() # EM_DECODER_CONFIGURATION => end of an adaptation round elif msg_type == rc.MT_EM_DECODER_CONFIGURATION: #self.msg_cnt += 1 self.reset_counters() self.update_gui_label_data() # END_TASK_STATE => end of a task elif msg_type == rc.MT_END_TASK_STATE: #self.msg_cnt += 1 mdf = rc.MDF_END_TASK_STATE() copy_from_msg(mdf, msg) # need to know: # begin task state code # final task state code # intertrial state code if (mdf.id == 1): self.trial_sync = 1 self.shadow_started_window.append(0) if (mdf.id == self.task_state_codes['begin']) & (mdf.outcome == 1): if self.trial_sync: #print "*** trial started ***" #self.rewards_given += 1 self.shadow_num_trial_started_postcalib += 1 self.shadow_success_window.append(0) self.shadow_givenup_window.append(0) self.shadow_started_window[-1] = 1 if mdf.reason == "JV_IDLE_TIMEOUT": if self.trial_sync: self.shadow_num_trial_givenup_postcalib += 1 self.shadow_givenup_window[-1] = 1 if (mdf.id == self.task_state_codes['final']) & (mdf.outcome == 1): if self.trial_sync: #print "*** trial complete and successful" self.shadow_num_trial_successful_postcalib += 1 self.shadow_success_window[-1] = 1 if (mdf.id == self.task_state_codes['intertrial']): if self.trial_sync: # do end-of-trial stuff here self.num_trials += 1 self.num_trials_postcalib += 1 self.num_trial_started_postcalib = self.shadow_num_trial_started_postcalib self.num_trial_successful_postcalib = self.shadow_num_trial_successful_postcalib self.num_trial_givenup_postcalib = self.shadow_num_trial_givenup_postcalib if len(self.shadow_success_window) > self.window_wide: #self.window_narrow: self.shadow_success_window.pop(0) if len(self.shadow_givenup_window) > self.window_wide: #self.window_narrow: self.shadow_givenup_window.pop(0) if len(self.shadow_started_window) > self.window_wide: #self.window_narrow: self.shadow_started_window.pop(0) self.success_window = copy.deepcopy(self.shadow_success_window) self.started_window = copy.deepcopy(self.shadow_started_window) self.givenup_window = copy.deepcopy(self.shadow_givenup_window) if self.num_trials_postcalib > 0: self.percent_start = 100 * self.num_trial_started_postcalib / self.num_trials_postcalib self.percent_givenup = 100 * self.num_trial_givenup_postcalib / self.num_trials_postcalib self.percent_success = 100 * self.num_trial_successful_postcalib / self.num_trials_postcalib percent_success_wide_window = np.NAN if len(self.success_window) >= self.window_wide: num_success_window = np.sum(self.success_window) percent_success_wide_window = 100 * num_success_window / len(self.success_window) percent_givenup_wide_window = np.NAN if len(self.givenup_window) >= self.window_wide: num_givenup_window = np.sum(self.givenup_window) percent_givenup_wide_window = 100 * num_givenup_window / len(self.givenup_window) percent_started_wide_window = np.NAN if len(self.started_window) >= self.window_wide: num_started_window = np.sum(self.started_window) percent_started_wide_window = 100 * num_started_window / len(self.started_window) percent_success_narrow_window = np.NAN if len(self.success_window) >= self.window_narrow: success_window_narrow = self.success_window[len(self.success_window)-self.window_narrow:] num_success_window = np.sum(success_window_narrow) percent_success_narrow_window = 100 * num_success_window / len(success_window_narrow) percent_givenup_narrow_window = np.NAN if len(self.givenup_window) >= self.window_narrow: givenup_window_narrow = self.givenup_window[len(self.givenup_window)-self.window_narrow:] num_givenup_window = np.sum(givenup_window_narrow) percent_givenup_narrow_window = 100 * num_givenup_window / len(givenup_window_narrow) if len(self.started_window) >= self.window_narrow: started_window_narrow = self.started_window[len(self.started_window)-self.window_narrow:] num_started_window = np.sum(started_window_narrow) percent_started_narrow_window = 100 * num_started_window / len(started_window_narrow) self.hist_narrow_STR.append(percent_started_narrow_window) self.hist_narrow_SUR.append(percent_success_narrow_window) self.hist_narrow_GUR.append(percent_givenup_narrow_window) self.hist_wide_STR.append(percent_started_wide_window) self.hist_wide_SUR.append(percent_success_wide_window) self.hist_wide_GUR.append(percent_givenup_wide_window) self.update_gui_label_data() elif msg_type == rc.MT_PING: respond_to_ping(self.mod, msg, 'TrialStatusDisplay') elif msg_type == MT_EXIT: self.exit() done = True else: done = True self.console_disp_cnt += 1 if self.console_disp_cnt == 50: self.update_plot() self.console_disp_cnt = 0
from PyDragonfly import copy_from_msg, copy_to_msg, MT_EXIT, MT_KILL import message_defs as mdefs import sys MID_REQUEST = 11 # Note: Request must be started second if __name__ == "__main__": mod = PyDragonfly.Dragonfly_Module(MID_REQUEST, 0) mod.ConnectToMMM() mod.Subscribe(mdefs.MT_TEST_DATA) mod.Subscribe(MT_EXIT) print "Request running...\n" while (1): mod.SendSignal(mdefs.MT_REQUEST_TEST_DATA) print("Sent request for data") msg = PyDragonfly.CMessage() mod.ReadMessage(msg) print "Received message", msg.GetHeader().msg_type if msg.GetHeader().msg_type == mdefs.MT_TEST_DATA: msg_data = mdefs.MDF_TEST_DATA() copy_from_msg(msg_data, msg) print "Data = [a: %d, b: %d, x: %f]" % (msg_data.a, msg_data.b, msg_data.x) time.sleep(1) mod.DisconnectFromMMM()
def process_message(self, msg): ''' Needs to: 1) combine non-conflicting controlledDims e.g. from OPERATOR_MOVEMENT_COMMANDs, into either extrinsic or intrinsic commands 2) combine intrinsic and extrinsic commands into final command ''' msg_type = msg.GetHeader().msg_type if msg_type in [ rc.MT_OPERATOR_MOVEMENT_COMMAND, rc.MT_PLANNER_MOVEMENT_COMMAND, rc.MT_EM_MOVEMENT_COMMAND, rc.MT_FIXTURED_MOVEMENT_COMMAND ]: if msg_type == rc.MT_OPERATOR_MOVEMENT_COMMAND: mdf = rc.MDF_OPERATOR_MOVEMENT_COMMAND() elif msg_type == rc.MT_PLANNER_MOVEMENT_COMMAND: mdf = rc.MDF_PLANNER_MOVEMENT_COMMAND() elif msg_type == rc.MT_EM_MOVEMENT_COMMAND: mdf = rc.MDF_EM_MOVEMENT_COMMAND() elif msg_type == rc.MT_FIXTURED_MOVEMENT_COMMAND: mdf = rc.MDF_FIXTURED_MOVEMENT_COMMAND() # MOVEMENT_COMMAND # ---------------- # controlledDims # pos # sample_header # sample_interval # tag # vel # ---------------- copy_from_msg(mdf, msg) tag = mdf.tag #if not tag in self.accepted_tags: # return dim = np.asarray(mdf.controlledDims, dtype=bool) #.astype(bool) if mdf.tag in self.intrinsic_tags: # intrinsic is AUTO command self.intrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim] #print "intr_vel = " + " ".join(["%5.2f" % (x) for x in self.intrinsic_vel]) elif mdf.tag in self.extrinsic_tags: #print "!" # extrinsic is non-AUTO, i.e. EM, command self.extrinsic_vel[dim] = np.asarray(mdf.vel, dtype=float)[dim] #self.extrinsic_vel[:8] *= self.gate if tag == self.timer_tag: self.send_output(mdf.sample_header) elif msg_type == rc.MT_ROBOT_CONTROL_CONFIG: mdf = rc.MDF_ROBOT_CONTROL_CONFIG() copy_from_msg(mdf, msg) self.autoVelControlFraction[:] = mdf.autoVelControlFraction elif msg_type == rc.MT_IDLE: mdf = rc.MDF_IDLE() copy_from_msg(mdf, msg) self.gate = float(np.asarray(mdf.gain, dtype=float).item()) elif msg_type == rc.MT_IDLE_DETECTION_ENDED: self.gate = 1.0 elif msg_type == rc.MT_TASK_STATE_CONFIG: mdf = rc.MDF_TASK_STATE_CONFIG() copy_from_msg(mdf, msg) self.idle_gateable = mdf.idle_gateable
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 process_message(self, in_msg): msg_type = in_msg.GetHeader().msg_type #print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing))) if self.status == False: pass elif self.testing: 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 = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:]))) if self.Ptest_count == 250 & self.Gtest_count == 250: self.testing = False self.status = False print 'done' else: if in_mdf.tool_id == (self.pointer + 1): self.pointer_test[self.Ptest_count, :] = positions if np.any(np.isnan(positions)) == True: sys.stdout.write('\n Pointer: NaN present') self.Gtest_count -= 1 elif np.any(np.isnan(orientations)) == True: sys.stdout.write('\n Pointer: NaN present') self.Gtest_count -= 1 elif np.any(np.isinf(positions)) == True: sys.stdout.write('\n Pointer: inf present') self.Gtest_count -= 1 elif np.any(np.isinf(orientations)) == True: sys.stdout.write('\n Pointer: inf present') self.Gtest_count -= 1 elif np.all(positions) == positions[0]: sys.stdout.write('\n Pointer: Zeros present') self.Gtest_count -= 1 elif np.all(orientations) == orientations[0]: sys.stdout.write('\n Pointer: Zeros present') self.Gtest_count -= 1 else: self.Ptest_count += 1 if in_mdf.tool_id == (self.glasses + 1): self.headP_test[self.Gtest_count, :] = positions self.headQ_test[self.Gtest_count, :] = orientations if np.any(np.isnan(positions)) == True: sys.stdout.write('\n Glasses: NaN present') self.Ptest_count -= 1 elif np.any(np.isnan(orientations)) == True: sys.stdout.write('\n Glasses: NaN present') self.Ptest_count -= 1 elif np.any(np.isinf(positions)) == True: sys.stdout.write('\n Glasses: inf present') self.Ptest_count -= 1 elif np.any(np.isinf(orientations)) == True: sys.stdout.write('\n Glasses: inf present') self.Ptest_count -= 1 elif np.all(positions) == positions[0]: sys.stdout.write('\n Glasses: Zeros present') self.Ptest_count -= 1 elif np.all(orientations) == orientations[0]: sys.stdout.write('\n Glasses: Zeros present') self.Ptest_count -= 1 else: self.Gtest_count += 1 print orientations if (self.Ptest_count < 0) or (self.Gtest_count < 0): self.Gtest_count = 0 else: if msg_type == rc.MT_POLARIS_POSITION: # handling input message in_mdf = rc.MDF_POLARIS_POSITION() copy_from_msg(in_mdf, in_msg) if in_mdf.tool_id == (self.glasses + 1): positions = np.array(in_mdf.xyz[:]) orientations = qa.norm( self.shuffle_q(np.array(in_mdf.ori[:]))) self.glasses_pos_buffer = positions self.glasses_orientation_buffer = orientations else: pass elif msg_type == rc.MT_HOTSPOT_POSITION: in_mdf = rc.MDF_HOTSPOT_POSITION() copy_from_msg(in_mdf, in_msg) positions = np.array(in_mdf.xyz[:]) vector = np.array(in_mdf.ori[:3]) self.coil_pos_buffer = positions self.coil_vector_buffer = vector elif msg_type == rc.MT_TMS_TRIGGER: self.find_hotspot_to_cz(self.coil_pos_buffer, self.glasses_orientation_buffer, self.glasses_pos_buffer, self.coil_vector_buffer)