Пример #1
0
    def start(self):

        self.proxy.start()

        self.get_component('m3actuator')
        print "starting components"
        self.start_components(['act', 'act_ec', 'pwr'], None)
        print "done starting components"

        #		for k,v in self.comps.items():
        #			# accomplishes this: self.act=m3s.M3Actuator(self.comp_name)
        #			setattr(self, k, v['type'](v['name']) )
        #			self.comps[k]['comp'] = getattr(self,k)
        #			self.proxy.subscribe_status(getattr(self,k))
        #			self.proxy.publish_command(getattr(self,k))
        #			self.proxy.publish_param(getattr(self,k))
        #			self.proxy.make_operational(v['name'])

        #		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name'])
        #		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
        #		pr=m3f.create_component(pwr_rt)
        #		self.proxy.publish_command(pr)
        #		self.proxy.make_operational(pwr_rt)
        #		self.proxy.make_operational(pwr_ec)
        #		pr.set_motor_power_on()

        self.proxy.step()

        #Create gui
        self.mode = [0]

        self.current_desired = [0]
        self.pwm_desired = [0]

        #self.enable_ctrl_dev=[0]
        self.save = False
        self.save_last = False
        self.do_scope = False
        self.scope = None
        self.status_dict = self.proxy.get_status_dict()

        #extract status fields
        self.scope_keys = m3t.get_msg_fields(self.act.status,
                                             prefix='',
                                             exclude=['ethercat', 'base'])
        self.scope_keys.sort()
        self.scope_keys = ['None'] + self.scope_keys
        self.scope_field1 = [0]
        self.scope_field2 = [0]

        self.f1_last = None
        self.f2_last = None

        self.zero_motor_theta = False
        self.zero_motor_theta_last = False
        self.zero_joint_torque = False
        self.zero_joint_torque_last = False
        self.zero_joint_torque_lc = False
        self.zero_joint_torque_lc_last = False
        self.zero_joint_theta = False
        self.zero_joint_theta_last = False

        current_max = 2.5
        pwm_max = 200

        self.param_dict = self.proxy.get_param_dict()
        #		self.joint_torque	= self.param_dict[self.comps['act']['name']]['calib']['torque']['cb_bias']
        #		self.joint_theta	= self.param_dict[self.comps['act']['name']]['calib']['theta']['cb_bias']

        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=2)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param_dict'), [], [],
                     m3g.M3GuiWrite,
                     column=3)

        self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1),
                     [['Off', 'PWM', 'Current'], 1], m3g.M3GuiWrite)

        self.gui.add('M3GuiSliders', 'PWM (counts)', (self, 'pwm_desired'),
                     range(1), [-pwm_max, pwm_max], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'Current (A)', (self, 'current_desired'),
                     range(1), [-current_max, current_max], m3g.M3GuiWrite)

        self.gui.add('M3GuiToggle', 'ZeroJointTheta',
                     (self, 'zero_joint_theta'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ZeroJointTorque',
                     (self, 'zero_joint_torque'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'ZeroJointTorqueLc',
                     (self, 'zero_joint_torque_lc'), [], [['On', 'Off']],
                     m3g.M3GuiWrite)

        self.gui.add('M3GuiModes', 'Scope1', (self, 'scope_field1'), range(1),
                     [self.scope_keys, 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'Scope2', (self, 'scope_field2'), range(1),
                     [self.scope_keys, 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'Scope', (self, 'do_scope'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)

        self.gui.start(self.step)
	def start(self):
		self.proxy.start()

		self.get_component('m3actuator')
		print "starting components"
		self.start_components(['ctrl','act','act_ec'],None)
		print "done starting components"
		

#		for k,v in self.comps.items():
#			# accomplishes this: self.act=m3s.M3Actuator(self.comp_name)
#			setattr(self, k, v['type'](v['name']) )
#			self.comps[k]['comp'] = getattr(self,k)
#			self.proxy.subscribe_status(getattr(self,k))
#			self.proxy.publish_command(getattr(self,k)) 
#			self.proxy.publish_param(getattr(self,k)) 
#			self.proxy.make_operational(v['name'])
#			
		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name'])
		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
		pr=m3f.create_component(pwr_rt)
		self.proxy.publish_command(pr)
		self.proxy.make_operational(pwr_rt)
		self.proxy.make_operational(pwr_ec)
		pr.set_motor_power_on()
			

		self.proxy.step()

		#Create gui
		self.mode = [0]
		self.traj = [0]
		
		self.current	= [0]
		self.theta	= [0]
		self.torque	= [0]
		self.torque_lc	= [0]
		
		self.save		= False
		self.save_last	= False
		self.do_scope	= False
		self.scope		= None
		
		# extract status fields
		self.status_dict = self.proxy.get_status_dict()
		
		self.ctrl_scope_keys	= m3t.get_msg_fields(self.ctrl.status,prefix='',exclude=['ethercat','base'])
		self.ctrl_scope_keys.sort()
		self.ctrl_scope_keys	= ['None']+self.ctrl_scope_keys

		print self.ctrl_scope_keys

		self.ctrl_scope_field1	= [0]
		self.ctrl_scope_field2	= [0]
		self.ec_scope_field1	= [0]

		current_max = 3.5
		theta_max	= 100.0
		torque_max = 40.0

		self.param_dict=self.proxy.get_param_dict()
		
		print str(self.param_dict)

		self.modes = mec._CTRL_SIMPLE_MODE.values_by_number
		self.mode_names = [self.modes[i].name for i in sorted(self.modes.keys())]
		
		self.trajs = mec._CTRL_SIMPLE_TRAJ_MODE.values_by_number
		self.traj_names = [self.trajs[i].name for i in sorted(self.trajs.keys())]

		self.gui.add('M3GuiTree',   'Status',	(self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',	(self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		
		self.gui.add('M3GuiModes',  'Mode',		(self,'mode'),range(1),[self.mode_names,1],m3g.M3GuiWrite)
																			
		self.gui.add('M3GuiModes',  'Traj',		(self,'traj'),range(1),[self.traj_names,1],m3g.M3GuiWrite)
#		self.gui.add('M3GuiModes',  'Mode',		(self,'mode'),range(1),[['Off','Current','Theta','Torque','Torque_LC'],1],m3g.M3GuiWrite)
																			
#		self.gui.add('M3GuiModes',  'Traj',		(self,'traj'),range(1),[['Off','Current Square','Current Sine','Theta Square','Theta Sine','Torque Square','Torque Sine','TorqueLC Square','TorqueLC Sine'],1],m3g.M3GuiWrite)
		
		self.gui.add('M3GuiSliders','Current',		(self,'current'),range(1),	[-current_max,current_max],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','Theta(deg)',	(self,'theta'),range(1),[-theta_max,theta_max],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','Torque',		(self,'torque'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','TorqueLC',		(self,'torque_lc'),range(1),[-torque_max,torque_max],m3g.M3GuiWrite)
		
		self.gui.add('M3GuiToggle', 'Save',			(self,'save'),[],[['On','Off']],m3g.M3GuiWrite)

		self.gui.add('M3GuiModes',  'Ctrl Scope1',	(self,'ctrl_scope_field1'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Ctrl Scope2',	(self,'ctrl_scope_field2'),range(1),[self.ctrl_scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Scope',		(self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
		
		self.gui.start(self.step)
	def start(self):
	
		self.proxy.start()
		
		self.get_component('m3actuator')
		print "starting components"
		self.start_components(['act','act_ec','pwr'],None)
		print "done starting components"
		
#		for k,v in self.comps.items():
#			# accomplishes this: self.act=m3s.M3Actuator(self.comp_name)
#			setattr(self, k, v['type'](v['name']) )
#			self.comps[k]['comp'] = getattr(self,k)
#			self.proxy.subscribe_status(getattr(self,k))
#			self.proxy.publish_command(getattr(self,k)) 
#			self.proxy.publish_param(getattr(self,k)) 
#			self.proxy.make_operational(v['name'])
			
#		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.comps['act_ec']['name'])
#		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
#		pr=m3f.create_component(pwr_rt)
#		self.proxy.publish_command(pr)
#		self.proxy.make_operational(pwr_rt)
#		self.proxy.make_operational(pwr_ec)
#		pr.set_motor_power_on()
			

		self.proxy.step()

		#Create gui
		self.mode				= [0]
		
		self.current_desired	= [0]
		self.pwm_desired		= [0]
		

		#self.enable_ctrl_dev=[0]
		self.save		= False
		self.save_last	= False
		self.do_scope	= False
		self.scope		= None
		self.status_dict=self.proxy.get_status_dict()
		
		#extract status fields
		self.scope_keys=m3t.get_msg_fields(self.act.status,prefix='',exclude=['ethercat','base'])
		self.scope_keys.sort()
		self.scope_keys		= ['None']+self.scope_keys
		self.scope_field1	= [0]
		self.scope_field2	= [0]
		

		self.f1_last	= None
		self.f2_last	= None
		
		self.zero_motor_theta			= False
		self.zero_motor_theta_last		= False
		self.zero_joint_torque			= False
		self.zero_joint_torque_last		= False
		self.zero_joint_torque_lc		= False
		self.zero_joint_torque_lc_last	= False
		self.zero_joint_theta			= False
		self.zero_joint_theta_last		= False
		
		current_max = 2.5
		pwm_max = 200

		self.param_dict = self.proxy.get_param_dict()
#		self.joint_torque	= self.param_dict[self.comps['act']['name']]['calib']['torque']['cb_bias']
#		self.joint_theta	= self.param_dict[self.comps['act']['name']]['calib']['theta']['cb_bias']
	
		
		self.gui.add('M3GuiTree',   'Status',		(self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',		(self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)

		self.gui.add('M3GuiModes',  'Mode',			(self,'mode'),range(1),[['Off','PWM','Current'],1],m3g.M3GuiWrite)

		self.gui.add('M3GuiSliders','PWM (counts)',	(self,'pwm_desired'),range(1),[-pwm_max,pwm_max],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','Current (A)',	(self,'current_desired'),range(1),[-current_max,current_max],m3g.M3GuiWrite)

		
		
		self.gui.add('M3GuiToggle', 'ZeroJointTheta',	(self,'zero_joint_theta'),	[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'ZeroJointTorque',	(self,'zero_joint_torque'),	[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'ZeroJointTorqueLc',(self,'zero_joint_torque_lc'),	[],[['On','Off']],m3g.M3GuiWrite)


		self.gui.add('M3GuiModes',  'Scope1',		(self,'scope_field1'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Scope2',		(self,'scope_field2'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Scope',		(self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Save',			(self,'save'),[],[['On','Off']],m3g.M3GuiWrite)

		self.gui.start(self.step)
Пример #4
0
	def start(self):

		self.proxy.start()
		cnames=self.proxy.get_available_components('m3actuator_ec')
		self.names=m3t.user_select_components_interactive(cnames)
		if len(self.names)==0:
			return
		self.actuator_ec=[]
		for name in self.names:
			self.actuator_ec.append(m3f.create_component(name))
			self.proxy.subscribe_status(self.actuator_ec[-1])
			self.proxy.publish_command(self.actuator_ec[-1]) 
			self.proxy.publish_param(self.actuator_ec[-1]) 
			self.proxy.make_operational(name)
		
		#pwr_ec=self.proxy.get_available_components('m3pwr_ec')
		#pwr_rt=self.proxy.get_available_components('m3pwr')
		#print 'A',pwr_rt[0],pwr_ec[0]
		#if len(pwr_rt):
			#pr=m3f.create_component(pwr_rt[0])
			#self.proxy.publish_command(pr)
			#self.proxy.make_operational(pwr_rt[0])
			#self.proxy.make_operational(pwr_ec[0])
			#pr.set_motor_power_on()
			
		pwr_rt=m3t.get_actuator_ec_pwr_component_name(self.names[0])
		pwr_ec=pwr_rt.replace('m3pwr','m3pwr_ec')
		pr=m3f.create_component(pwr_rt)
		self.proxy.publish_command(pr)
		self.proxy.make_operational(pwr_rt)
		self.proxy.make_operational(pwr_ec)
		self.proxy.subscribe_status(pr)
		pr.set_motor_power_on()
		
		tmax=[x.param.t_max for x in self.actuator_ec]
		tmin=[x.param.t_min for x in self.actuator_ec]
		
		
		self.proxy.step()
		for c in self.actuator_ec:
			self.bias.append(c.status.adc_torque)
		tl=min(tmin)-self.bias[0]
		tu=max(tmax)-self.bias[0]
		
		#Create gui
		self.mode=[0]
		self.pwm_desire=[0]
		self.current_desire=[0]

		self.save=False
		self.save_last=False
		self.do_scope=False
		self.scope = None
		
		self.scope_keys=m3t.get_msg_fields(self.actuator_ec[0].status)
		self.scope_keys.sort()
		self.scope_keys=['None']+self.scope_keys
		self.scope_field1=[0]
		self.scope_field2=[0]

		self.status_dict=self.proxy.get_status_dict()
		self.param_dict=self.proxy.get_param_dict()
		self.gui.add('M3GuiTree',   'Status',    (self,'status_dict'),[],[],m3g.M3GuiRead,column=2)
		self.gui.add('M3GuiTree',   'Param',   (self,'param_dict'),[],[],m3g.M3GuiWrite,column=3)
		self.gui.add('M3GuiModes',  'Mode',      (self,'mode'),range(len(self.actuator_ec)),[['off','pwm','torque','current','brake'],1],m3g.M3GuiWrite)
		self.gui.add('M3GuiSliders','pwmDesire', (self,'pwm_desire'),range(len(self.actuator_ec)),[-1000,1000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiSliders','currentDesire', (self,'current_desire'),range(len(self.actuator_ec)),[-10000,10000],m3g.M3GuiWrite) 
		self.gui.add('M3GuiToggle', 'Save',      (self,'save'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiToggle', 'Scope',      (self,'do_scope'),[],[['On','Off']],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Scope1',	(self,'scope_field1'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
		self.gui.add('M3GuiModes',  'Scope2',	(self,'scope_field2'),range(1),[self.scope_keys,1],m3g.M3GuiWrite)
	
		self.gui.start(self.step)
Пример #5
0
    def start(self):
        self.proxy.start()

        self.get_component('m3actuator')
        print "starting components"
        self.start_components(['ctrl', 'act', 'act_ec'], None)
        print "done starting components"

        #		for k,v in self.comps.items():
        #			# accomplishes this: self.act=m3s.M3Actuator(self.comp_name)
        #			setattr(self, k, v['type'](v['name']) )
        #			self.comps[k]['comp'] = getattr(self,k)
        #			self.proxy.subscribe_status(getattr(self,k))
        #			self.proxy.publish_command(getattr(self,k))
        #			self.proxy.publish_param(getattr(self,k))
        #			self.proxy.make_operational(v['name'])
        #
        pwr_rt = m3t.get_actuator_ec_pwr_component_name(
            self.comps['act_ec']['name'])
        pwr_ec = pwr_rt.replace('m3pwr', 'm3pwr_ec')
        pr = m3f.create_component(pwr_rt)
        self.proxy.publish_command(pr)
        self.proxy.make_operational(pwr_rt)
        self.proxy.make_operational(pwr_ec)
        pr.set_motor_power_on()

        self.proxy.step()

        #Create gui
        self.mode = [0]
        self.traj = [0]

        self.current = [0]
        self.theta = [0]
        self.torque = [0]
        self.torque_lc = [0]

        self.save = False
        self.save_last = False
        self.do_scope = False
        self.scope = None

        # extract status fields
        self.status_dict = self.proxy.get_status_dict()

        self.ctrl_scope_keys = m3t.get_msg_fields(self.ctrl.status,
                                                  prefix='',
                                                  exclude=['ethercat', 'base'])
        self.ctrl_scope_keys.sort()
        self.ctrl_scope_keys = ['None'] + self.ctrl_scope_keys

        print self.ctrl_scope_keys

        self.ctrl_scope_field1 = [0]
        self.ctrl_scope_field2 = [0]
        self.ec_scope_field1 = [0]

        current_max = 3.5
        theta_max = 100.0
        torque_max = 40.0

        self.param_dict = self.proxy.get_param_dict()

        print str(self.param_dict)

        self.modes = mec._CTRL_SIMPLE_MODE.values_by_number
        self.mode_names = [
            self.modes[i].name for i in sorted(self.modes.keys())
        ]

        self.trajs = mec._CTRL_SIMPLE_TRAJ_MODE.values_by_number
        self.traj_names = [
            self.trajs[i].name for i in sorted(self.trajs.keys())
        ]

        self.gui.add('M3GuiTree',
                     'Status', (self, 'status_dict'), [], [],
                     m3g.M3GuiRead,
                     column=2)
        self.gui.add('M3GuiTree',
                     'Param', (self, 'param_dict'), [], [],
                     m3g.M3GuiWrite,
                     column=3)

        self.gui.add('M3GuiModes', 'Mode', (self, 'mode'), range(1),
                     [self.mode_names, 1], m3g.M3GuiWrite)

        self.gui.add('M3GuiModes', 'Traj', (self, 'traj'), range(1),
                     [self.traj_names, 1], m3g.M3GuiWrite)
        #		self.gui.add('M3GuiModes',  'Mode',		(self,'mode'),range(1),[['Off','Current','Theta','Torque','Torque_LC'],1],m3g.M3GuiWrite)

        #		self.gui.add('M3GuiModes',  'Traj',		(self,'traj'),range(1),[['Off','Current Square','Current Sine','Theta Square','Theta Sine','Torque Square','Torque Sine','TorqueLC Square','TorqueLC Sine'],1],m3g.M3GuiWrite)

        self.gui.add('M3GuiSliders', 'Current', (self, 'current'), range(1),
                     [-current_max, current_max], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'Theta(deg)', (self, 'theta'), range(1),
                     [-theta_max, theta_max], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'Torque', (self, 'torque'), range(1),
                     [-torque_max, torque_max], m3g.M3GuiWrite)
        self.gui.add('M3GuiSliders', 'TorqueLC', (self, 'torque_lc'), range(1),
                     [-torque_max, torque_max], m3g.M3GuiWrite)

        self.gui.add('M3GuiToggle', 'Save', (self, 'save'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)

        self.gui.add('M3GuiModes', 'Ctrl Scope1', (self, 'ctrl_scope_field1'),
                     range(1), [self.ctrl_scope_keys, 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiModes', 'Ctrl Scope2', (self, 'ctrl_scope_field2'),
                     range(1), [self.ctrl_scope_keys, 1], m3g.M3GuiWrite)
        self.gui.add('M3GuiToggle', 'Scope', (self, 'do_scope'), [],
                     [['On', 'Off']], m3g.M3GuiWrite)

        self.gui.start(self.step)