def process_motor_states(self, state_list):
JointControllerMX.process_motor_states(self, state_list)
# this block of crap creates the running average of velocity
self.velocity_array.pop(0)
self.velocity_array.append(self.joint_state.velocity)
self.avg_velocity = sum(self.velocity_array) / len(self.velocity_array)
# this block of crap creates the running average of current
self.current_array.pop(0)
self.current_array.append(self.joint_state.current)
self.avg_current = sum(self.current_array)/len(self.current_array)
# if we have any waiting flags set, see if the wait should be cleared
if self.check_for_move:
if abs(self.avg_velocity) > self.min_moving_velocity:
self.unblock()
self.check_for_move = False
if self.check_for_stop:
if abs(self.avg_velocity) < self.max_stopped_velocity:
self.unblock()
self.check_for_stop = False
if self.check_for_position:
if abs(self.joint_state.error) < self.position_tol:
self.unblock()
self.check_for_position = False
if self.check_for_timeout:
if (rospy.Time.now() - self.timeout_start_time) > self.timeout:
self.timed_out = True
self.unblock()
self.check_for_timeout = False
if self.check_current is not None:
if abs(self.avg_current) > self.check_current:
self.unblock()
self.check_current = None
