Simple robot control and navigation based on aruco markers — part 2

Navigation

Now lets finally control the robot and let it drive automatically between the waypoints.

You can find the code here:

https://github.com/eziosoft/ArucoRobotNavigation/blob/master/robot_navigation_vision_server_pygame.py

This is how it works:

  • connect to vision server
  • connect to mqtt server
  • main loop starts
  • pull data from vision server
  • find marker that is on the robot ( in my case markerID = 333 )
  • calculate heading to the target
(robot_heading_to_target = math.atan2((current_target_position[0] — robot_center_position[0]), (current_target_position[1] — robot_center_position[1])) # in radians)
  • use PID controller to rotate the robot in the direction of the target ( it tries to make robot heading equal to heading to target, pid_left_right.setpoint = robot_heading_to_target )
  • use another PID to drive forward
  • if distance to target is less than waypoint radius switch to the next waypoint
  • main loop ends

As I already said I never used Python before so the code is not perfect and few things doesn’t work as they should.

simulation

Buttons:
a — auto — enables control of the robot
n — navigation ON
w — add waypoint
b — enable borders

This is how it looks like working.

Leave a Reply

Your email address will not be published. Required fields are marked *