Fix issues in SimpleTeleop

Add diagonal movement
Fix sideways movement