Two encoders working
First round of PID controller software
- need to add output pid controller object