示例#1
0
    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()
示例#2
0
    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                
示例#3
0
 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])
示例#4
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()               
示例#5
0
    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
示例#6
0
    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()
示例#7
0
    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
示例#8
0
    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')
示例#9
0
    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
示例#10
0
 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)
示例#11
0
 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
示例#12
0
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)
示例#13
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)
示例#14
0
 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    
示例#15
0
    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()
示例#16
0
    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
示例#17
0
    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()
示例#18
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[:]
示例#20
0
 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()
示例#22
0
 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
示例#24
0
    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)
示例#25
0
    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)
示例#26
0
    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
示例#27
0
    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
示例#28
0
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
示例#29
0
#!/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()
        
示例#30
0
    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)
示例#31
0
    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()
示例#32
0
    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
示例#33
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()
示例#34
0
    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)