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