After the meeting with Daniel, I made updates to the controller. Now the robot can go to a targeted location.
Here is an example, with only P controller in it.
Here is an example with PD controller
Webots try-out environment
After the discussion on Thu, we agreed that we should use some static CAD solid as environment to test out our system, while at the same time, Daniel will be working on Scenic environment.
Here is an environment i created using CAD and imported into Webots: