Added encoder

Also removed interrupt based PID calculation to prevent data corruption.