Fixed point tracking, added PID control

Need to get better K values