Lego swerve drive (programming in progress)