Advertisement
Guest User

Untitled

a guest
Nov 15th, 2018
88
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.33 KB | None | 0 0
  1. def calc_extra_speed(self) -> None:
  2. """Calculate extra speed to add for moving the robot."""
  3. if len(self.speed_extra_history) >= self.speed_extra_history_len:
  4. self.speed_extra_history.pop(0)
  5.  
  6. left_encoder, right_encoder = self.current_encoders
  7. avg_speed_extra = tuple(sum(x) / len(self.speed_extra_history) for x in zip(*self.speed_extra_history))
  8. print("avg extra: {}".format(avg_speed_extra))
  9. left_extra, right_extra = self.speed_extra
  10.  
  11. if not self.task_recalc_extra_speeds:
  12. self.speed_extra = avg_speed_extra
  13. return
  14.  
  15. if abs(left_encoder) > abs(right_encoder):
  16. if abs(self.speed[0]) + self.speed_extra[0] > self.base_speed:
  17. left_extra = ((self.speed_extra[0] - 1 + avg_speed_extra[0]) / 2)
  18. else:
  19. right_extra = ((self.speed_extra[1] + 1 + avg_speed_extra[1]) / 2)
  20. else:
  21. if abs(self.speed[1]) + self.speed_extra[1] > self.base_speed:
  22. right_extra = ((self.speed_extra[1] - 1 + avg_speed_extra[1]) / 2)
  23. else:
  24. left_extra = ((self.speed_extra[0] + 1 + avg_speed_extra[0]) / 2)
  25.  
  26. self.speed_extra_history.append((left_extra, right_extra))
  27. self.speed_extra = (round(left_extra), round(right_extra))
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement