I did something simular with DC motors. It uses 2 variable "z" -forward movement and "y" - left and right movement.
Note this is only a snip of the full code, the rest is just receiving the data and sending it to here. You may also be able to alter it to use servos instead of DC motors.
Hope this gives you some ideas.
int DRV2 = map(z, -15, 0, 255, 0);
int DRV1 = map(z, 0, 15, 0, 255);
int STRL = map(y, -10, 0, 255, 0);
int STRR = map(y, 0, 10, 0, 255);
//constrain((DRV1,DRV2,STRL,STRR),0,255);
/*Serial.print(DRV1);
Serial.print(",");
Serial.print(DRV2);
Serial.print(",");
Serial.print(STRL);
Serial.print(",");
Serial.print(STRR);
//Serial.print(",");
//Serial.print(y);
Serial.println();*/
if(z > 0)//forwards with skew left or right
{
//Serial.println("Driving");
analogWrite(M1L, constrain(abs(DRV1 - STRL),0,255)); analogWrite(M1R, constrain(abs(DRV1 - STRR),0,255));
digitalWrite(M2L, LOW); digitalWrite(M2R, LOW);
}
else if(z < 0)//backwards with skew left or right
{
//Serial.println("Driving");
digitalWrite(M1L, LOW); digitalWrite(M1R, LOW);
analogWrite(M2L, constrain(abs(DRV2 - STRL),0,255)); analogWrite(M2R, constrain(abs(DRV2 - STRR),0,255));
}
else if(z < 4 && z > -4 && y > 0)//Right (+-4 = limits before movement)
{
//Serial.println("Driving");
digitalWrite(M2L, LOW); analogWrite(M2R, STRR);
analogWrite(M1L, STRR); digitalWrite(M1R, LOW);
}
else if(z < 4 && z > -4 && y < 0)//Left
{
//Serial.println("Driving");
analogWrite(M2L, STRL); digitalWrite(M2R, LOW);
digitalWrite(M1L, LOW); analogWrite(M1R, STRL);
}
else //full stop
{
digitalWrite(M1L, LOW); digitalWrite(M1R, LOW);
digitalWrite(M2L, LOW); digitalWrite(M2R, LOW);
}