Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def calc_extra_speed(self) -> None:
- """Calculate extra speed to add for moving the robot."""
- if len(self.speed_extra_history) >= self.speed_extra_history_len:
- self.speed_extra_history.pop(0)
- left_encoder, right_encoder = self.current_encoders
- avg_speed_extra = tuple(sum(x) / len(self.speed_extra_history) for x in zip(*self.speed_extra_history))
- print("avg extra: {}".format(avg_speed_extra))
- left_extra, right_extra = self.speed_extra
- if not self.task_recalc_extra_speeds:
- self.speed_extra = avg_speed_extra
- return
- if abs(left_encoder) > abs(right_encoder):
- if abs(self.speed[0]) + self.speed_extra[0] > self.base_speed:
- left_extra = ((self.speed_extra[0] - 1 + avg_speed_extra[0]) / 2)
- else:
- right_extra = ((self.speed_extra[1] + 1 + avg_speed_extra[1]) / 2)
- else:
- if abs(self.speed[1]) + self.speed_extra[1] > self.base_speed:
- right_extra = ((self.speed_extra[1] - 1 + avg_speed_extra[1]) / 2)
- else:
- left_extra = ((self.speed_extra[0] + 1 + avg_speed_extra[0]) / 2)
- self.speed_extra_history.append((left_extra, right_extra))
- self.speed_extra = (round(left_extra), round(right_extra))
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement