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