For the multirobot planning assignment, Susu and I decided not to use the RVO library and to code everything ourselves. This turned out to be very challenging but we are happy with our current solution. The overall problem basically broke down into three subproblems: goal finding, staying inside the boundary, and collision avoidance. The first two were relatively painless but the third was quite difficult. Here, I define a timestep as one update cycle of the positions and orientations of the robots by the camera.
In order to find the goal, our algorithm simply finds the angle the goal is at relative to the robot (using basic trigonometry). Then the robot spins until its orientation is in the neighborhood of that angle.
To deal with boundaries, we also use angles. When a robot hits a boundary, it bounces off at a specific angle that takes it in the oposite direction that it was previously travelling. This is meant to mimic a reflection without the equations necessary to find the reflection.
We deal with collsion avoidance in the following way. All robots check to see if they are within a certain distance (800) of any other robot. If they are, they go into a "collision avoidance" mode. In this mode, the robot first turns 90 degrees clockwise and sets a counter. This counter tells the robot how many steps it has left to get out of the way of the other robot. Both robots are doing this during this timestep. During the next timestep, both robots begin moving away from each other by moving forward. But, first, they decide whether they will be colliding with other robots by doing so. If there is another robot in the way, the 90 degree turning cycle will begin again. Otherwise, the robot will continue moving forward every timestep until the counter runs out. Then it will go back into goal finding mode.