start of encoder implementation

Motor Encoder implementation and PID