Add encoder based movement to drivetrain
Improve encoder based drivgiiving