Basic Navigation Algorithm

I implemented a naive navigation algorithm for the robot. the algorithm uses an obstacle avoidance strategy. So far, it uses a linear transform that pushes the robot away from nearby items. However, I noticed that IRL, the LiDAR doesn’t feel the push in the opposite direction until the obstacle gets too close. I’m planning on fixing this later by using an inversion function or something rather than just subtraction.