Robotics Programming: PID Algorithm - Wall Following and Balancing