Currently, I'm debugging my code. You can take a look on my code in the Detail part. The current plan is to debug my code asap and then I can work on adding rplidar for drawing map. Then I will make a video footage when my hexapod draws a map of my room.