You actually have most of what you need right here: all you need to do is put it in a psuedocode format, then fill in the psuedocode with actual code.
This is one way to do this code, and you may need to tweak it to fit what you want to do. There is a variable known as a state counter to see if the servos have turned already, and each if/else if statement checks to see if the variable is at a zero value (the servos are currently at a 0 degree position) or 1 (servos are at a 90 degree position).
Each servo will be turned to the correct position, with a wait1MSec command giving each time to reach their target value(s). You will need to determine the amount of time you want to give each servo to ensure that it reaches the target value before the next servo starts moving.
Check out our Blog
! And our Facebook page
Need help? Take a look at our updated help documentation
and the ROBOTC Forums.