The following code includes:
- Two PID controllers to help regulate the position of both motors.
- A path for the robot to follow, made of several 2D coordinates (x,y) in a given order.
- An odometry algorithm to help the robot estimate its current position and navigate in 2D space.
- Obstacle avoidance algorithm to stop the robot if an obstacle shows.