More tuning of the lego main.

Also added movements to pick up all four green poms (needs much testing)