Added PID Control

Added a new tape following function
Further updated the pid controller