Added more motor controller code
Started implementing PID code