def step(self): self.proxy.step() if self.do_scope_torque and self.scope_torque is None and len(self.actuator_ec)==1: self.scope_torque=m3t.M3Scope2(xwidth=100,yrange=None) if False and self.cnt%5==0: for n in self.names: self.proxy.pretty_print_component(n) if False and self.cnt%5==0: print '---------------' for c in self.actuator_ec: print 'Timestamp',c.name,m3t.timestamp_string(c.status.timestamp) self.cnt=self.cnt+1 self.status_dict=self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) idx=0 for c in self.actuator_ec: if not self.cycle_last_pwm and self.cycle_pwm: self.step_start=time.time() if not self.cycle_last_tq and self.cycle_tq: self.step_start=time.time() self.cycle_last_pwm=self.cycle_pwm self.cycle_last_tq=self.cycle_tq pwm=self.pwm_desire_a[idx] tq=self.t_desire_a[idx] current=self.current_desire_a[idx] if self.cycle_pwm: dt=time.time()-self.step_start if math.fmod(dt,self.step_period[idx]/1000.0)>self.step_period[idx]/2000.0: pwm=self.pwm_desire_b[idx] if self.cycle_tq: dt=time.time()-self.step_start if math.fmod(dt,self.step_period[idx]/1000.0)>self.step_period[idx]/2000.0: tq=self.t_desire_b[idx] c.command.mode=int(self.mode[idx]) if self.mode[idx]==mec.ACTUATOR_EC_MODE_PWM: c.command.t_desire=int(pwm) if self.mode[idx]==mec.ACTUATOR_EC_MODE_TORQUE: c.command.t_desire=int(tq+self.bias[idx]) #Bias slider around 'zero' print 'Desired',c.name,c.command.t_desire if self.mode[idx]==mec.ACTUATOR_EC_MODE_CURRENT: c.command.t_desire=int(current) print 'Desired',c.name,c.command.t_desire if self.do_scope_torque and self.scope_torque is not None: if self.mode[idx]==mec.ACTUATOR_EC_MODE_TORQUE: self.scope_torque.plot(c.status.adc_torque,c.command.t_desire) else: self.scope_torque.plot(c.status.adc_torque,c.status.adc_torque) idx=idx+1 if (self.save and not self.save_last): c.write_config() c.command.brake_off=int(self.brake[0]) print 't_desire:', c.command.t_desire self.save_last=self.save
def triangle_wave(): scope = m3t.M3Scope2(xwidth=100, yrange=None) des = [] cc = 1.0 scale = 5000 for i in range(10): des.append(cc) cc = cc * -1.0 print 'Enable power. Hit enter to continue' raw_input() for ii in range(len(des)): print ii, 'Des: ', des[ii] ramp_to_torque(des[ii] * scale, scope)
def fill_cells_random(): scope = m3t.M3Scope2(xwidth=100, yrange=None) ns = 30 #5#K*2 #Build desired vector log = { 'ns': ns, 'type': 'fill_cells_random', 'actuator': 'MA12J1', 'L': L, 'K': K, 'CW': cw, 'a': list(a), 'b': list(b), 'u_max': u_max, 'u_min': u_min, 'ut': [], 'wt': [], 'gt': [], 'des': [] } des = [u_min] for nn in range(ns): while True: x = u_min + (u_max - u_min) * (random.random()) #randomly in range if abs(des[-1] - x) > 5 * cw: #make excursion at least 5 cells des.append(x) break log['des'] = des print 'Des', des print 'Enable power. Hit enter to continue' raw_input() for ii in range(len(des)): print ii, 'Des: ', des[ii] uu, ww, gg = ramp_to_torque(des[ii], scope) print len(uu), len(ww), len(gg) log['ut'].append(list(uu)) #input log['wt'].append(list(ww)) #output log['gt'].append(list(gg)) #hysteron states print 'Save data [y]?' if m3t.get_yes_no('y'): fn = m3t.get_m3_config_path( ) + 'data/preisach_data_random_ma12j1_' + m3t.time_string() + '.yml' print 'Enter annotation string [None]' note = m3t.get_string('None') log['note'] = note f = file(fn, 'w') print 'Saving...', fn f.write(yaml.safe_dump(log, default_flow_style=False, width=200)) f.close() return log
def step(self): self.proxy.step() for c in self.actuator_ec: if self.do_scope and self.scope is None: self.scope=m3t.M3Scope2(xwidth=100,yrange=None) self.status_dict=self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) c.command.mode=int(self.mode[idx]) if self.mode[idx]==mec.ACTUATOR_EC_MODE_PWM: c.command.pwm_desired=int(self.pwm_desire[idx]) print 'Desired',c.name,c.command.pwm_desired if self.mode[idx]==mec.ACTUATOR_EC_MODE_CURRENT: c.command.current_desired=int(self.current_desire[idx]) print 'Desired',c.name,c.command.current_desired if self.do_scope and self.scope is not None: f1=self.scope_keys[self.scope_field1[0]] f2=self.scope_keys[self.scope_field2[0]] x1=x2=None if f1!='None' and f1!='base': x1=m3t.get_msg_field_value(self.actuator_ec[0].status,f1) print f1,':',x1 if f2!='None' and f2!='base': x2=m3t.get_msg_field_value(self.actuator_ec[0].status,f2) print f2,':',x2 if x1==None: x1=x2 if x2==None: x2=x1 if x1!=None and x2!=None: #Handle only one value or two self.scope.plot(x1,x2) print'-----------------' if (self.save and not self.save_last): c.write_config() self.save_last=self.save
def step(self): # print self.comps['act_ec']['comp'].status.timestamp if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100, yrange=None) self.proxy.step() self.cnt = self.cnt + 1 self.status_dict = self.proxy.get_status_dict() if self.zero_joint_theta and not self.zero_joint_theta_last: self.joint_theta -= self.act.get_joint_theta() print 'New joint_theta zero', self.joint_theta if self.zero_joint_torque and not self.zero_joint_torque_last: self.joint_torque -= self.act.get_joint_torque() print 'New joint_torque zero', self.joint_torque # # if self.zero_joint_torque_lc and not self.zero_joint_torque_lc_last: # self.joint_torque_lc -= self.act.get_joint_torque_lc() # print 'New joint_torque_lc zero',self.joint_torque_lc # # self.param_dict[self.comp_name]['calibration']['zero_motor_theta'] = self.motor_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_theta'] = self.joint_theta # self.param_dict[self.comp_name]['calibration']['zero_joint_torque'] = self.joint_torque # self.param_dict[self.comp_name]['calibration']['zero_joint_torque_lc'] = self.joint_torque_lc # # self.zero_joint_theta_last = self.zero_joint_theta # self.zero_joint_torque_last = self.zero_joint_torque # self.zero_joint_torque_lc_last = self.zero_joint_torque_lc self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: f1 = self.scope_keys[self.scope_field1[0]] f2 = self.scope_keys[self.scope_field2[0]] x1 = x2 = None if f1 != 'None' and f1 != 'base': x1 = m3t.get_msg_field_value(self.act.status, f1) print f1, ':', x1 if f2 != 'None' and f2 != 'base': x2 = m3t.get_msg_field_value(self.act.status, f2) print f2, ':', x2 if x1 == None: x1 = x2 if x2 == None: x2 = x1 if x1 != None and x2 != None: #Handle only one value or two self.scope.plot(x1, x2) print '-----------------' if self.mode[0] == 0: #Off self.act.set_mode(mec.ACTUATOR_MODE_OFF) elif self.mode[0] == 1: #Pwm self.act.set_mode(mec.ACTUATOR_MODE_PWM) self.act.set_pwm(self.pwm_desired[0]) elif self.mode[0] == 2: #Current self.act.set_mode(mec.ACTUATOR_MODE_CURRENT) self.act.set_i_desired(self.current_desired[0] * 1000.0) else: self.act.set_mode(mec.ACTUATOR_MODE_OFF) if (self.save and not self.save_last): self.act.write_config() self.save_last = self.save
def step(self): if self.do_scope and self.scope is None: self.scope = m3t.M3Scope2(xwidth=100, yrange=None) self.proxy.step() self.cnt = self.cnt + 1 self.status_dict = self.proxy.get_status_dict() self.proxy.set_param_from_dict(self.param_dict) if self.do_scope and self.scope is not None: ctrl_f1 = self.ctrl_scope_keys[self.ctrl_scope_field1[0]] ctrl_f2 = self.ctrl_scope_keys[self.ctrl_scope_field2[0]] x1 = x2 = None if ctrl_f1 != 'None': x1 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f1) print ctrl_f1, ':', x1 if ctrl_f2 != 'None': x2 = m3t.get_msg_field_value(self.ctrl.status, ctrl_f2) print ctrl_f2, ':', x2 if x1 == None: x1 = x2 if x2 == None: x2 = x1 if x1 != None and x2 != None: #Handle only one value or two self.scope.plot(x1, x2) print '-----------------' self.ctrl.set_traj_mode(self.traj[0]) if self.mode[0] == 0: self.ctrl.set_mode_off() # elif self.mode[0]==1: # self.ctrl.set_mode_pwm() # self.ctrl.set_pwm(self.current[0]) elif self.mode[0] == 1: self.ctrl.set_mode_current() self.ctrl.set_current(self.current[0]) elif self.mode[0] == mec.CTRL_MODE_THETA: self.ctrl.set_mode_theta() self.ctrl.set_theta_deg(self.theta[0]) # elif self.mode[0] == mec.CTRL_MODE_TORQUE: print "kp: " + str(self.ctrl.param.pid_torque.k_p) self.ctrl.set_mode_torque() self.ctrl.set_torque(self.torque[0]) # # elif self.mode[0]==mec.CTRL_MODE_TORQUE_LC: # self.ctrl.set_mode_torque_lc() # self.ctrl.set_torque_lc(self.torque_lc[0]) else: self.ctrl.set_mode_off() if (self.save and not self.save_last): self.ctrl.write_config() self.save_last = self.save