def step(self): self.proxy.step() 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 and self.cycle: self.step_start=time.time() self.cycle_last=self.cycle pwm=self.pwm_desire_a[idx] if self.cycle: 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] 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(self.t_desire[idx]+self.bias[idx]) #Bias slider around 'zero' print 'Desired',c.name,c.command.t_desire idx=idx+1 if (self.save and not self.save_last): c.write_config() self.save_last=self.save
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 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
