Polished some stuff up. Will work on PID next